Project BioRobotics Group 19
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 6:246b05228f52
- Parent:
- 4:ea7689bf97e1
- Child:
- 9:c19f6f0f5080
--- a/main.cpp Wed Oct 11 10:33:11 2017 +0000 +++ b/main.cpp Mon Oct 23 08:15:07 2017 +0000 @@ -14,6 +14,7 @@ states currentState = MOTORS_OFF; // Start with motors off bool stateChanged = true; // Make sure the initialization of first state is executed +/* // ENCODER QEI Encoder(D12,D13,NC,32); // CONNECT ENC2A TO D13, ENC2B TO D12 float vorig_omwentelingen = 0; @@ -24,6 +25,7 @@ float snelheid_tpm; float snelheid_degps; float snelheid_tps; +*/ // PINS DigitalOut motor1DirectionPin(D4); // Value 0: CCW; 1: CW @@ -32,12 +34,14 @@ PwmOut motor2MagnitudePin(D6); InterruptIn button1(D2); // CONNECT BUT1 TO D2 InterruptIn button2(D3); // CONNECT BUT2 TO D3 +AnalogIn potPin(A1); // DEFINITIONS -const float motorVelocity = 1; // unit: rad/s +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 +float motorValue = 0; //TICKERS Ticker encoder; @@ -47,15 +51,22 @@ void RotateMotor1CW() { motor1DirectionPin = 1; - motor1MagnitudePin = fabs(motorVelocity / motorGain); - //pc.printf("Rotating motor 1 CW \r\n"); + //motor1MagnitudePin = fabs(motorVelocity / motorGain); + motorValue = potPin; + motor1MagnitudePin = motorValue; + //pc.printf("Rotating motor 1 CW \r\n"); + pc.printf("%f \r\n", motorValue); } void RotateMotor1CCW() { motor1DirectionPin = 0; - motor1MagnitudePin = fabs(motorVelocity / motorGain); - //pc.printf("Rotating motor 1 CounterCW \r\n"); + //motor1MagnitudePin = fabs(motorVelocity / motorGain); + //pc.printf("Rotating motor 1 CounterCW \r\n"); + motorValue = potPin; + motor1MagnitudePin = motorValue; + //pc.printf("Rotating motor 1 CW \r\n"); + pc.printf("%f \r\n", motorValue); } void TurnMotor1Off() @@ -81,7 +92,7 @@ break; } } - +/* void EncoderCalc() { counts = Encoder.getPulses()/motorRatio; @@ -94,6 +105,7 @@ vorig_degrees = degrees; //pc.printf("%.1f pulsen, %.2f graden, %.2f omwentelingen, %.2f tpm, %.2f tps, %.2f deg/s\r\n",counts, degrees, omwentelingen, snelheid_tpm, snelheid_tps, snelheid_degps); } +*/ // States function void ProcessStateMachine() @@ -178,8 +190,8 @@ pc.printf("START \r\n"); - encoder.attach(EncoderCalc, 0.5); - checkMotorState.attach(CheckMotor1, 0.01); + //encoder.attach(EncoderCalc, 0.5); + checkMotorState.attach(CheckMotor1, 0.0002); while(true) {