GPS Code Help

This is my first post to the forum. My team has been working on a GPS code for our robot over the last month or so and it mostly works now but it is still inconsistent on getting to the position I put in the gotoposition function. I don’t know why though this is because I have messed with the PD variables quite a bit and still haven’t gotten it to work. Do I need to keep tuning it or is there something else wrong with my code? If anyone has any insight on this I would love feedback.

#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
    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()

    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());
1 Like

When I was first testing out the GPS sensor and what values it returned it would fluctuate its position by ±2 inches even without being moved. I thought the errorX, Y, and A floats were accounting for that but it looks more like they are solving for the x and y vectors to the desired point. I don’t know myself how you would account for the error other than taking the GPS’s position a bunch of times, averaging it, and then using that but I’m not sure it could even handle that. Another thing I saw was that the angle the bot turns to in order to face the desired point is an input instead of being solved by using the differences in the x and y positions to solve for the magnitude of the bot’s movement and then finding the angle from that. I may be looking at this from the wrong point of view but yall’s code is really intimidating compared to what I’m used to so I can’t see what everything is meant to do.