I tested my program for autonomous after taking out all the electronics and pneumatics on my robot, and rewiring all of them. However when I tested the autonomous, it seemingly skipped over the first distance sensor while loop. If anyone knows why this may be happening it would be greatly appreciated.
Here Is The Code:
using namespace vex;
// A global instance of competition
competition Competition;
// define your global instances of motors and other devices here
/*---------------------------------------------------------------------------*/
/* Pre-Autonomous Functions */
/* */
/* You may want to perform some actions before the competition starts. */
/* Do them in the following function. You must return from this function */
/* or the autonomous and usercontrol tasks will not be started. This */
/* function is only called once after the V5 has been powered on and */
/* not every time that the robot is disabled. */
/*---------------------------------------------------------------------------*/
void pre_auton(void) {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();
// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, ...
}
/*---------------------------------------------------------------------------*/
/* */
/* Autonomous Task */
/* */
/* This task is used to control your robot during the autonomous phase of */
/* a VEX Competition. */
/* */
/* You must modify the code to add your own robot specific commands here. */
/*---------------------------------------------------------------------------*/
void autonomous(void) {
motor_group Drive(lma, lmb, rma, rmb, lmc, rmc);
motor_group leftDrive(lma, lmb, lmc);
motor_group rightDrive(rma, rmb, rmc);
Drive.setVelocity(60, pct);
rSensor.setPosition(0, degrees);
Lift.setVelocity(100, pct);
Lift.setMaxTorque(100, pct);
Lift.setTimeout(4, sec);
Lift.setPosition(0, deg);
Drive.setStopping(coast);
iSensor.calibrate();
wait(2, sec);
digitalOut.set(false);
//Drive to goal
while(dSensor2.objectDistance(mm) > 45){
Drive.spin(reverse);
}
Drive.stop();
digitalOut.set(true);
wait(.5, sec);
//Drive back
//while(rSensor.position(turns){
//Drive.spin(fwd);
//}
//Drive.stop();
Drive.spinFor(fwd, 1.5, turns);
//set stopping mode
Drive.setStopping(brake);
//Turn to platform
iSensor.setRotation(0, deg);
inertial::quaternion Inertial_quaternion;
while(iSensor.rotation(deg) < 122){
leftDrive.spin(fwd, 25, pct);
rightDrive.spin(reverse, 25, pct);
}
//Drive to platform
Lift.startRotateFor(fwd, 2, turns);
Drive.rotateFor(fwd, 8.5,turns);
//10.625
//Drive back a bit
wait(2,sec);
rSensor.setPosition(0,turns);
while(rSensor.position(turns)>-.8){
Drive.spin(reverse,60,pct);
}
Drive.stop();
//Drive.spinFor(reverse, 1.5, turns);
iSensor.setRotation(0, deg);
//turn to platform
iSensor.setRotation(0, deg);
while(iSensor.rotation(deg) < 34){
leftDrive.spin(fwd, 25, percent);
rightDrive.spin(reverse, 25, percent);
}
wait(1, sec);
//drive to platform
Lift.startRotateFor(reverse, 2, turns);
barLift.startRotateFor(fwd, 2, turns);
Drive.rotateFor(reverse,3.3,turns);
//turn to platform
iSensor.setRotation(0, deg);
while(iSensor.rotation(deg) < 90){
leftDrive.spin(fwd);
rightDrive.spin(reverse);
}
Drive.setTimeout(4, sec);
//drive to platform
Drive.spinFor(reverse, 2 , turns);
Drive.setTimeout(60, sec);
barLift.spinFor(reverse, .5, turns);
//let go of goal
digitalOut.set(false);
wait(.5, sec);
//raise lift
barLift.spinFor(fwd, .5, turns);
//drive away from platform
//Drive.spinFor(fwd, .5, turns);
rSensor.setPosition(0,turns);
while(rSensor.position(turns) < .75){
Drive.spin(fwd,60,pct);
}
Drive.stop();
//turn towards red mogo
iSensor.setRotation(0, deg);
while(iSensor.rotation(deg) > -87){
leftDrive.spin(reverse, 25,percent);
rightDrive.spin(fwd, 25, percent);
}
Drive.setVelocity(40,percent);
//Drive to red mogo
Drive.rotateFor(reverse, 3.3, turns);
iSensor.setRotation(0,deg);
while(iSensor.rotation(deg) < 70){
leftDrive.spin(fwd,25,pct);
rightDrive.spin(reverse,25,pct);
}
rSensor.setPosition(0,turns);
while(rSensor.position(turns) > -2){
Drive.spin(reverse,60,pct);
}
rSensor.setPosition(0,turns);
while(rSensor.position(turns) < 2){
Drive.spin(fwd,60,pct);
barLift.spin(reverse,60,pct);
}
iSensor.setRotation(0,deg);
while(iSensor.rotation(deg) > -70){
leftDrive.spin(reverse,25,pct);
rightDrive.spin(fwd,25,pct);
}
//drive to it accurateley
while(dSensor2.objectDistance(mm) > 45){
Drive.spin(reverse,60,pct);
}
Drive.stop();
digitalOut.set(true);
Drive.setVelocity(60,percent);
wait(2,sec);
//back up a bit
//while(rSensor.position(turns){
//Drive.spin(fwd);
//}
//Drive.stop();
Drive.spinFor(fwd,1,turns);
//turn towards neutral mogo
iSensor.setRotation(0, deg);
while(iSensor.rotation(deg)< 80){
rightDrive.spin(reverse,25,pct);
leftDrive.spin(fwd,25,pct);
}
//Drive to mogo
//while(rSensor.position(turns){
//Drive.spin(fwd);
//}
//Drive.stop();
Drive.spinFor(fwd,7,turns);
//drive back to line
//Drive.spinFor(reverse,1,turns);
rSensor.setPosition(0,turns);
while(rSensor.position(turns) >-1.25){
Drive.spin(reverse,60,pct);
}
Drive.stop();
//turn along line
iSensor.setRotation(0, deg);
while(iSensor.rotation(deg)<85){
rightDrive.spin(reverse,25,pct);
leftDrive.spin(fwd,25,pct);
}
//drive along line
barLift.startRotateFor(fwd,4,turns);
Drive.rotateFor(reverse,3.5,turns);
//turn to platform
iSensor.setRotation(0, deg);
while(iSensor.rotation(deg)<90){
rightDrive.spin(reverse,25,pct);
leftDrive.spin(fwd,25,pct);
}
//drive into platform
//Drive.spinFor(reverse,3,turns);
rSensor.setPosition(0,turns);
while(rSensor.position(turns) >-2.75){
Drive.spin(reverse,60,pct);
}
Drive.stop();
//lower lift
barLift.spinFor(reverse,1,turns);
digitalOut.set(false);
Drive.setVelocity(25,pct);
Drive.spinFor(fwd, 5,turns);
}```