void wheelDrive( int point, char direction )
{
float maxPower = 60.0; // Limit Constants
float integralLimit = 80.0;
float P = 0.42; // PID Constants
float I = 0.001;
float D = 0.0;
float error = point - SensorValue[EncoderR]; // PID Calculations
integral_D += error;
float derivative = error - prev_D;
if( abs( error ) > integralLimit || error == 0 ) // Integral Limit Filter
integral_D = 0;
prev_D = error; // Derivative Reset
float power = P * error + I * integral_D + D * derivative; // Power Calculation
if( abs( power ) > maxPower ) // Power Limit Filter
power = power < 0 ? -maxPower : maxPower;
if(direction == 'R')
moveDrive( power, 0 );
else
moveDrive( 0, power);
}
void setDrive( int point )
{
float maxPower = 120.0; // Limit Constants
float integralLimit = 50.0;
float P = 0.118; // PID Constants
float I = 0.00075;
float D = 0.0;
float error = point - SensorValue[EncoderR]; // PID Calculations
integral_D += error;
float derivative = error - prev_D;
if( abs( error ) > integralLimit || error == 0 ) // Integral Limit Filter
integral_D = 0;
prev_D = error; // Derivative Reset
float power = P * error + I * integral_D + D * derivative; // Power Calculation
if( abs( power ) > maxPower ) // Power Limit Filter
power = power < 0 ? -maxPower : maxPower;
moveDrive( power, power );
}
/////////////////////////////////////////////////////////////////////////////////////////
//
// 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.
//
/////////////////////////////////////////////////////////////////////////////////////////
task autonomous()
{
// goal side Red
if(programChoice == 1){
// Move forward
moveDrive(1, 250);
//move back to pop out intake
moveDrive(-1,250);
//move to first position
moveDrive(1,500);
// Move arm up
moveArm(1,750);
// Move forward to second point
moveDrive(1, 500);
// Wait a secound so that big ball is out of the way
wait1Msec(750);
/*
// Outtake preload
moveIntake(-1);
wait1Msec(2000);
*/
// Drive back to starting square
moveDrive(-1, 1000);
//stop robot for button press to reposition
/* moveDrive(0,3000); */
while (SensorValue[touchSensor] != 1){}
//move robot forward at new angle
moveDrive(1,1300);
//dive back to starting square
moveDrive(-1,1300);
//stop robot wait for reposition
while (SensorValue[touchSensor] != 1) {}
//dive forward
moveDrive(1,1500);
//lift arm just a tig
moveArm(1,250);
//turn left 90 degrees
turnDrive(1,500);
//knock other colour bucky balls off the bump
moveDrive(1,250);
}
// goal side blue
if(programChoice == 2){
// Move forward
moveDrive(1, 250);
//move back to pop out intake
moveDrive(-1,250);
//move to first position
moveDrive(1,500);
// Move arm up
moveArm(1,750);
// Move forward to second point
moveDrive(1, 500);
// Wait a secound so that big ball is out of the way
wait1Msec(750);
// Outtake preload
moveIntake(-1);
wait1Msec(2000);
// Drive back to starting square
moveDrive(-1, 1000);
//stop robot for button press to reposition
/* moveDrive(0,3000); */
while (SensorValue[touchSensor] != 1){}
//move robot forward at new angle
moveDrive(1,1300);
//dive back to starting square
moveDrive(-1,1300);
//stop robot wait for reposition
while (SensorValue[touchSensor] != 1) {}
//dive forward
moveDrive(1,1500);
//lift arm just a tig
moveArm(1,250);
//turn right 90 degrees
turnDrive(-1,500);
//knock other colour bucky balls off the bump
moveDrive(1,250);
}
// Blue side hanging bar side of bump auto (auto 3)
else if(programChoice == 3) {
// Move forward
moveDrive(1, 250);
//move back to pop out intake
moveDrive(-1,250);
//move to first position
moveDrive(1,500);
//stop Robot and intake balls
moveIntake(1);
wait1Msec(750);
//move robot back to starting square
moveDrive(-1, 750);
//Stop robot for reposition 90 degrees
//moveDrive(0, 3000);
while (SensorValue[touchSensor] != 1){}
//Move robot in new direction
moveDrive(1, 750);
//robot Turn 90 degrees left
turnDrive(1, 750);
//Lift arm up a tig
moveArm(1,500);
//Move robot forward over the bump
moveDrive(1,1500);
//lift the arm all the way
moveArm(1,750);
//move forward alittle
moveDrive(1,250);
//outtake preload
moveIntake(1);
wait1Msec(2000);
}
//Red side hanging bar side of bump auto (auto 3)
else if(programChoice ==4) {
// Move forward
moveDrive(1, 250);
//move back to pop out intake
moveDrive(-1,250);
//move to first position
moveDrive(1,500);
//stop Robot and intake balls
moveIntake(1);
wait1Msec(750);
//move drive back to original starting space
moveDrive(-1, 750);
//Stop robot for reposition 90 degrees
//moveDrive(0, 3000);
while (SensorValue[touchSensor] != 1){}
//Move robot in new direction
moveDrive(1, 750);
//robot Turn 90 degrees Right
turnDrive(-1, 750);
//Lift arm up a tig
moveArm(1,500);
//Move robot forward over the bump
moveDrive(1,1500);
//move arm all the way up
moveArm(1,750);
//move move drive a tig
moveDrive(1,250);
//outtake preload
moveIntake(1);
wait1Msec(2000);
}
//skills challenge
else if(programChoice == 3) {
moveDrive(0);
}
}
