So yesterday I posted my old code for incorporating velocity control into our robot and it was pretty bad, but we got some help and I tried to clean it up. I just want to make sure this works properly, so could someone look over it and give me some pointers?
Thanks, here is the code.
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1, leftFlywheel, sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Sensor, I2C_2, rightFlywheel, 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 !!*//
static int target; //defines global variable for target velocity
task velocityControl; //defines velocityControl don't touch this
float FwMotorEncoderGet(int side); // defines the two functions used to calc actual velocity
float FwCalculateSpeed(int side);
void setVelocity(int speed); //function to choose target velocity
task main() //beginning of driver control
{
nMotorEncoder[leftFlywheel] = 0; //sets the motor encoders to 0 outside of the loop
nMotorEncoder[rightFlywheel] = 0; //so that it doesn't continually reset
startTask (velocityControl); //starts up the velocity control task
while(1 == 1){
//dummy text for flywheel
if(vexRT[Btn8D] == 1 ){
setVelocity(70);
}
//Intake code
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;
}
//Wheel 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;
}
//end wheel control
}
//end drivercontrol
}
task velocityControl(){
float Lvelocity; //initilizes and calculates
float Rvelocity; //left and right velocities
Lvelocity = FwCalculateSpeed(1);
Rvelocity = FwCalculateSpeed(0);
//initilize all the variables needed in PID
float kp = 0.01; //proportional constant
float ki = 0.000000; //integral constant
float kd =0.00; //derivative constant
float Lcurrent = 0;
float Rcurrent = 0;//starts with no power
float Lerror; //current error constants
float Rerror;
float integralActiveZone = 2; //integral error accumulates
float LerrorT; //total error
float LlastError; //last error recored
float RerrorT; //total error
float RlastError; //last error recored
float Lproportion; //proportion term
float Lintegral; //integral term
float Lderivative; //derivative term
float Rproportion; //proportion term
float Rintegral; //integral term
float Rderivative; //derivative term
while(true){
//start of PID algorithm loop
Lerror = target - Lvelocity; //difference between current vs target velocity
Rerror = target - Rvelocity;
if(Lerror<integralActiveZone && Lerror !=0){ //active integral zone
LerrorT += Lerror; //adds error
}
else {
LerrorT = 0;
}
if(Rerror<integralActiveZone && Rerror !=0){ //active integral zone
RerrorT += Rerror; //adds error
} else {
RerrorT = 0;
}
if(LerrorT>50/ki){ //caps error at 50
LerrorT = 50/ki;
}
if(Lerror == 0) { //if error zero then derivative zero
Lderivative = 0;
}
if(RerrorT>50/ki) { //caps error at 50
RerrorT = 50/ki;
}
if(Rerror == 0) { //if error zero then derivative zero
Rderivative = 0;
}
//A bunch of PID calculations for the left and right sides
Lproportion = Lerror * kp; //setting proportion term
Lintegral = LerrorT * ki; //setting integral term
Lderivative = (Lerror - LlastError)*kd;//setting derivative term
Rproportion = Rerror * kp; //setting proportion term
Rintegral = RerrorT * ki; //setting integral term
Rderivative = (Rerror - RlastError)*kd;//setting derivative term
LlastError = Lerror; //last error becomes current error for next loop
RlastError = Rerror; //last error becomes current error for next loop
Lcurrent = Lproportion + Lderivative + Lintegral; //setting power
Rcurrent = Rproportion + Rderivative + Rintegral; //setting power
//take the value from PID and put it into the motors
motor[flyWheelL1] = motor[flyWheelL2] = Lcurrent; //giving motors power
motor[flyWheelR1] = motor[flyWheelR2] = Rcurrent;
wait1Msec(20); //proventing hogging of CPU power
}
//end velocity control
}
//function to set desired speed in program
void setVelocity( int speed ) {
target = speed; //setss what you put into it as the variable target
}
//function to get the actual velocity of each side
float FwCalculateSpeed(int side){
float velocity; //initializes all the variables
float delta_ms;
float delta_enc;
float nSysTime_last = 0;
float ticks_per_rev = 627.2;
float encoder_counts;
float encoder_counts_last;
// Get current encoder value for a side
if (side == 1) {
encoder_counts = FwMotorEncoderGet(1);
}
if (side != 1) {
encoder_counts = FwMotorEncoderGet(0);
}
// 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
velocity = (1000.0 / delta_ms) * delta_enc * 60.0 / ticks_per_rev;
return velocity; //returns the value so that velocity control can store it in a variable
}
//function to get the value of the motor encoders
float FwMotorEncoderGet(int side) {
float count; //init variable we store ticks in
if (side == 1) { //gets the ticks for one side depending on what you feed into it
nMotorEncoder[leftFlywheel] = count;
}
if (side != 1) {
nMotorEncoder[rightFlywheel] = count;
}
return count;
}