task autonomous() {
// Autonomous
setIntakeSpeed(127); //drop buckie ball
setArmSpeed(100);
wait1Msec(1000);
setArmSpeed(5);
driveArcade(70,0); //drive forward
wait1Msec(1100);
driveArcade(0,0);
setIntakeSpeed(0);
setArmSpeed(20);
wait1Msec(1000);
driveArcade(-70,0); //drive back
wait1Msec(1200);
driveArcade(0,0);
wait1Msec(5000);
driveArcade(70,0); //drive forward
wait1Msec(1300);
driveArcade(0,0);
wait1Msec(1000);
driveArcade(-70,0); //drive back
wait1Msec(1300);
driveArcade(0,0);
}
void simpleAutoIntake(int cycles){
for (int i =0; i< cycles; i++){
delay(1000);
setIntakeSpeed(100);
delay(1000);
setIntakeSpeed(0);
}
}
task usercontrol() {
// User control initialization
while (true) { // User control loop
int driveX = scaleInput(vexRT[Ch4]);
int driveY = scaleInput(vexRT[Ch3]);
int armSpeed = vexRT[Ch2];
int intakeSpeed = 127*((vexRT[Btn5U]|vexRT[Btn5D])-(vexRT[Btn6U]|vexRT[Btn6D]));
/* string potenString, gyroString;
s sprintf(potenString, "%3f%c", SensorValue[poten]);
sprintf(gyroString, "%3f%c", SensorValue[gyro]);
displayString(potenString, gyroString);*/
if (SensorValue[poten] > ARMAX) armSpeed = 0; // Limit the Poten from dying.
driveX = turnPID.enabled ? calculate(turnPID, SensorValue[gyro]) : driveX;
armSpeed = armPID.enabled ? calculate(armPID, SensorValue[poten]) : armSpeed;
if (abs(driveY) < 5) driveY = 0; // Deadband
if (abs(driveX) < 5) driveX = 0;
driveArcade(driveY, driveX);
setArmSpeed(armSpeed);
setIntakeSpeed(intakeSpeed);
}
}
void intakeAction(void *param) {
int iterations = 0;
while (!isIntakeReady() && (iterations++ < (intakeBreakout / 10))) {
setIntakeSpeed(pLoopDetermineIntakeSpeed());
delay(10);
}
intakeDone = true;
}