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

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

}
}
``````

I browse CD

Chief Delphi
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