ROBOTC has PID to control your flywheel

VEX V5 General Discussion Technical Discussion
#21

An IME has the ability to internally measure velocity (it measures the amount time from the last transition of the internal gear with the black/white segments). ROBOTC can read this information but to do that you need to turn on that ability, that’s what the flag bUseVexI2CEncoderVelocity is use for. The downside to having this turned on is that we need to read more information from the IME and this takes a little longer and slows down the poll rate slightly.

Not something you need to use really, has its origins in the LEGO version I think.

2 Likes
#22

This looks great. We have not yet updated to RobotC 4 yet but plan on doing so in the next couple weeks. I look forward to trying this. My only question is how this is different/better than coding my own PID loop. I’m also curious about how RobotC handles integral wind-up.

#23

Quick question, can I use a single motor with a encoder that in the program is a single motor just Y’ed off, what I am thinking is that I can reduce any kind of error by 50% and then also have the PID loop for the other side of the robot for the wheels?

#24

Not sure I completely understand what you are asking, however, is the question is “Can I use this with two motors on a Y cable?” then then the answer is “as long as the motor under PID control has an IME, then yes this will work”. Using a Y cable is just like slaving two motors.

2 Likes
#25

I wanted to explain a little more about the numbers in the PID dialog.
pid_3.jpg

The defaults for a 393 motor on an MC29 are

50 - Update interval - How often the internal PID loop runs
10 - Max rate of change, the pwm value will only change by 10 every interval, this is the slew rate
12 - Close enough, only relevant when using position control PID.
1000 - max encoder counts per second, see explanation below.
10 - Motor deadband, absolute values below this will not move the motor.
33 - Kp
45 - Ki
60 - Kd
100 - Divisor. The actual constants are 0.33 (ie. 33/100), 0.45 and 0.60. We use a divisor so all math is integer. Floating point math is computationally expensive.

So what does the “max encoder counts per second” do. The ROBOTC PID loop is working a little differently from code that I and others have published. The idea is that if you send the motor a maximum control value of 127 then that means that the motor will try and run at the maximum encoder count per second rate. ie. 127 would be 1000 counts/second. From this you can figure out what the speed in rpm will be knowing the motor gearing. From ROBOTC’s point of view it does not matter if the motor is set for torque, speed or turbo. The IME actually counts in the same way no matter how the motor is geared. When you send a value of 64 (or half maximum control value) the PID loop will try and run the motor at (max encoder counts per second)/2 or 500 counts/sec with the default numbers.

Example, with the above default numbers, motor on speed gearing, how to get 120 rpm on the motor output shaft. We know that for a speed motor there are 392 counts per revolution of the output shaft. 120 rpm is two rotations/second so we need 784 counts/second from the IME. Max is 1000 so control value will be 127/1000*784 = 99.57, so send the motor a value of 100. Another way would be to drop the max counts per second to 830 and then motor control values would be in rpm directly, you can figure out the math for that. Downside will be maximum motor speed will be reduced. To get the absolute maximum from an individual motor, the max counts per second should be measured and the default changed.

2 Likes
#26

I’ve personally never used any of RobotC’s PID functions (mainly because I was comfortable with robotC 3.6x, and I tried my hand at using the tutorials to program my own) but now that I’m running off of trial versions of robotC (since my computer crashed and is getting repaired), I was interested in using 4.x’s features for PID. Is it as simple as enabling it, tuning the values, and just say, set something for example


if(vexRT[Btn5U])
{
     motor[port1] = 100;
}
else motor[port1] = 0;

in driver control, and it will automatically use a pre-programmed thing to try its best to keep it at 100 speed?

And in autonomous, would it work the same way if I were to try keeping it under a certain tick count? Such as

while(SensorValue[leftmotor] < 1000)
{
     motor[port1] = 100;
}
motor[port1] = 0;

Or would I have to use a different function for autonomous?

I couldn’t ever find this basic knowledge through hours of browsing so I apologize if it’s been answered countless times. Thank you.

#27

A question was asked about how to turn on and off the ROBOTC PID. For the example in post #1.

// Turn off PID
 if( vexRT Btn7U ] == 1 )
    nMotorPIDSpeedCtrl port2 ] = RegIdle;

// Turn on PID
if( vexRT Btn7D ] == 1 )
    nMotorPIDSpeedCtrl port2 ] = RegSpeed;

However, although this works there is one small problem. When PID is turned off with the motors running, the master motor will immediately jump to the commanded speed you sent it. For example, lets say you sent it a value of 50, the PID may actually be sending a small pwm value to the motor, say 25, you can see these values in the motors debug window. As we had three slave motors in the original code they also would be getting the same pwm value. When PID is turned off for the master motor the slave motors will not be sent the last command you had sent to the master motor. Bottom line is that you need to resend the command to the master motor if PID control is disabled. Here is a revised version of the code in post #1 with PID enable and disable.


#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port2,            ,             tmotorVex393HighSpeed_MC29, PIDControl, encoderPort, I2C_1)
#pragma config(Motor,  port3,            ,             tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port4,            ,             tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor,  port5,            ,             tmotorVex393HighSpeed_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
    int motorSpeed;
    
    // Motors on ports 3, 4 & 5 will follow the motor on port 2
    slaveMotor(port3, port2);
    slaveMotor(port4, port2);
    slaveMotor(port5, port2);

    // Driver control loop
    while(1) {
      // Two different speeds.
      if( vexRT Btn8U ] == 1 )
        motorSpeed = 50;
      else
      if( vexRT Btn8R ] == 1 )
        motorSpeed = 100;
      else
      if( vexRT Btn8D ] == 1 )
        motorSpeed = 0;

      // Enable/disable PID  
      if( vexRT Btn7U ] == 1 )
        nMotorPIDSpeedCtrl port2 ] = RegIdle;
      else
      if( vexRT Btn7D ] == 1 )
        nMotorPIDSpeedCtrl port2 ] = RegSpeed;

      motor port2 ] = motorSpeed;

      wait1Msec(20);
    }
}
2 Likes
#28

I assume RobotC has a actual PID Loop for you to input all the necessary values. Is there any way we can see that PID Loop?

#29

No, sorry, that’s proprietary information that belongs to Robomatter. Even if I could show you that code it’s not easy to understand.

1 Like
#30

Will this work with y-cables? I have my 4 flywheel motors on the power expander, and through y-cables (so I can use 12 motors).
I tried it, followed instructions, and it simply sends a motor power of 127 to all 4 motors no matter what ‘PID power’ I send to it. Thanks for the help!

#31

It should work through Y cables. Are you sure you have everything configured properly?

#32

Its configured properly, I followed instructions. However, my y-cables control motors on two different sides of the launcher. So, one port controls a motor on the left and right side of the launcher. I feel like that might be causing the problem.

#33

Wait, so you’re still assigning power? Or is this function assigning speed, because if it is still battery dependent then what is the purpose of even using the control? I’m sure I am missing something.

nevermind, found the post, sorry about that.

#34

Its not assigning power, the PID loop assigns a varying power based on the encoder values. Not battery dependent. You still use the same motor[value] command, but it is on a scale of 0-100.

#35

Yeah, makes sense, I didn’t know this was available, pretty cool!

#36

Alright, so changing my y-cable setup did fix the problem. Now I need to learn how to tune the PID loop…
It seems to take a while to learn where it needs to be, it speeds up and slows down, by a smaller and smaller amount until it becomes stable. Any tuning tips?

#37

sounds like your gain is too high, it’s oscillating.

#38

Correct me if I am wrong as I am still new to VEX, but don’t the motors turn negatively when they overshoot? Yesterday, before I saw this thread, I tried exactly what you did and I ended up destroying some of the internal gears in our motors, so now I am off to replace the gears. Where in this program would I incorporate if the motor runs at a power below 0, the motors will stop, and then PID control will take over again? Has anyone dealt with what I have before?

#39

Yes, this is correct

#40

I have had this problem with a drive, when operating the PID controller I would have my robot moving backwards. This was due to the Derivative term of the PID controller. It would be ideal to use PI controller or many teams including myself have been accustomed this year to use TBH Controllers