Issue with our Autonomous

Programming Support
#1

Hello everyone, my team is in need of help as we’ve hit a roadblock. In our code we have run into two problems. Firstly, whenever our robot executes a certain piece of code in our autonomous, the robot doesn’t finish any of its other commands during autonomous and just lies on the field without making any moves. In our code linked down below, the command is, “FrontClampForward(100,500);”. Additionally, at the beginning of our code for each Auton, its seems that if our robot is drifting toward the left side. Does anyone know why this happens? Lastly, if you have any suggestions or tips for our code, please let us know, this is our first year in participating in vexV5 competitions!

/*----------------------------------------------------------------------------*/
/* */
/* Module: main.cpp */
/* Author: {} */
/* Created: {1/14/21} */
/* Description: V5 project */
/* */
/*----------------------------------------------------------------------------*/

// Include the V5 Library
#include "vex.h"

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// BumperA bumper A 
// ---- END VEXCODE CONFIGURED DEVICES ----
// Allows for easier use of the VEX Library
using namespace vex;

//Main Competition
vex::competition Competition;

//Robot Controller
vex::controller Controller1 = vex::controller();

//vex::gearSetting::ratio18_1

//The Wheels
vex::motor BackLeftMotor (vex::PORT2, false);
vex::motor BackRightMotor (vex::PORT14, false);
vex::motor FrontLeftMotor (vex::PORT10, false);
vex::motor FrontRightMotor (vex::PORT18, true);

//Forklift
vex::motor ForkLiftLeftMotor (vex::PORT12, vex::gearSetting::ratio18_1, false);
vex::motor ForkLiftRightMotor (vex::PORT13, vex::gearSetting::ratio18_1, true);

//Forklift Clamp
vex::motor ForkLiftClampMotor (vex::PORT9, vex::gearSetting::ratio18_1, true);

//Backside Claw
vex::motor BackClawMotor (vex::PORT4, vex::gearSetting::ratio18_1, true);

////////////////////////////////////////////////
////////////////////////////////////////////////

//Degrees to Inches
float inch = 28.6487346;

//Arm Speed In UserControl
int ArmSpeed = 300;

//Auton Select Variaables
int HS = 1;
int AutonLeft1 = 0;
int AutonLeft2 = 0;
int AutonLeft3 = 0;
int TrialandError = 0;
int AutonRight1 = 0;
int AutonRight2 = 0;
int AutonRight3 = 0;
int Skills = 0;

//Function For Moving Forward
void TurnLeft(int speed, int TurnValue){
 FrontRightMotor.spinFor(-TurnValue, rotationUnits::deg, speed, velocityUnits::pct, false );
 FrontLeftMotor.spinFor(TurnValue , rotationUnits::deg, -speed, velocityUnits::pct, false);
 BackRightMotor.spinFor(TurnValue, rotationUnits::deg, -speed, velocityUnits::pct, false);
 BackLeftMotor.spinFor(TurnValue , rotationUnits::deg, -speed, velocityUnits::pct, true);
}

//Function For Turning Right
void TurnRight(int speed, int TurnValue){
 FrontRightMotor.spinFor(TurnValue, rotationUnits::deg, -speed, velocityUnits::pct, false );
 FrontLeftMotor.spinFor(-TurnValue, rotationUnits::deg, speed, velocityUnits::pct, false);
 BackRightMotor.spinFor(-TurnValue, rotationUnits::deg, speed, velocityUnits::pct, false);
 BackLeftMotor.spinFor(-TurnValue, rotationUnits::deg, speed, velocityUnits::pct, true);

}

//Function For Moving Forward
void MoveForward(int speed, int NumberOfInches){
 FrontRightMotor.spinFor(-NumberOfInches * inch, rotationUnits::deg, speed, velocityUnits::pct, false );
 FrontLeftMotor.spinFor(-NumberOfInches * inch, rotationUnits::deg, speed, velocityUnits::pct, false);
 BackRightMotor.spinFor(NumberOfInches * inch, rotationUnits::deg, -speed, velocityUnits::pct, false);
 BackLeftMotor.spinFor(-NumberOfInches * inch, rotationUnits::deg, speed, velocityUnits::pct, true);

}

//Function For Moving Backward
void MoveBackward(int speed, int NumberOfInches){
 FrontRightMotor.spinFor(NumberOfInches * inch, rotationUnits::deg, -speed, velocityUnits::pct, false);
 FrontLeftMotor.spinFor(NumberOfInches * inch, rotationUnits::deg, -speed, velocityUnits::pct, false);
 BackRightMotor.spinFor(-NumberOfInches * inch, rotationUnits::deg, speed, velocityUnits::pct, false);
 BackLeftMotor.spinFor(NumberOfInches * inch, rotationUnits::deg, -speed, velocityUnits::pct, true);

}

//Function For Moving Arms Down
void MoveArmDown(int speed, int ArmMillimeters){
 ForkLiftRightMotor.spinFor(ArmMillimeters, rotationUnits::deg, speed, velocityUnits::pct, true );
 ForkLiftLeftMotor.spinFor(ArmMillimeters, rotationUnits::deg, speed, velocityUnits::pct, true );
}

//Function For Moving Arms Up
void MoveArmUp(int speed, int ArmMillimeters){
 ForkLiftRightMotor.spinFor(-ArmMillimeters, rotationUnits::deg, -speed, velocityUnits::pct, true );
 ForkLiftLeftMotor.spinFor(-ArmMillimeters, rotationUnits::deg, -speed, velocityUnits::pct, true );
}

//Function For Grabbing Neutral Mobile Goals
void FrontClampForward(int speed, int ClampMillimeters){
 ForkLiftClampMotor.spinFor(-ClampMillimeters, rotationUnits::deg, -speed, velocityUnits::pct, true );
}

//Function For Dropping Neutral Mobile Goals
void FrontClampBackward(int speed, int ClampMillimeters){
 ForkLiftClampMotor.spinFor(ClampMillimeters, rotationUnits::deg, speed, velocityUnits::pct, true );
}

//Function For Grabbing Alliance Mobile Goals
void BackClawFoward(int speed, int ClawMillimeters){
 BackClawMotor.spinFor(-ClawMillimeters, rotationUnits::deg, -speed, velocityUnits::pct, true );
}

//Function For Dropping Alliance Mobile Goals
void BackClawBackward(int speed, int ClawMillimeters){
 BackClawMotor.spinFor(ClawMillimeters, rotationUnits::deg, speed, velocityUnits::pct, true );
}


//HomeScreen Of Brain On AutonSelect
void HomeScreen(){
 Brain.Screen.clearScreen();

 Brain.Screen.drawRectangle(0,0,120,120, black);
 Brain.Screen.printAt(45,60, false, "L#1");

 Brain.Screen.drawRectangle(120,0,120,120, black);
 Brain.Screen.printAt(165,60, false, "L#2");

 Brain.Screen.drawRectangle(240,0,120,120, black);
 Brain.Screen.printAt(285,60, false, "L#3");

 Brain.Screen.drawRectangle(360,0,120,120, red);
 Brain.Screen.printAt(405,60, false, "T&E");

 Brain.Screen.drawRectangle(0,120,120,120, blue);
 Brain.Screen.printAt(45,180, false, "R#1");

 Brain.Screen.drawRectangle(120,120,120,120, blue);
 Brain.Screen.printAt(165,180, false, "R#2");

 Brain.Screen.drawRectangle(240,120,120,120, blue);
 Brain.Screen.printAt(285,180, false, "R#3");

 Brain.Screen.drawRectangle(360,120,120,120, green);
 Brain.Screen.printAt(405,180, false, "SK");
 
 Brain.Screen.drawLine(240,0,240,240);
 Brain.Screen.drawLine(0,120,480,120);
 Brain.Screen.drawLine(120,0,120,240);
 Brain.Screen.drawLine(360,0,360,240);

//While Loop Used To Determine Which Side Of Autonomous Will Be Used
while(HS == 1){
 if(Brain.Screen.pressing()){
 //Set Up Auton Left #1
 if(Brain.Screen.xPosition() >= 0 && Brain.Screen.xPosition() <= 120 && Brain.Screen.yPosition() >= 0 && Brain.Screen.yPosition() <= 120){
 AutonLeft1 = 1;
 HS = 0;
 Brain.Screen.print("Autonomous Left Side #1");
 Brain.Screen.clearScreen();
 Brain.Screen.setCursor(1,1);
 }

 //Set Up Auton Left #2
 if(Brain.Screen.xPosition() >= 120 && Brain.Screen.xPosition() <= 240 && Brain.Screen.yPosition() >= 0 && Brain.Screen.yPosition() <= 120){
 AutonLeft2 = 1;
 HS = 0;
 Brain.Screen.print("Autonomous Left Side #2");
 Brain.Screen.clearScreen();
 Brain.Screen.setCursor(1,1);
 }

 //Set Up Auton Left #3
 if(Brain.Screen.xPosition() >= 240 && Brain.Screen.xPosition() <= 360 && Brain.Screen.yPosition() >= 0 && Brain.Screen.yPosition() <= 120){
 AutonLeft3 = 1;
 HS = 0;
 Brain.Screen.print("Autonomous Left Side #3");
 Brain.Screen.clearScreen();
 Brain.Screen.setCursor(1,1);
 }

 //Set Up Trial and Error
 else if(Brain.Screen.xPosition() >= 360 && Brain.Screen.xPosition() <= 480 && Brain.Screen.yPosition() >= 0 && Brain.Screen.yPosition() <= 120){
 TrialandError = 1;
 HS = 0;
 Brain.Screen.print("Autonomous Trial & Error");
 Brain.Screen.clearScreen();
 Brain.Screen.setCursor(1,1);
 }
 
 //Set Up Auton Right #1
 else if(Brain.Screen.xPosition() >= 0 && Brain.Screen.xPosition() <= 120 && Brain.Screen.yPosition() >= 120 && Brain.Screen.yPosition() <= 240){
 AutonRight1 = 1;
 HS = 0;
 Brain.Screen.print("Autonomous Right Side #1");
 Brain.Screen.clearScreen();
 Brain.Screen.setCursor(1,1);
 }

 //Set Up Auton Right #2
 else if(Brain.Screen.xPosition() >= 120 && Brain.Screen.xPosition() <= 240 && Brain.Screen.yPosition() >= 120 && Brain.Screen.yPosition() <= 240){
 AutonRight2 = 1;
 HS = 0;
 Brain.Screen.print("Autonomous Right Side #2");
 Brain.Screen.clearScreen();
 Brain.Screen.setCursor(1,1);
 }

 //Set Up Auton Right #3
 else if(Brain.Screen.xPosition() >= 240 && Brain.Screen.xPosition() <= 360 && Brain.Screen.yPosition() >= 120 && Brain.Screen.yPosition() <= 240){
 AutonRight3 = 1;
 HS = 0;
 Brain.Screen.print("Autonomous Right Side #3");
 Brain.Screen.clearScreen();
 Brain.Screen.setCursor(1,1);
 }

 //Set Up Auton Skills
 else if(Brain.Screen.xPosition() >= 360 && Brain.Screen.xPosition() <= 480 && Brain.Screen.yPosition() >= 120 && Brain.Screen.yPosition() <= 240){
 Skills = 1;
 HS = 0;
 Brain.Screen.print("Skills Auton");
 Brain.Screen.clearScreen();
 Brain.Screen.setCursor(1,1);
 }

 else{}

 }
}
}

//AutonSelect Will Be Played During Pre Auton
void pre_auton( void ){
 HomeScreen();
 ForkLiftClampMotor.setStopping(hold);
 BackClawMotor.setStopping(hold);
}

void auton_left1(){

 //Robot Moves Forward
 MoveForward(100,48);

 wait(1000, msec);

 //Robot Moves Forward
 MoveForward(80,1);

 //Robot Clamp Moves Forward
 FrontClampForward(100,500);

 //Robot Moves Backward
 MoveBackward(100,46);

 //Robot Turns Left
 TurnLeft(70,325);

 //Robot Moves Backward
 MoveBackward(100,3);

 wait(1000, msec);

 //Robot Moves Backward
 MoveBackward(80,1);

 //Robot Claw Moves Forward
 BackClawFoward(100, 500);
}

void auton_left2(){

 //Robot Moves Forward
 MoveForward(100,78);

 wait(1000, msec);

 //Robot Moves Forward
 MoveForward(80,1);

 //Robot Clamp Moves Forward
 FrontClampForward(100,500);

 //Robot Moves Backward
 MoveBackward(100,68);

 //Robot Turns Left
 TurnLeft(70,325);

 //Robot Moves Backward
 MoveBackward(100,8);

 wait(1000, msec);

 //Robot Moves Backward
 MoveBackward(80,1);

 //Robot Claw Moves Forward
 BackClawFoward(100, 500);
} 

void auton_left3(){

 //Robot Moves Forward
 MoveForward(100,48);

 wait(1000, msec);

 //Robot Moves Forward
 MoveForward(80,1);

 //Robot Clamp Moves Forward
 FrontClampForward(100,500);

 //Robot Turns Left
 TurnLeft(70,355);

 //Robot Moves Backward
 MoveBackward(100, 33);

 //Robot Claw Moves Forward
 BackClawFoward(100, 500);

 //Robot Turns Left
 TurnLeft(70,163);

 //Robot Moves Forward
 MoveForward(100,48);
}


void TandE(){
 //Robot Claw Moves Forward
 BackClawFoward(100, 500);
}

void auton_right1(){

 //Robot Moves Forward
 MoveForward(80,44);

 wait(1000, msec);

 //Robot Moves Forward
 MoveForward(80,1);

 //Robot Clamp Moves Forward
 FrontClampForward(100,500);

 //Robot Moves Backward
 MoveBackward(100,35);

 //Robot Turns Left
 TurnLeft(70,325);

 //Robot Moves Backward
 MoveBackward(100,12);

 wait(1000, msec);

 //Robot Moves Backward
 MoveBackward(80,1);

 //Robot Claw Moves Forward
 BackClawFoward(100, 500);

 //Robot Moves Forward
 MoveForward(100,8);
} 

void auton_right2(){

 //Robot Moves Forward
 MoveForward(80,78);

 wait(1000, msec);

 //Robot Moves Forward
 MoveForward(80,1);

 //Robot Clamp Moves Forward
 FrontClampForward(100,500);

 //Robot Moves Backward
 MoveBackward(100, 51);

 //Robot Turns Left
 TurnLeft(70,163);

 //Robot Moves Backward
 MoveBackward(80,12);

 wait(1000, msec);

 //Robot Moves Backward
 MoveBackward(80,1);

 //Robot Claw Moves Forward
 BackClawFoward(100, 500);

 //Robot Moves Forward
 MoveForward(100,8);

} 

void auton_right3(){
 //Robot Moves Forward
 MoveForward(80,56);

 wait(1000, msec);

 //Robot Moves Forward
 MoveForward(80,1);

 //Robot Clamp Moves Forward
 FrontClampForward(100,500);

 //Robot Turns Right
 TurnRight(70,325);

 //Robot Moves Backward
 MoveBackward(100, 33);

 wait(1000, msec);

 //Robot Moves Backward
 MoveBackward(80,1);

 //Robot Claw Moves Forward
 BackClawFoward(100, 500);

 //Robot Turns Right
 TurnRight(70,163);

 //Robot Moves Forward
 MoveForward(100,48);
} 

void auton_skills(){
 //Robot Moves Forward
 MoveForward(50,100);
 
 //Robot Turns Left
 TurnLeft(50,30);

 //Robot Moves Backward
 MoveBackward(50,70);

 //Robot Turns Right
 TurnRight(50,60);

 //Robot Moves Forward
 MoveForward(50,70);

 //Robot Turns Left
 TurnLeft(50,60);

 //Robot Moves Backward
 MoveBackward(50,70);

} 

//Void Function For Autonomous
void autonomous( void ){
 //Code for left side of field will be displayed for autonomous
 if (AutonLeft1 == 1){
 auton_left1();
 }

//Code for left side of field will be displayed for autonomous
 else if (AutonLeft2 == 1){
 auton_left2();
 }

 //Code for left side of field will be displayed for autonomous
 else if (AutonLeft3 == 1){
 auton_left3();
 }

 //Code for trial and error will be displayed for autonomous
 else if (TrialandError == 1){
 TandE();
 }

 //Code for right side of field will be displayed for autonomous
 else if (AutonRight1 == 1){
 auton_right1();
 }

 //Code for right side of field will be displayed for autonomous
 else if (AutonRight2 == 1){
 auton_right2();
 }

 //Code for right side of field will be displayed for autonomous
 else if (AutonRight3 == 1){
 auton_right3();
 }

 //Code for skills will be displayed
 else if (Skills == 1){
 auton_skills();
 }

 else{}
}

//Void Function For Driver Control
void usercontrol( void ) {
 while(1) {
 //Using Arcade Control
 //////////////////////
 //Motors Allow Robot To Move Forward, Backward, Right, and Left
 BackLeftMotor.spin(vex::directionType::rev, Controller1.Axis3.value()+Controller1.Axis1.value(), vex::velocityUnits::pct);
 BackRightMotor.spin(vex::directionType::fwd, Controller1.Axis3.value()-Controller1.Axis1.value(), vex::velocityUnits::pct);
 FrontLeftMotor.spin(vex::directionType::rev, Controller1.Axis3.value()+Controller1.Axis1.value(), vex::velocityUnits::pct);
 FrontRightMotor.spin(vex::directionType::rev, Controller1.Axis3.value()-Controller1.Axis1.value(), vex::velocityUnits::pct);

 //Arms Can Move Down
 if (Controller1.ButtonR1.pressing()) {
 ForkLiftLeftMotor.spin(vex::directionType::fwd, ArmSpeed, vex::velocityUnits::pct);
 ForkLiftRightMotor.spin(vex::directionType::fwd, ArmSpeed, vex::velocityUnits::pct);
 }

 //Clamp Can Move Down 
 else if (Controller1.ButtonX.pressing()) {
 ForkLiftClampMotor.spin(vex::directionType::fwd, ArmSpeed, vex::velocityUnits::pct);
 } 

 //Claw Can Move Down
 else if (Controller1.ButtonUp.pressing()) {
 BackClawMotor.spin(vex::directionType::fwd, ArmSpeed, vex::velocityUnits::pct);
 } 

 //Arms Can Move Up
 else if (Controller1.ButtonR2.pressing()) {
 ForkLiftLeftMotor.spin(vex::directionType::rev, ArmSpeed, vex::velocityUnits::pct);
 ForkLiftRightMotor.spin(vex::directionType::rev, ArmSpeed, vex::velocityUnits::pct);
 //Bumper Used To Stop Arms From Moving Too Far Back
 if (BumperA.pressing()){
 ForkLiftLeftMotor.stop(brakeType::hold);
 ForkLiftRightMotor.stop(brakeType::hold);
 }
 }

 //Clamp Can Move Up
 else if (Controller1.ButtonB.pressing()) {
 ForkLiftClampMotor.spin(vex::directionType::rev, ArmSpeed, vex::velocityUnits::pct);
 }

 //Claw Can Move Up
 else if (Controller1.ButtonDown.pressing()) {
 BackClawMotor.spin(vex::directionType::rev, ArmSpeed, vex::velocityUnits::pct);
 }
 //Motors Will Not Move
 else {
 ForkLiftLeftMotor.stop(vex::brakeType::brake);
 ForkLiftRightMotor.stop(vex::brakeType::brake);
 ForkLiftClampMotor.stop(vex::brakeType::brake);
 BackClawMotor.stop(vex::brakeType::brake);
 }

 vex::task::sleep(20);
 }
}

int main(){
 //Tells Competition To Run 'Usercontrol' During Drivercontrol
 Competition.drivercontrol(usercontrol);

 //Tells Competition To Run 'Autonomous' During Autonomous
 Competition.autonomous(autonomous);
 
 pre_auton();

 while(1){
 //Conserves Energy During Competition
 vex::task::sleep(100);
 }
}

edit: code tags added by mods, please remember to use them,

1 Like
#2

It probably means that this motor cannot achieve moving that far, either change the number of degrees you ask the motor to move (and you are mixing mm and deg) or add a motor timeout.

5 Likes
#3

When your robot stops moving, pick it up (carefully!) and if it keeps going, what jpearman said is probably right. You can set timeouts to end the command after such-and-such amount of time, regardless if the clamp reached the intended position.

1 Like
#4

your code can be simple. Moving forward and backward could be one function with positive/negative for forward and backward respectively.

In your drive code, too much if-else. Make it short for each motor/motor group. You need one if-else if-else for arm. and then other if-else if-else for lift, etc.

#5

If you want to get into advanced coding and save code space, you could look into a switch case for this:

Disclaimer: I barely know what a switch case is.

So I could be wrong. And I think you probably are not up to needing that yet. If you declared AutonLeft1, etc. as bool instead of int, you could (and would have to) assign to them true or false instead of 1 or 0, and then you could just code: if (AutonLeft1) {... and leave out the == 1, since AutonLeft1 evaluates to true by itself.

#6

Thanks for the advice! I noticed that you said we were mixing mm and deg. Can you please elaborate on that?

#7

There are other places like this in your code, too. You named your input variable ArmMillimeters, which would suggest you think your input value is in millimeters, but your spinFor input is rotationUnits::deg, which means the motor will spin ArmMillimeters degrees, where 360 is a full rotation. It will not cause any errors in the code, since the code doesn’t care what you name your variables, but it may cause confusion to your readers, and it suggests you think you’re working in millimeters.

And, like @largetail said, you don’t need one function for driving forwards and one for driving backwards, you could just put in negative numbers. I wouldn’t prefer coding that way, though, since it seems unintuitive to write driveFoward(-100, -500); and have the robot go backwards. My personal habit is to write really short function names. My function for going forward is called fd(), backward is bk(), right is rt(), and left is lt().

And if we’re going to get into

Most of the time when you input your speed, you are putting in 100. When you define a function, you say, void MoveArmDown(int speed, int ArmMillimeters) {... If you said void MoveArmDown(int ArmMillimeters, int speed = 100) {..., it would set the default for speed to 100, and whenever you only entered one parameter into MoveArmDown(), that would be ArmMillimeters and speed would default to 100. Be warned, though, you would have to go through all your code and switch around your ArmMillimeters and speed inputs.

Another thing you can do with only positive results is to make your second and third if s in your auton selector else ifs. It saves brain power and makes code easier to read.

4 Likes
#8

exactly as @242EProgrammer explained.
When reading the code, the parameter name suggests the parameter units are mm, but in fact it’s used as degrees. Functionally the code works, but anyone else reading the code may mis-understand how it works, and that may be you. One of the often quoted laws of programming.

“Any code of your own that you haven’t looked at for six or more months might as well have been written by someone else.”

5 Likes