# Programming for PID from square 1

I am trying to help the team I am mentoring implement something like PID using IMEs on each side of their dual flywheel ball shooter.

We have reviewed what PID is and why they might need it. Now comes the implementation in the code. I have a limited understanding of coding in ROBOTC, please help me walk step by step though this process.

They are using high speed geared 393 motors.
HS motor and IMEs = 392 ticks per revolution
62,720 TPM (at full speed)
1,045 Ticks per second (at full speed)

My first question is, how do you measure Ticks per Revolution over time?

``````
//create two variables
float measuredTicksR;
float measuredTicksL;

//set the initial value of the variables
measuredTicksR = 0;
measuredTicksL = 0;

while (true)
{
nMotorEncoder[rightWheel] = 0;
nMotorEncoder[leftWheel] = 0;
wait(10);
measuredTicksR = nMotorEncoder[rightWheel];
measuredTicksL = nMotorEncoder[leftWheel];

}
``````

The next goal is creating an error variable, setting a speed goal, and comparing the goal to the measured ticks, then setting motor speeds.

``````
//create variables
float measuredTicksR;
float measuredTicksL;
float speed; //measured in tics per (10) ms
float erroR;
float erroL;

//set the initial value of measuredTicks
measuredTicksR = 0;
measuredTicksL = 0;
speed = 0;
errorR = 0;
errorL = 0;

while (true)
{

//when button is pressed, increase speed by the equiv. of power level 10
if (vexRT[Btn8U] == 1)
{
speed = speed + .822;
}

//when button is pressed, decrease speed by the equiv. of power level 10
if (vexRT[Btn8D] == 1)
{
speed = speed - .822;
}

//limit speed value
if (speed > 10.45)
{
speed = 10.45;
}

if (speed <= 0)
{
speed = 0;
}

nMotorEncoder[rightWheel] = 0;
nMotorEncoder[leftWheel] = 0;
wait(10);
measuredTicksR = nMotorEncoder[rightWheel];
measuredTicksL = nMotorEncoder[leftWheel];

errorR = speed - measuredTicksR;
errorL = speed - measuredTicksL;

//speed value is measured in tics per (10) ms
motor[rightWheel] = (speed + errorR) * 12.15;

//speed value is measured in tics per (10) ms
motor[leftWheel] = (speed + errorL) * 12.15;

}
``````

I know this isn’t proportional yet.
I know this would be really clunky, especially without a deadband for the target speed.
Is the basic concept close to correct?
How do you clean up the code?
What would the next step be?

Any help will be appreciated.

I can give you feedback on the code, but have you looked at what I posted in this thread? There’s not that much difference in implementation between the TBH (take back half) algorithm and PID.

Thanks, from what I have read so far you have done a great job explaining it with that post.

I like how you measure/handle elapsed time.

``````
long
FwMotorEncoderGet()
{
return( nMotorEncoder Motor_FW1 ] );
}

void
FwCalculateSpeed()
{
int     delta_ms;
int     delta_enc;

// Get current encoder value
encoder_counts = FwMotorEncoderGet();

delta_ms = nSysTime - nSysTime_last;
nSysTime_last = nSysTime;
}
``````

Why use “FwMotorEncoderGet()” and not just put “nMotorEncoder Motor_FW1 ]” in your code? Is that so you can customize it easier in the future? or does it have to do with executing multiple functions concurrently?

It’s mostly habit and the desire to abstract hardware from the functionality of the rest of the code.

You could easily use nMotorEncoder motor_port ], but what would happen of you switched to a red quad encoder? Then that line of code would need to be SensorValue digital_port ], and what of the quad encoder returned values counting the wrong way, then it would need to be negated. If you want that encoder value at more than one place in the code then all lines need to be changed, sometimes we forget and miss one, so I wrap the call and put the detail of how to access the hardware in that call.

Notice I do the same with the motor control, I use FwMotorSet( motor_drive ); rather then setting motors directly, this abstracts how many motors I have and if any need negated control values are needed (which is a bad habit by the way, set the reversed flag in the motors&sensors dialog).