So my team has been struggling to find an efficient way to code Autonomous for our robot. We’ve been using rotateFor and setting the motors that run at the same time as false and that works well enough, but it is a lot of typing. I’ve tried to come up with a way to call functions into the autonomous() function and then set variables for the motor rotation and velocity. However, only the first function is called and then the program stops. Any help or explanation as to why this happens is very appreciated.
// Autonomous Variables
float LDriveRotation;
float RDriveRotation;
float LDriveVelocity;
float RDriveVelocity;
float LiftRotation;
float LiftVelocity;
float ClawRotation;
float ClawVelocity;
float IntakeRotation;
float LocationVal = 0;
bool DriveTF = false;
bool LiftTF = false;
bool ClawTF = false;
bool IntakeTF = false;
// Autonomous Functions
void Drive(){
LFMotor.rotateFor(LDriveRotation, rotationUnits::rev, LDriveVelocity, velocityUnits::rpm, false);
LBMotor.rotateFor(LDriveRotation, rotationUnits::rev, LDriveVelocity, velocityUnits::rpm, false);
RFMotor.rotateFor(RDriveRotation, rotationUnits::rev, RDriveVelocity, velocityUnits::rpm, false);
RBMotor.rotateFor(RDriveRotation, rotationUnits::rev, RDriveVelocity, velocityUnits::rpm, DriveTF);
}
void Lift(){
LiftMotor.rotateFor(LiftRotation, rotationUnits::rev, LiftVelocity, velocityUnits::rpm, LiftTF);
}
void Claw(){
ClawMotor.rotateFor(ClawRotation, rotationUnits::deg, ClawVelocity, velocityUnits::rpm, ClawTF);
}
void Punch(){
PunchMotor.rotateFor(1, rotationUnits::rev, 100, velocityUnits::pct, true);
}
void Intake(){
IntakeMotor.rotateFor(IntakeRotation, rotationUnits::rev, -200, velocityUnits::rpm, IntakeTF);
}
void Reset(){
LDriveRotation = 0;
RDriveRotation = 0;
LDriveVelocity = 0;
RDriveVelocity = 0;
LiftRotation = 0;
LiftVelocity = 0;
ClawRotation = 0;
ClawVelocity = 0;
IntakeRotation = 0;
DriveTF = false;
LiftTF = false;
ClawTF = false;
IntakeTF = false;
}
void pre_auton( void ) {
}
void autonomous( void ) {
// Calls the corresponding function and sets the motor variables
DriveTF = true;
Punch();
LDriveRotation = 5;
LDriveVelocity = 100;
RDriveVelocity = 5;
RDriveVelocity = 100;
Drive();
Reset();
DriveTF = true;
LDriveRotation = -3;
LDriveVelocity = 100;
RDriveVelocity = -3;
RDriveVelocity = 100;
Drive();
Reset();
DriveTF = true;
LDriveRotation = 0;
LDriveVelocity = 100;
RDriveVelocity = -2;
RDriveVelocity = 100;
Drive();
Reset();
DriveTF = true;
IntakeRotation = 5;
LDriveRotation = 3;
LDriveVelocity = 100;
RDriveVelocity = 3;
RDriveVelocity = 100;
Drive();
Intake();
Reset();
}