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