Anywhere I can do better?

VEX V5 General Discussion Technical Discussion
1

Would this code be good for a mobile goal lift with PID in driver control? The target (designated as mogoT) 3900 is just having the mobile goal lift near mobile goal height on the floor so I don’t have to worry about my mobile goal being at the correct height when I want to pick up the mobile goal, and the target 2500 is when I hold the mobile goal in my bot. I wrote this as PSEUDO CODE since that I did not have access to RobotC at the time. Please criticize and give suggestions on where I could be more efficient so I can improve as a programmer.

int mogoT = 0;

Void moveMogo (int speed)
{
motor[mogoLeft] = speed;
motor[mogoRight] = speed;
}

Task DriverControl
{

While (true)
{

If (vexRT[Btn8U] == 1)
{
mogoT = 3900;
StartTask(mogoPID);

}

else if (vexRT[Btn8D] == 1)
{
mogoT = 2500;
StartTask(mogoPID);

}

else
{
StopTask(mogoPID);

}

}

}

Task mogoPID
{
Float mogoSpeed;
Float mogoError;
Float mogoKp = .04;
Float mogoProportion;

Float mogoIntegral = 0;
Float mogointegralRaw;
Float mogoKi = 0.01;

Float mogoDerivative = 0;
Float mogolastError = 0;
Float mogoKd = 9;

motor[mogoLeft] = 0;
motor[mogoRight] = 0;

while(true)
{

mogoError = mogoT - ((SensorValue[mogopotLeft] + SensorValue[mogopotRight])/2);

mogoProportion = mogoError * mogoKp;

mogointegralRaw += mogoError;

mogoIntegral = mogointegral * mogoKi;

mogoDerivative = (mogoError - mogolastError) * mogoKd;

mogoSpeed = (mogoProportion + mogoIntegral - mogoDerivative);

mogolastError = mogoError;

moveMogo (mogoSpeed);

wait1Msec(20);

if (mogoIntegral > 50)
{
mogoIntegral = 0
}
else if (mogoError < 0 && mogoIntegral < -50)
{
mogoIntegral = 0;
}
}

}

2

Assuming your PID works correctly, what’s happening right now is that you have to hold down those buttons in order to hold the mobile goal at that desired value. That may work fine for you, but I highly suggest just always having the PID running and the button just changing the desired value. So instead of doing


startTask(mogoPID);

every time the button is pressed, start it at the beginning of the driver control method and just have the buttons only change the mogoT.

3

Ayy a fellow Siege fan!

4

I would look to generalize your PID functions. We’re deploying something like 5 loops this year, and having a separate task for each would be quite impractical (and very inefficent)

5

Yeah that makes more sense to me now that I think about it. Less stress on my fingers as I drive.

Yep!

It would kinda suck for me to have to write so many different tasks… But then again, my bot only needs two PID tasks for driver period, one for the mobile goal and one for the lift.

6

You wouldn’t want to stop the PID task like you are currently. PID works best when it can keep running repeatedly. Also, there’s no reason to set the motor speeds to 0 in your task.

7

@WL457 Structurally, your code looks OK, but you’ve got an issue with your wind-up prevention logic - at the end, you’re conditionally resetting the “mogoIntegral” value that noone cares for anymore. You’d need to reset the mogointegralRaw instead. Also, you might want to limit the integral rather than reset it.
I see there are more typos in the code, so it’s hard to reason about the behavior, but integral limit of 50 times Ki of 0.01 is pretty much zero (Perhaps the limit is meant to be applied to the Ki-multiplied integral term, yet reset the raw value, that would be reasonable).

Other than that, the real-world behavior depends on your constant selection. From the cursory inspection, your Kp looks too low - for those 1400 ticks you’re moving, your proportional term contribution would only be in the 30-50 range.
On the other hand, your derivative constant Kd seems waaaay too large - let me assume 1s linear transition between 2500 and 3900, that makes the derivation about 70 every 20ms and you’re multiplying it by 9.
Or, taken from the other side, with proportional term in fifties, your Kd of 9 would only allow moving at the speed of 5 ticks per iteration (20ms), thus forcing the transition to be slower than 5-6s…

8