Hi Everyone. How can I improve my code for my flywheel? I looked up how to code for a flywheel and people used Take Back Half instead of PID but I don’t get the Take Back Half thing. So I used a PID code. Is there a way to optimize my code? And is my code even rational?
Thanks,
#pragma config(Sensor, dgtl2, FlywheelEncoder, sensorQuadEncoder)
#pragma config(Motor, port1, FlywheelMotor, tmotorVex393HighSpeed_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
long CurrentFlywheelSpeed;//In RPM
long CurrentFlywheelEncoder = SensorValue(FlywheelEncoder);
long FlywheelDeltaTime = time1[T1];
long TargetFlywheelSpeed;//In RPM
long FlywheelError;
long FlywheelPreviousError;
long Integral;
long Derivative;
long Kp = 0;//Placeholder
long Ki = 0;//Placeholder
long Kd = 0;//Placeholder
long FinalPIDSpeed;
long MotorTargetFlywheelSpeed;//range of -127 to 127
long FlywheelTolerance = abs(200)//200 RPM Tolerable Range of Flywheel speed difference (in RPM)
void CurrentFlywheel()//Finds out Current Flywheelspeed
{
while(true)
{
clearTimer(T1);//Timer starts
CurrentFlywheelSpeed = CurrentFlywheelEncoder/FlywheelDeltaTime;//Calculates Flywheel Speed from Encoder/Time it takes
}
}
void TargetFlywheel(int HighFlag, int LowFlag)
{
while(true)
{
if((HighFlag) == 1 )
{
TargetFlywheelSpeed = 3000;//Placeholder for Speed in RPM High flag
}
else if((LowFlag) == 1)
{
TargetFlywheelSpeed = 2500;//Placeholder for Speed in RPM Low flag
}
else
{
TargetFlywheelSpeed = 0;//Flywheel Stop
}
}
}
task FlywheelPID()
{
FlywheelError = TargetFlywheelSpeed - CurrentFlywheelSpeed;//Calculate Error Proportion
FlywheelPreviousError = FlywheelError;//Previous Error
Integral = Integral + FlywheelError*FlywheelDeltaTime;//Integral Calculation
if(FlywheelError == 0)//No Error makes no Integral
{
Integral = 0;
}
else if(Integral < abs(127))//Integral Outside usable Range then Integral goes to max inside range
{
Integral = 127;
}
Derivative = (FlywheelError - FlywheelPreviousError)/FlywheelDeltaTime;//Derivative Calculation
FinalPIDSpeed = Kp*FlywheelError + Ki*Integral + Kd*Derivative;//Final Speed Calculation
motor[FlywheelMotor] = FinalPIDSpeed;//Send to Motors
}
task FlywheelMtr()
{
while(true)
{
MotorTargetFlywheelSpeed = 127/4000*TargetFlywheelSpeed;
//Motor Speed Calculation as 127 is motor maximum capacity and 4000RPM is maximum
if(SensorValue(FlywheelEncoder) < (TargetFlywheelSpeed + FlywheelTolerance)*360)
{
motor[FlywheelMotor] = MotorTargetFlywheelSpeed;
}
//When Flywheel runs less than Target and tolerance, run mtoor
else if(SensorValue(FlywheelEncoder) > (TargetFlywheel + FlywheelTolerance)*360)
{
motor[FlywheelMotor] = 50;//Stays on for warm up
}
//If flywheel is faster than wanted, make motor slower
}
}
1.) long Kp = 0;//Placeholder
long Ki = 0;//Placeholder
long Kd = 0;//Placeholder
This is not quite useful. Take a look here: Integral = 127;
So, an integer multiplied by 127 will give either… 0, 127, or n*127, with n being an integer. That invalidates the attempt to keep your ‘FinalPIDSpeed’ within typical motor values.
1.) The encoder gives back the total ticks it’s traveled. So, your encoder will give back ever increasing numbers, even though it’s velocity might not be changing.
2.) By clearing the timer right before calculating the speed, you’ll have an astronomically low time that you’re dividing by, giving extremely large errors.
void TargetFlywheel(int HighFlag, int LowFlag)
{
while(true)
{
if((HighFlag) == 1 )
{
TargetFlywheelSpeed = 3000;//Placeholder for Speed in RPM High flag
}
else if((LowFlag) == 1)
{
TargetFlywheelSpeed = 2500;//Placeholder for Speed in RPM Low flag
}
else
{
TargetFlywheelSpeed = 0;//Flywheel Stop
}
This would be much better:
void TargetFlywheel(int Flag)
{
while(true)
{
if((Flag) == 1 )
{
TargetFlywheelSpeed = 3000;//Placeholder for Speed in RPM High flag
}
else if((Flag) == 2)
{
TargetFlywheelSpeed = 2500;//Placeholder for Speed in RPM Low flag
}
else
{
TargetFlywheelSpeed = 0;//Flywheel Stop
}
There is no need to over-complicate variables.
Integral = Integral + FlywheelError*FlywheelDeltaTime;//Integral Calculation
if(FlywheelError == 0)//No Error makes no Integral
{
Integral = 0;
}
Integral will already converge towards 0 at an exponential rate, no need to define as 0.
else if(Integral < abs(127))//Integral Outside usable Range then Integral goes to max inside range
{
Integral = 127;
}
Derivative = (FlywheelError - FlywheelPreviousError)/FlywheelDeltaTime;//Derivative Calculation
FinalPIDSpeed = Kp*FlywheelError + Ki*Integral + Kd*Derivative;//Final Speed Calculation
You should be checking if the FinalPIDSpeed is over 127, not integral. Remember, the K values are used to convert the final control value into motor power!
In task FlywheelPID(), your deltaTime is continuously changing! This is not a PID loop, it’s now a PTDTIT loop.
/******************************************************************************
A couple notes:
-dt in continuous mathematics means "an infinitisimally small amount of unit x (in this case, x is time)"
-dt in programmatic mathematics means "the amount of unit x between the last existing value and the current value (again, x is time)"
-think of the d as standing for "delta", or "difference between"
-An integral is equal to all of the instantaneuous values contained on a curve added up. Continuously, this can be thought of as the area under a curve. We call the time between each value dt. in this case, dt is infinitisimally small.
-discretely, we need to use the block theorem. As processors cannot calculate an infinite amount of values per second, we need to 'cheat', by increasing the size of dt.
-Here, we cheat by 'assuming' that the value does not change between measurements. ie, we simply multiply each value by the amount of time between it and the last measured value.
-again, think of the area under the curve. All we are doing is approximating the area under the curve by extending rectangles from the x axis to the first point the rectangle hits on the curve.
-the width is the delta time.
-A differential is equal to the difference between two values, divided by the time it took to get there.
-ie, we get the rate of change:
-delta between valueLast and valueCurrent / delta between their corresponding time stamps.
-A proportion is.... well, just a change in the order of magnitude of the measured value. we increase the value by multiplying some factor to the value.
*******************************************************************************/
#include <stdio.h>
int enc_val_last = 0.0;
int nSysTime_last = 0.0;
int motorPower = 0.0;
//kp,i,d modify just how much each part of the PID loop affects the correction. As motor power and rpm have a linear relationship to eachother, these kvalues can be used to convert rpm to power as well.
const float kp = 1.0;
const float ki = 1.0;
const float kd = 1.0;
int intSum = 0.0;
int error_old = 0.0;
int long dt;
int main()
{
motorPower = motorPower + PID(2500); //call PID loop with target 2500 rpm, adding the resultant correction to the current power.
}
//calculates the correction
int PID(int setPoint){
int error = setPoint - updateRPM();
int correction = prop(error) + integral(error) + diff(error);
nSysTime_last = nSysTime; //save current time, so that we can calculate the next dt
return correction;
}
//Here, we calculate the current flywheel RPM.
int updateRPM(){
int enc_val = sensorValue[flywheelEncoder]; //Get current encoder value
dt = (nSysTime - nSysTime_last);
int rpm = (enc_val_last - enc_val) / dt; //(difference between the last measured encoder value and new measured value) / (delta current time - old time) nSysTime is a predfined variable equal to milliseconds run.
enc_val_last = enc_val; //save current enc_val to old enc_val
return rpm;
}
int prop(int error){
return kp * error; //refer to header
}
int integral(int error){
intSum = intSum + (error * dt); //refer to header
return intSum;
}
int diff(int error){
int diffError = (error_old - error) / dt; //refer to header
return diffError;
}
Here’s an example code I wrote a few years ago. It’ll require slight modification for vex, but it covers the principle. If I remember correctly, JPearman or Tabor have some pretty good codes that are also excellent resources. Lot’s to learn there too.
Yeah! No problem! Actually, you don’t have to convert. The values won’t be in true rpm, but they’ll have a lot less error in them if you don’t simplify them. What we are really going off of here is the the encoder ticks per time unit. You can of course convert the final integral, derivatives and proportional units back into rpm (or convert into rpm there) but you’d have to convert without fixed factors, as dt can vary depending on your other tasks.
You can’t turn the dt into anything except it’s raw value. The dt is the time difference between two measurements. You would have to take your encoder ticks/ 360 (depending on gear ratio) dividing by dt, then multiply by 1000 (1000 for 1000 milliseconds).
@RougeScaless correct, you convert right before multiplying the k-factors. I don’t usually do that though. Rather, I convert my setpoint to match the incoming data, as I’ve found that to be more accurate.
All of the explanations to each component are also in my title block if you’d like some more clarification!
