Project BioRobotics Group 19
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 9:c19f6f0f5080
- Parent:
- 6:246b05228f52
--- a/main.cpp Mon Oct 23 08:15:07 2017 +0000 +++ b/main.cpp Mon Oct 23 14:13:47 2017 +0000 @@ -34,28 +34,33 @@ PwmOut motor2MagnitudePin(D6); InterruptIn button1(D2); // CONNECT BUT1 TO D2 InterruptIn button2(D3); // CONNECT BUT2 TO D3 -AnalogIn potPin(A1); +InterruptIn button3(SW2); +InterruptIn button4(SW3); +AnalogIn pot1Pin(A0); +AnalogIn pot2Pin(A1); // DEFINITIONS const float motorVelocity = 4.2; // unit: rad/s const float motorGain = 8.4; // unit: (rad/s) / PWM const int motorRatio = 131; // Ratio of the gearbox in the motors int motor1State = 0; // 0: Off, 1: CW, 2: CCW +int motor2State = 0; float motorValue = 0; //TICKERS Ticker encoder; -Ticker checkMotorState; +Ticker checkMotor1State; +Ticker checkMotor2State; // FUNCTIONS void RotateMotor1CW() { motor1DirectionPin = 1; //motor1MagnitudePin = fabs(motorVelocity / motorGain); - motorValue = potPin; + motorValue = pot1Pin; motor1MagnitudePin = motorValue; //pc.printf("Rotating motor 1 CW \r\n"); - pc.printf("%f \r\n", motorValue); + pc.printf("Motor 1 CW %f \r\n", motorValue); } void RotateMotor1CCW() @@ -63,16 +68,16 @@ motor1DirectionPin = 0; //motor1MagnitudePin = fabs(motorVelocity / motorGain); //pc.printf("Rotating motor 1 CounterCW \r\n"); - motorValue = potPin; + motorValue = pot1Pin; motor1MagnitudePin = motorValue; //pc.printf("Rotating motor 1 CW \r\n"); - pc.printf("%f \r\n", motorValue); + pc.printf("Motor 1 CCW %f \r\n", motorValue); } void TurnMotor1Off() { motor1MagnitudePin = 0; - //pc.printf("Motors off \r\n"); + pc.printf("Motor 1 off \r\n"); } void CheckMotor1() // Checks the state of motor1 and rotates motor1 depending on the state @@ -92,6 +97,51 @@ break; } } + +void RotateMotor2CW() +{ + motor2DirectionPin = 1; + //motor1MagnitudePin = fabs(motorVelocity / motorGain); + motorValue = pot2Pin; + motor2MagnitudePin = motorValue; + //pc.printf("Rotating motor 1 CW \r\n"); + pc.printf("Motor 2 CW %f \r\n", motorValue); +} + +void RotateMotor2CCW() +{ + motor2DirectionPin = 0; + //motor1MagnitudePin = fabs(motorVelocity / motorGain); + //pc.printf("Rotating motor 1 CounterCW \r\n"); + motorValue = pot2Pin; + motor2MagnitudePin = motorValue; + //pc.printf("Rotating motor 1 CW \r\n"); + pc.printf("Motor 2 CCW %f \r\n", motorValue); +} + +void TurnMotor2Off() +{ + motor2MagnitudePin = 0; + pc.printf("Motor 2 off \r\n"); +} + +void CheckMotor2() // Checks the state of motor1 and rotates motor1 depending on the state +{ + switch(motor2State) + { + case 0: // Turn motors off + {TurnMotor2Off(); break;} + + case 1: // Rotate motor 1 CW + {RotateMotor2CW(); break;} + + case 2: // Rotate motor 2 CCW + {RotateMotor2CCW(); break;} + + default: + break; + } +} /* void EncoderCalc() { @@ -147,6 +197,10 @@ else if(!button2){motor1State = 2;} else{motor1State = 0;} + if(!button3){motor2State = 1;} + else if(!button4){motor2State = 2;} + else{motor2State = 0;} + /* // Hit command if(// HIT COMMAND) @@ -177,6 +231,7 @@ default: { TurnMotor1Off(); // Turn motors off for safety + TurnMotor2Off(); break; } } @@ -191,7 +246,8 @@ pc.printf("START \r\n"); //encoder.attach(EncoderCalc, 0.5); - checkMotorState.attach(CheckMotor1, 0.0002); + checkMotor1State.attach(&CheckMotor1, 0.01); + checkMotor2State.attach(&CheckMotor2, 0.01); while(true) {