Urgent Help ROBOTC PID

VEX V5 Technical Support
1

I am using ROBOTC built in PID to slave two motors for my flywheel. When I tried to give power to the intake after I start the flywheel it does not work in autonomous mode. I am including the code.


#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, dgtl1,  limitswitch,    sensorTouch)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_4,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port2,           rearRight,     tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,           frontRight,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port4,           rearLeft,      tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port5,           BLfly,         tmotorVex393TurboSpeed_MC29, PIDControl, encoderPort, I2C_1)
#pragma config(Motor,  port6,           Intake,        tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           BRfly,         tmotorVex393TurboSpeed_MC29, PIDControl, encoderPort, I2C_4)
#pragma config(Motor,  port8,           TRfly,         tmotorVex393TurboSpeed_MC29, openLoop, encoderPort, I2C_2)
#pragma config(Motor,  port9,           TLfly,         tmotorVex393TurboSpeed_MC29, openLoop, reversed, encoderPort, I2C_4)
#pragma config(Motor,  port10,          frontLeft,     tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(15)
#pragma userControlDuration(115)

#include "Vex_Competition_Includes_withPID.c"   //Main competition background code...do not modify!


/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
	// Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
	// Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
	bStopTasksBetweenModes = true;
	bLCDBacklight = true;
	clearLCDLine(0);
	clearLCDLine(1);




	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task autonomous()
{
	while (bIfiAutonomousMode)
	{

	// new code for testing;
	//original intake value is 50 and flywheel speed was 63


	 clearLCDLine(1);
	 displayLCDPos(1, 0);
		
		motor[TLfly] = 49;
    motor[TRfly] = 49;

	  wait1Msec(2000);
	  
		motor[Intake] = 80;

		int rpm = getMotorVelocity(TLfly);
    displayLCDNumber(1, 0, rpm);
    
   
      if (rpm <= 146)  {
    		motor[Intake] = 0;
    	}
    	else {
    		motor[Intake] = 80;
      }

	}
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{



	// User control code here, inside the loop

	while (true)
	{
		clearLCDLine(1);         // Clear line 2 (1) of the LCD


		motor[frontLeft]  = vexRT[Ch3];// Left Joystick Y value
		motor[rearLeft] = vexRT[Ch3];
		motor[frontRight] = vexRT[Ch2];   // Right Joystick Y value
		motor[rearRight] = vexRT[Ch2];
		motor[Intake] = vexRT[Btn5U];     // This button is for the intake

		if (vexRT[Btn6U] == 1)
		{
			motor[TLfly] = 49;
      motor[TRfly] = 49;
		}
		if (vexRT[Btn6D] == 1)
		{
			motor[TLfly] = 40;
      motor[TRfly] = 40;
		}

		 clearLCDLine(1);         // Clear line 2 (1) of the LCD
     displayLCDPos(1, 0);
     
	   int rpm = getMotorVelocity(TLfly);
     displayLCDNumber(1, 0, rpm);

		if (vexRT[Btn5U] == 1)
		{
			motor[Intake] = 127;
		}
		if (vexRT[Btn5D] == 1)
		{
			motor[Intake] = -127;
		}

		if ((vexRT[Btn5D] == 0) && (vexRT[Btn5U] == 0) && (vexRT[Btn6D] == 0) && (vexRT[Btn6U] == 0))
		{

			motor[TRfly] = 0;
			motor[TLfly] = 0;
			motor[Intake] = 0;
			motor[BLfly] = 0;
			motor[BRfly] = 0;
		}

	}
}

//////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                       VEX Competition Control Include File
//
// This file provides control over a VEX Competition Match. It should be included in the user's
// program with the following line located near the start of the user's program
//        #include "VEX_Competition_Includes.h"
// The above statement will cause this program to be included in the user's program. There's no
// need to modify this program.
//
// The program displays status information on the new VEX LCD about the competition state. You don't
// need the LCD, the program will work fine whether or not the LCD is actually provisioned.
//
// The status information is still useful without the LCD. The ROBOTC IDE debugger has a "remote screen"
// that contains a copy of the status information on the LCD. YOu can use this to get a view of the
// status of your program. The remote screen is shown with the menu command
//   "Robot -> Debugger Windows -> VEX Remote Screen"
//
// The LCD is 2 lines x 16 characters. There are three display formats to look for:
//
//        State          Description
//
//    ----------------
//   |Disabled        |  The robot is idle. This occurs before both the autonomous and user
//   |Time mm:ss.s    |  control states. The time display is minutes and seconds it has been idle.
//    ----------------
//
//    ----------------
//   |Autonomous      |  The robot is running autonomous code.
//   |Time mm:ss.s    |  control states. The time display is minutes and seconds it has been running.
//    ----------------
//
//    ----------------
//   |User Control    |  The robot is running user control code.
//   |Time mm:ss.s    |  control states. The time display is minutes and seconds it has been running.
//    ----------------
//////////////////////////////////////////////////////////////////////////////////////////////////////


void allMotorsOff();
void allTasksStop();
void pre_auton();
task autonomous();
task usercontrol();

int nTimeXX = 0;
bool bStopTasksBetweenModes = true;

static void displayStatusAndTime();

task main()
{
	// Master CPU will not let competition start until powered on for at least 2-seconds
	clearLCDLine(0);
	clearLCDLine(1);
	displayLCDPos(0, 0);
	displayNextLCDString("Startup");
	wait1Msec(2000);
	resetMotorEncoder(BLfly); // reset encoders
	resetMotorEncoder(BRfly);
	resetMotorEncoder(TLfly);
	resetMotorEncoder(TRfly);
	
  slaveMotor(BLfly, TLfly);
  slaveMotor(BRfly, TRfly);

	pre_auton();

	//wait1Msec(500);
2

A couple of issues here.

You have assigned BLfly and BRfly to be under PID control. These should be the motors you control and are the “master” motors. The slaveMotor function has the first parameter as the slave motor, so you have those backwards. They should be.


  slaveMotor(TLfly, BLfly);
  slaveMotor(TRfly, BRfly);

In your autonomous code you need to command the master motors, so modify that as well to be.


    motor[BLfly] = 49;
    motor[BRfly] = 49;

You will need to make similar changes in the usercontrol code.

Finally, although not strictly an issue, I would try and avoid modifying the Vex_Competition_Includes.c file, the few lines you have added can be placed in the pre_auton function as follows.

void pre_auton()
{
  resetMotorEncoder(BLfly); // reset encoders
  resetMotorEncoder(BRfly);
  resetMotorEncoder(TLfly);
  resetMotorEncoder(TRfly);
  
  slaveMotor(TLfly, BLfly);
  slaveMotor(TRfly, BRfly);
  
  bStopTasksBetweenModes = true;
  bLCDBacklight = true;
  clearLCDLine(0);
  clearLCDLine(1);
}

You can see where the pre_auton function is called right after the lines you inserted so it’s better to keep the use the default file.

3

Thank You @pearman for taking your time. Will modify and let you know how this goes

4

@pearman, I have swaped the wires and also modified the code to reflect your recommendations. Now I have issue with encoders they are not blinking at all and when I enable PID wheels doesn’t spin. I see that power is being sent but flywheels are not moving at all.

5
6

If you’re seeing that power is being sent to the motors in the debug window, that means it’s likely a wiring issue. Make sure everything is plugged in properly.

7

thanks for your prompt response, I have replaced the wires and made sure that I have all the wires connected. Now I see red light on all the IME’s do I need to do any reset for them to get initialized. Another Question I have is, I have four motors overall two motors are powering one flywheel and other two motors are powering another flywheel. In the master/slave setup. Do I need to have IME’s plugged in through daisy chain for the slave motors or I don’t need to worry about that. In my current setup I have all the motors are daisy chained with IME’s.

Any help is greatly appreciated. I am nearing to the competition, I wanted to really make the PID thing to make it work.

8

You do not need IME’s on the slave motors, you can disconnect those. Make sure you did not accidentally plug the IME’s into a UART port, red led means not initialized or some other problem.

9

@Jpearman, I appreciate your prompt response, I have finally removed other two slave motors from the daisy chain of IME’S and sending only power to the two master motors on each flywheel. Now I see that all the motors are running at 127 speed. I am not sure what is going on here. Any recommendations

10

You are probably trying to set a speed for the motors that is too high. The program is running the motors at the highest power possible in an attempt to reach this speed, but it’s simply not possible for the motors to go that fast. Try lowering the speed you set the flywheels to.

11

I have set the master motors speed to 48, do you think it is too high?