/** * 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(); }