We have some code issues. Help?

VEX V5 Technical Support
1

Hello there, My friend and I have been trying to get this code working for about a week. I have spent more time on the forums then talking to my family, but I know something isn’t right. We are trying to get PID working for our double flywheel (3 standard torque motors on each side, 35:1 external gear ratio, and a IME on a motor on each side. I can send a picture if anyone wants one) a week ago we recreated the flywheel and for the first time all season it has shot WAYYYYY over the high goal, which is very exciting but now we have new problems like accuracy, which we didn’t have to worry about when our robot didn’t actually work. Anyway, after hearing something about PID on the forums we decided to take a look (we also read about TBH but it was after we had already worked on PID a bunch and didn’t want to switch over, plus we didn’t understand the code any more than we did with PID). So we watched a bunch of videos and borrowed some code (of course from people who said it was okay to borrow stuff) made some up ourselves and we think we have PID working fine (the constants aren’t calibrated yet, but we know how to do that) the main issue is getting the actual velocity of the flywheel and putting it into PID for it to calculate the error and stuff. We have a task getVelocity (I don’t know if it would be better as a function instead of a task, same with task shooterControl (which is PID) I’m thinking shooterControl would be better as a function that took in the desired power and the actual velocity like “void shooterControl (power, velocity)” or something but that’s not the main problem at the moment) the main problem is the getVelocity task. I don’t think it works properly. Just looking at the code it doesn’t seem like it actually gets the velocity of the flywheel. (and yes I realize that at the moment everything is set up for a single flywheel, but that isn’t much of a change we just have to double up on PID and the IME code).
Okay so that’s all probably very confusing, but I just wanted to know if there was anyone out there who could help me fix up this code. Specifically the getVelocity task and the parts of the shooterControl task that call the getVelocity task. Also anything else you see that could help us out would be very much appreciated. Thank you to anyone who decides to help.

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port1,           IW1,           tmotorVex393HighSpeed_HBridge, openLoop)
#pragma config(Motor,  port2,           DW1,           tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port3,           DW2,           tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port4,           DW3,           tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port5,           DW4,           tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port6,           flyWheelL1,    tmotorVex393_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor,  port7,           flyWheelL2,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           flyWheelR1,    tmotorVex393_MC29, openLoop, encoderPort, I2C_2)
#pragma config(Motor,  port9,           flyWheelR2,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port10,          IW2,           tmotorVex393HighSpeed_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

float power;
static float velocity;
float error;
task shooterControl; //defines shooterControl don't touch this
task getVelocity;

task main()
{
	while(1 == 1)
{

//dummy text for flywheel
if(vexRT[BtnXX] == 1 ){
	power = 65;
	startTask (shooterControl);
}


 //Intake

  if(vexRT[Btn7L] == 1)
  {
	motor[IW1] = 127;
	motor[IW2] = 127;
	}
	else
	{
		if(vexRT[Btn7D] == 1)
		{
				motor[IW1] = -127;
				motor[IW2] = -127;
	}
		else
		{
	motor[IW1] = 0;
	motor[IW2] = 0;
		}
	}

	//drive control

  if(vexRT[Btn6U] == 1 )
  {
  	motor[DW1] = -75;
  	motor[DW2] = -75;
  	motor[DW3] = -75;
  	motor[DW4] = -75;
  }
  else if(vexRT[Btn5U] == 1)
	  {
	  motor[DW1] = 75;
  	motor[DW2] = 75;
  	motor[DW3] = 75;
  	motor[DW4] = 75;
		}
	else if(vexRT[Ch2] > 15) {
				motor[DW1] = -100;
				motor[DW2] = 100;
				motor[DW3] = 100;
				motor[DW4] = -100;
			}
	else if(vexRT[Ch2] < -15)
					{
								motor[DW1] = 100;
								motor[DW2] = -100;
								motor[DW3] = -100;
								motor[DW4] = 100;
					}
		else
			{
				motor[DW1] = 0;
				motor[DW2] = 0;
				motor[DW3] = 0;
				motor[DW4] = 0;
			}


}
}

task shooterControl()
{
resetMotorEncoder(nMotor);
resetMotorEncoder(nMotor);
startTask(getVelocity);
float kp = 0.01; //proportional constant
float ki = 0.000000; //integral constant
float kd =0.00; //derivative constant

float current = 0; //starts with no power

float integralActiveZone = 2; //integral error accumulates

float errorT; //total error
float lastError; //last error recored
float proportion; //proportion term
float integral; //integral term
float derivative; //derivative term


while(true)
 {

	error = power - velocity; //difference between current vs target velocity

	if(error<integralActiveZone && error !=0) //active integral zone
	{
			errorT += error; //adds error
	}else
	{
		errorT = 0;
	}

	if(errorT>50/ki) //caps error at 50
  {
		errorT = 50/ki;
  }
	if(error == 0) //if error zero then derivative zero
	{
		derivative = 0;
	}

	proportion = error * kp; //setting proportion term
	integral = errorT * ki; //setting integral term
	derivative = (error - lastError)*kd;//setting derivative term

	lastError = error; //last error becomes current error for next loop

	current = proportion + derivative + integral; //setting power

	motor[flyWheelL1] = motor[flyWheelL2] = motor[flyWheelR1] = motor[flyWheelR2] = current; //giving motors power

	wait1Msec(20); //proventing hogging of CPU power

 }
}

task getVelocity() {
	while(1 == 1)
	{
	float gearRatio = 35.0;
 	float timeDelay = 20.0;
 	float ticks = 627.2;

 	float leftEncoderLast;
 	float rightEncoderLast;
 	float rightEncoder;
 	float leftEncoder;
 	float leftVelocity;
 	float rightVelocity;

 	float speedRPM;


 	leftEncoder = SensorValue[I2C_1];
 	rightEncoder = SensorValue[I2C_2];

 	leftVelocity = (leftEncoder - leftEncoderLast)/(timeDelay * 0.001);
 	rightVelocity = (rightEncoder - rightEncoderLast)/(timeDelay * 0.001);

 	velocity = (leftVelocity + rightVelocity)/2;

	leftEncoderLast = leftEncoder;
	rightEncoderLast = rightEncoder;


	speedRPM = velocity * gearRatio * 60 * ticks;


	wait1Msec(timeDelay);

	}
}
2

Funny story, I think I actually may have found some velocity code that I should be able to adapt to work for my robot by the infamous jpearman. I had seen a bunch of his stuff before but it was above me and not much help, but I recently stumbled upon a post he made that was more for beginners. This is the code, I’m pretty sure it will work but if it won’t for my set up , don’t hesitate to say something.

/*-----------------------------------------------------------------------------*/
/** @brief      Calculate the current flywheel motor velocity                  */
/*-----------------------------------------------------------------------------*/
void
FwCalculateSpeed()
{
    int     delta_ms;
    int     delta_enc;
    
    // Get current encoder value
    encoder_counts = FwMotorEncoderGet();

    // This is just used so we don't need to know how often we are called
    // how many mS since we were last here
    delta_ms = nSysTime - nSysTime_last;
    nSysTime_last = nSysTime;

    // Change in encoder count
    delta_enc = (encoder_counts - encoder_counts_last);

    // save last position
    encoder_counts_last = encoder_counts;

    // Calculate velocity in rpm
    motor_velocity = (1000.0 / delta_ms) * delta_enc * 60.0 / ticks_per_rev;
}
3

You would first need to name your Integrated Motor Encoders (IMEs):

#pragma config(Sensor, I2C_1,  leftFlywheel,   sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  rightFlywheel,  sensorQuadEncoderOnI2CPort,    , AutoAssign )

(You can name them leftFw, leftF, or whatever you want)
And to reset your encoder values, you will need to use


nMotorEncoder**name**] = 0;


because they are IMEs.

The general flowchart for velocity control for driver control is:
1. Define, initialize, etc. Have a function that sets velocity like setVelocity(speed).
2. Start flywheel velocity control task. This will be:
a) Calculate speed.
b) Find error from target. Calculate P (error), I (accumulation of error), and D (change of error per time).
c) Set motor power.
d) Wait for a bit (like .025 seconds).
e) Go back to a).
3. Start driver control loop. Use buttons to change flywheel velocity using setVelocity(speed).
Now, you need to figure out what you should make a task and what you should make a function. This post is very helpful.
Your velocity control and driver control will be tasks (driver control basically already is a task), and your velocity set and calculation will be functions.
Your velocity calculation is good. However, you should use nMotorEncoder] instead of SensorValue[I2C_1] to get encoder values. The velocity calculation should be a function that is called in the velocity control loop to make sure it gets new values every run. (Also, you may want to calculate left and right velocity separately in the future since you may want to make both sides run close to the same speed.)
Additionally, just to be Claritin® clear, you may want to rename “power” as “target”.
The setVelocity function will look something like this:

void setVelocity(int speed)
{
power /* or "target" if you rename it */ = speed;
}

And you would call it like:

if(vexRT[BtnXX] == 1 )
{
	setVelocity(65);
}

Your program is pretty good for a first attempt :wink: Good luck!
If you have any more questions, I’m sure others will be glad to help. (I think I could have been a bit less ambiguous on some parts, but meh.)

4

Thank You, this helps a lot.

5