void ArmResetStateMachine() {
// switch(armResetState) {
// case STOP_PID:
StopIntegratedMotorEncoderPID(armR);
StopIntegratedMotorEncoderPID(armL);
// armResetState = LOWER_ARM;
// break;
// case LOWER_ARM:
while(GetDigitalInput(armResetSwitch)
&& GetJoystickDigital (1, 7, 2)){
SetArm(-75);
}
WaitInMsec(150);
// armResetState = START_PID;
// break;
// case START_PID:
PresetIntegratedMotorEncoder(armR, 0);
PresetIntegratedMotorEncoder(armL, 0);
StartIntegratedMotorEncoderPID(armR, 0);
StartIntegratedMotorEncoderPID(armL, 0);
SetArmState(GROUND);
// armResetState = DONE;
// break;
// case DONE:
// break;
// }
}
void InitArm() {
long groundTicks = degreesToIMEticks(ArmState);
PresetIntegratedMotorEncoder(armL, groundTicks);
PresetIntegratedMotorEncoder(armR, groundTicks);
DefineIntegratedMotorEncoderPID(armR, ARM_kP, ARM_kI, ARM_kD, ARM_pidTOL);
DefineIntegratedMotorEncoderPID(armL, ARM_kP, ARM_kI, ARM_kD, ARM_pidTOL);
StartIntegratedMotorEncoderPID(armR, groundTicks);
StartIntegratedMotorEncoderPID(armL, groundTicks);
ArmState = GROUND;
}
void AutonomousStartUp() {
StartTimer(GAME_TIMER); //starts game timer
autoAbort = 0;
tripWatch = 0;
SetArmState(GROUND);
//StartIntegratedMotorEncoderPID(driveRIME, 0);
//StartIntegratedMotorEncoderPID(driveLIME, 0);
StartIntegratedMotorEncoderPID(armR, 0);
StartIntegratedMotorEncoderPID(armL, 0);
SetRunningMode(MODE_AUTO);
//ResetArmEncoders();
}
/**
* 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);
}
}
