Over the last couple of weeks, I and my team have been working on using a GSP sensor for our robot skills challenge. I have got the code mostly finalized but I wanted to see if anything looks off to anyone. In theory, the PD values are the only things I need to adjust before it will fully work. The code below is the most up-to-date version of my code with comments to help understand what is going on.
#include "vex.h" // Include vex library to use vex functions
#include <cmath> // Include cmath library to use math functions
float currentX; // initialize variable
float currentY; // initialize variable
float currentA; // initialize variable
float checkX; // initialize variable
float checkY; // initialize variable
float checkA; // initialize variable
float ErrorXCheck; // initialize variable
float ErrorYCheck; // initialize variable
float ErrorACheck; // initialize variable
double derivative_of_error;
double prevError;
double speed;
int gpsTask(){ // define gpsQuality() function
while(1){
if(gpsL.quality() > gpsR.quality()) { // check if the quality of gpsL is greater than gpsR or not
currentX = gpsL.xPosition(inches); // Assign value of x position of gpsL to currentX variable
currentY = gpsL.yPosition(inches); // Assign value of y position of gpsL to currentY variable
currentA = gpsL.heading(degrees); // Assign value of heading of gpsL to currentA variable
Brain.Screen.printAt(1, 20, "Absolute X: %f inches", currentX);
Brain.Screen.printAt(1, 40, "Absolute Y: %f inches", currentY);
Brain.Screen.printAt(1, 60, "Absolute Rotation: %f Radians, %f Degrees", currentA * M_PI/180, currentA);
} else {
currentX = gpsR.xPosition(inches); // Assign value of x position of gpsR to currentX variable
currentY = gpsR.yPosition(inches); // Assign value of y position of gpsR to currentY variable
currentA = gpsR.heading(degrees); // Assign value of heading of gpsR to currentA variable
Brain.Screen.printAt(1, 200, "Absolute X: %f inches", currentX);
Brain.Screen.printAt(1, 220, "Absolute Y: %f inches", currentY);
Brain.Screen.printAt(1, 240, "Absolute Rotation: %f Degrees", currentA);
}
}
}
void setDriveSpeed( float LFSpeed, float RFSpeed, float RBSpeed, float LBSpeed , float RMSpeed, float LMSpeed) { // Define setDriveSpeed() function
LF.spin(fwd, LFSpeed, pct); // Set speed of LF to LFSpeed
LB.spin(fwd, LBSpeed, pct); // Set speed of LB to LBSpeed
RB.spin(fwd, RBSpeed, pct); // Set speed of RB to RBSpeed
RF.spin(fwd, RFSpeed, pct); // Set speed of RF to RFSpeed
LM.spin(fwd, LMSpeed, pct); // Set speed of LM to RBSpeed
RM.spin(fwd, RMSpeed, pct); // Set speed of RM to RFSpeed
}
void driveStop() { // define driveStop() function
LF.stop(hold); // Stop LF motor
LB.stop(hold); // Stop LB motor
RB.stop(hold); // Stop RB motor
RF.stop(hold); // Stop RF motor
RM.stop(hold); // Stop RM motor
LM.stop(hold); // Stop LM motor
}
float PD(double error, double kp, double kd){
derivative_of_error = (error - prevError);
prevError = error;
speed = (kp * error) + (kd * derivative_of_error);
return speed;
}
void goToPosition(float targetX, float targetY, float targetA, float timeoutsec) { // define goToPosition() function
Brain.setTimer(0,sec); // Set timer 0 to sec
float errorX = targetX - currentX; // Calculate the error in X axis
float errorY = targetY - currentY; // Calculate the error in Y axis
float errorA = targetA - currentA; // Calculate the error in heading
float lastErrorX = errorX; // initialize variable lastErrorX with errorX
float lastErrorY = errorY; // initialize variable lastErrorY with errorY
float lastErrorA = errorA; // initialize variable lastErrorA with errorA
double kPX = 1.35;
double kDX = .05;
double kPY = 1.35;
double kDY = .05;
double kPA = .13;
double kDA = 0.007;
float maxAngle = 20; // initialize variable maxAngle with 60
float maxX = 20; // initialize variable maxX with 80
float maxY = 20; // initialize variable maxY with 80
while (Brain.timer(sec) < timeoutsec) { // Loop will run till timer 0 value is less than timeoutsec
errorX = targetX - currentX; // Calculate the error in X axis
errorY = targetY - currentY; // Calculate the error in Y axis
errorA = targetA - currentA; // Calculate the error in heading
ErrorXCheck = errorX; ErrorYCheck = errorY; ErrorACheck = errorA; // Assign values to Error variables
float radA = currentA * (M_PI/180); // Calculate radian value of currentA
float outputX = PD(errorY * std::cos(radA) + errorX * std::sin(radA), kPX, kDX); // Calculate outputX
float outputY = PD(errorX * std::cos(radA) - errorY * std::sin(radA), kPY, kDY); // Calculate outputY
float outputA = PD(errorA * 1.5, kPA, kDA); // Calculate outputA
checkX = outputX; checkY = outputY; checkA = outputA; // Assign values to check variables
float motorSpeedLF = outputX + outputY + outputA; //LF // Calculate LF motor speed
float motorSpeedRF = outputX - outputY - outputA; //RF // Calculate RF motor speed
float motorSpeedRB = outputX + outputY - outputA; //RB // Calculate RB motor speed
float motorSpeedLB = outputX - outputY + outputA; //LB // Calculate LB motor speed
float motorSpeedRM = outputX - outputA; //RM // Calculate RM motor speed
float motorSpeedLM = outputX + outputA; //LM // Calculate LM motor speed
if(outputA > maxAngle) {outputA = maxAngle;} // Check if outputA is greater than maxAngle or not
if(outputA < -maxAngle) {outputA = -maxAngle;} // Check if outputA is less than -maxAngle or not
if(outputY > maxY) {outputY = maxY;} // Check if outputY is greater than maxY or not
if(outputY < -maxY) {outputY = -maxY;} // Check if outputY is less than -maxY or not
if(outputX > maxX) {outputX = maxX;} // Check if outputX is greater than maxX or not
if(outputX < -maxX) {outputX = -maxX;} // Check if outputX is less than -maxX or not
setDriveSpeed( motorSpeedLF, motorSpeedRF, motorSpeedRB, motorSpeedLB, motorSpeedRM, motorSpeedLM); // Call function setDriveSpeed()
lastErrorX = errorX; // Assign errorX value to lastErrorX
lastErrorY = errorY; // Assign errorY value to lastErrorY
lastErrorA = errorA; // Assign errorA value to lastErrorA
}
setDriveSpeed(0, 0, 0, 0, 0, 0); // Call function setDriveSpeed()
driveStop(); // Call function driveStop()
}
int positionTracking() { // Define function positionTracking()
while(1){
//gpsTask();
Brain.Screen.printAt(1, 20, "Absolute X: %f inches", currentX);
Brain.Screen.printAt(1, 40, "Absolute Y: %f inches", currentY);
Brain.Screen.printAt(1, 60, "Absolute Rotation: %f Radians, %f Degrees", currentA * M_PI/180, currentA);
Brain.Screen.printAt(1, 80, "Speed X: %f percent", checkX);
Brain.Screen.printAt(1, 100, "Speed Y: %f percent", checkY);
Brain.Screen.printAt(1, 120, "Speed angle: %f percent", checkA);
Brain.Screen.printAt(1, 140, "Error X: %f inches", ErrorXCheck);
Brain.Screen.printAt(1, 160, "Error Y: %f inches", ErrorYCheck);
Brain.Screen.printAt(1, 180, "Error angle: %f inches", ErrorACheck);
Brain.Screen.printAt(1, 180, "Inertial heading: %f degrees", Inertial.heading());
}
}