Coding Help from more experienced coders

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());
    
  }
}

I browse CD to learn what’s going on in the world of robots. I have absolutely no idea what you are doing and I wanted to find out for fun and for my team.

I was interested in your use of GSP sensor - there is such a beast for temperature and humidity. Then I see you refer to GPS in the code variables. Confusing!

I will guess Screen.printAt is a column and row number. I wonder why there are multiple characters put at the same location especially (1, 180, and the redundant 1, 20 1,40 1,60.

Comments are a great addition to any code but if they are confusing they need more explanation. For example, explain how these statements work in light of the comments - something is wrong or there is magic happening and magic in coding always needs to be explained.

float maxAngle = 20; // initialize variable maxAngle with 60
float maxX = 20; // initialize variable maxX with 80
float maxY = 20; // initialize variable maxY with 80

Comments don’t need to repeat half of a statement to which they belong.

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

A more meaningful and complete comment might be:
// Clamp output to the range -max to +max

You might consider making a function to “clamp” and call the function the three times that you use it. That streamlines the main code flow.

And there is no comment at the beginning of the code about what the purpose or objective of the program is. There seem to be outputs and errors and some sort of geometric computation but not much explanation what they are.

What does it mean to define a variable and not assign anything to it? What is the initialization? Zero, null, not a number? Make the effort to write a comment worth it.

float currentX; // initialize variable

Sorry to shred your code. Maybe this is not what you were asking for but it’s all I know about this subject that is new to me.

The purpose of my code is to take the input of one of the two gps sensors on our robot and then assigns the x, y, and angle so I know where it is on the field. It then uses a PD loop to drive to a target position by finding the x, y, and angle speed and assigning the speed to our 6 motor asterisk drive (a x-drive with two forward drive motors for speed)

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.