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