Encapsulating Basic Robot Movements in VEXcode Pro V5

The main goal is to show you how to encapsulate some of the fundamental motion codes into functions, not for encoder calculation and motion manipulations.

The following sample motion functions will help make your basic motions code:

• More organized
• More readable
• Less error prone

First of all, you will need to define the following basic variables:

float Wheel = 10.16;
float WB = 36.75;   //based on the V5 Clawbot configuration

Note: If you change the gear ratio, the following calculation may change slightly based on the gear ratio.

This will be discussed in another article focusing on the motor encoder:

float EncPerCm = 360/(wheel* M_PI);
float EncPerDeg = WB/Wheel;

This sample is for pivot turn only. E.g. right motor = 100, left motor = -100, etc.

Secondly, when you review the following samples, you are encouraged to look up the online API reference for the parameters and their specifications. Visit: https://api.vexcode.cloud/v5/html/ and search for, e.g., rotateTo in order to familiarize yourself with the parameters.

---

Sample 1: Encapsulate Going Straight Motion Based on Various Distances

float Wheel = 10.16; 
float WB = 36.75; 
float EncPerCm = 360.0 / (Wheel* M_PI); 

void reportMotionValues(motor m, int line){

  Brain.Screen.printAt(5,line, "%8.2lf%8.2lf%8.2f", 
    m.position(rev), 
    m.position(deg)); 
}

// distance is in centimeters
void goStraight( float distance ){
	
	LeftMotor.resetPosition();
	LeftMotor.resetRotation();
	RightMotor.resetPosition();
	RightMotor.resetRotation();
  	
	float totalEnc = distance * EncPerCm;
	
	// note:  “deg” is from the vex:: namespace. Therefore, you should not 
        // create another variable also named as “deg”. 

       LeftMotor.setVelocity(50.0, percent);
       LeftMotor.spinToPosition(totalEnc, deg, false);
 
       RightMotor.setVelocity(50.0, percent);
       RightMotor.spinToPosition(totalEnc, deg, false);

       while (LeftMotor.isSpinning() || RightMotor.isSpinning() ) 
            wait(50, msec);
  	
	return;
}

int main() {  
  vexcodeInit();

    goStraight(100.0);
    Brain.Screen.printAt(5,60, "Done");
    reportMotionValues(LeftMotor, 30);
    reportMotionValues(RightMotor, 60); 
}

---

Sample 2: Encapsulate Left Turns Based on Various Degrees

float Wheel = 10.16; 
float WB = 36.75; 
float EncPerCm =  360.0 / (Wheel* M_PI); 
float EncPerDeg =  WB / Wheel;

void turnRight( float degrees){
	
	LeftMotor.resetPosition();
	LeftMotor.resetRotation();
	RightMotor.resetPosition();
	RightMotor.resetRotation();
  	
	float totalEnc = degrees * EncPerDeg;
	
       // note: this “deg” is from vex namespace
       LeftMotor.setVelocity(100.0, percent);
       LeftMotor.spinToPosition(totalEnc, deg, false);
 
       RightMotor.setVelocity(-100.0, percent);
       RightMotor.spinToPosition(-totalEnc, deg, false);

       while(LeftMotor.isSpinning() || RightMotor.isSpinning() ) 
          wait(50, msec);
  	
	return;
}

int main(){
   ...
   turnRight(90.0);
   ...
}

---

Sample 3: Encapsulate Both Left and Right in a Single Function

void doTurning( float degrees ) {
	// exactly the same code as the turnRight above
}

int main() {
    ...
   doTurning(90.0);  // Right pivot turn
   ...
   doTurning(-90.0); // Left pivot turn
}

For more advanced programming, if you want to make your code much more readable, you may try the following:

#define doLeftTurn(d)    doTurning(d)
#define doRigtTurn(d)    doTurning(-d)

int main() {
   ...
   doLeftTurn(90.0);  
   doRightTurning(90.0);
   ...
}

#define is called a preprocessor macro expression. Look at this expression as a symbol. Wherever this symbol occurs in the code, it will be replaced with the expression doTurning(the value specified).

Preprocessor macro is a different extensive topic and will not be discussed in this article.