/**
* initialize an IME process (call after initIME())
**/
void initIMEProc(int motor, int withPID, int tol) {
resetIME(motor);
if (withPID == 1) {
DefineIntegratedMotorEncoderPID(motor, 0.5, 0, 0.3, tol);
StartIntegratedMotorEncoderPID(motor, 0);
}
}
////////////////////////////////////////
//autonomous
task autonomous()
{
slaveMotor(armL2, armL3);
slaveMotor(armL1, armL2);
slaveMotor(armR1, armR3);
slaveMotor(armR2, armR1);
resetIME();
grabAuto(127, 1250);//clutch preload
resetIME();
drive(127, 1000); //drive forward
resetIME();
rotateL(127, 1000);//turn 180 degrees
resetIME();
drive(-127, -850);//reverse to wall
resetIME();
armAuto(127, 250);//lift arm
resetIME();
grabAuto(-127, 1500);//release preload
resetIME();
armAuto(-127, -800);//arm back down
resetIME();
rotateR(127, 400);//rotate towards cube
resetIME();
drive(127,500);//drive towards cube
resetIME();
grabAuto(127, 1250);//clutch cube
resetIME();
rotateL(127, 400);//rotate towards fence
resetIME();
drive(-127, -850);//backup
resetIME();
armAuto(127,250);//raise arm
resetIME();
grabAuto(-127, 1250);//dump cube
resetIME();
armAuto(-127, -800);//lower arm
resetIME();
}
