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 ResetArmEncoders() { //asdfadsf SetArm(-75); while(GetDigitalInput(armResetSwitch)){ } PresetIntegratedMotorEncoder(armR, 0); PresetIntegratedMotorEncoder(armL, 0); }
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; }
/** * preset the motor's encoder to 0 **/ void resetIME(int motor) { PresetIntegratedMotorEncoder(motor, 0); }