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