Project BioRobotics Group 19
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 5:0d3e8694726e
- Parent:
- 4:ea7689bf97e1
- Child:
- 7:757e95b4dc46
diff -r ea7689bf97e1 -r 0d3e8694726e main.cpp --- a/main.cpp Wed Oct 11 10:33:11 2017 +0000 +++ b/main.cpp Fri Oct 20 11:04:26 2017 +0000 @@ -1,29 +1,24 @@ #include "BiQuad.h" #include "FastPWM.h" #include "HIDScope.h" -#include "MODSERIAL.h" +#include <math.h> #include "mbed.h" +#include "MODSERIAL.h" #include "QEI.h" +const double pi = 3.1415926535897; // Definition of pi + // SERIAL COMMUNICATION WITH PC MODSERIAL pc(USBTX, USBRX); // STATES enum states{MOTORS_OFF, MOVING, HITTING}; - 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; -float degrees; -float vorig_degrees = 0; -float omwentelingen; -float counts; -float snelheid_tpm; -float snelheid_degps; -float snelheid_tps; +QEI Encoder1(D10,D11,NC,32); // CONNECT ENC1A TO D10, ENC1B TO D11 +QEI Encoder2(D12,D13,NC,32); // CONNECT ENC2A TO D12, ENC2B TO D13 // PINS DigitalOut motor1DirectionPin(D4); // Value 0: CCW; 1: CW @@ -33,69 +28,103 @@ InterruptIn button1(D2); // CONNECT BUT1 TO D2 InterruptIn button2(D3); // CONNECT BUT2 TO D3 -// DEFINITIONS -const float motorVelocity = 1; // 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 +// MOTOR CONTROL +Ticker controllerTicker; +const double controller_Ts = 1/5000; // Time step for controllerTicker; Should be between 5kHz and 10kHz [s] +const double motorRatio = 131.25; // Ratio of the gearbox in the motors [] +const double radPerPulse = 2*pi/(32*motorRatio); +// Motor 1 +// Controller gains +const double motor1_KP = 2.5; // Position gain [] +const double motor1_KI = 1.0; // Integral gain [] +const double motor1_KD = 0.5; // Derivative gain [] +double motor1_err_int = 0, motor1_prev_err = 0; +// Derivative filter coefficients +const double motor1_f_a1 = 1.0, motor1_f_a2 = 2.0; +const double motor1_f_b0 = 1.0, motor1_f_b1 = 3.0, motor1_f_b2 = 4.0; +// Filter variables +double motor1_f_v1 = 0, motor1_f_v2 = 0; +double reference1 = 0.0; // Wanted rotation of motor 1 [rad] +// Motor 2 +// Controller gains +// Derivative filter coefficients +// Filter variables -//TICKERS -Ticker encoder; -Ticker checkMotorState; // FUNCTIONS -void RotateMotor1CW() +// BIQUAD FILTER FOR DERIVATIVE OF ERROR +double biquad(double u, double &v1, double &v2, const double a1, + const double a2, const double b0, const double b1, const double b2) { - motor1DirectionPin = 1; - motor1MagnitudePin = fabs(motorVelocity / motorGain); - //pc.printf("Rotating motor 1 CW \r\n"); + double v = u - a1*v1 - a2*v2; + double y = b0*v + b1*v1 + b2*v2; + v2 = v1; v1 = v; + return y; } -void RotateMotor1CCW() +// P-CONTROLLER +double P_Controller(double error, const double Kp) { - motor1DirectionPin = 0; - motor1MagnitudePin = fabs(motorVelocity / motorGain); - //pc.printf("Rotating motor 1 CounterCW \r\n"); -} - -void TurnMotor1Off() -{ - motor1MagnitudePin = 0; - //pc.printf("Motors off \r\n"); + return Kp * error; } -void CheckMotor1() // Checks the state of motor1 and rotates motor1 depending on the state +// PID-CONTROLLER WITH FILTER +double PID_Controller(double e, const double Kp, const double Ki, + const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, + double &f_v2, const double f_a1, const double f_a2, const double f_b0, + const double f_b1, const double f_b2) { - switch(motor1State) - { - case 0: // Turn motors off - {TurnMotor1Off(); break;} - - case 1: // Rotate motor 1 CW - {RotateMotor1CW(); break;} - - case 2: // Rotate motor 2 CCW - {RotateMotor1CCW(); break;} - - default: - break; - } + // Derivative + double e_der = (e - e_prev)/Ts; + e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2); + e_prev = e; + // Integral + e_int = e_int + Ts*e; + // PID + return Kp*e + Ki*e_int + Kd*e_der; } -void EncoderCalc() +// MOTOR 1 +void RotateMotor1(double motor1Value) { - counts = Encoder.getPulses()/motorRatio; - degrees = counts/32*360; - omwentelingen = counts/32; - snelheid_tpm = (omwentelingen-vorig_omwentelingen)/0.5*60; - snelheid_tps = (omwentelingen-vorig_omwentelingen)/0.5; - snelheid_degps = (degrees-vorig_degrees)/0.5; - vorig_omwentelingen = omwentelingen; - 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); + if(currentState == MOVING) // Checks if state is MOVING + { + if(motor1Value >= 0) motor1DirectionPin = 1; // Rotate motor 1 CW + else motor1DirectionPin = 0; // Rotate motor 1 CCW + + if(fabs(motor1Value) > 1) motor1MagnitudePin = 1; + else motor1MagnitudePin = fabs(motor1Value); + } + else motor1MagnitudePin = 0; } -// States function +// MOTOR 1 P-CONTROLLER +void Motor1PController() +{ + double position1 = radPerPulse * Encoder1.getPulses(); // Calculates the rotation of motor 1 + double motor1Value = P_Controller(reference1 - position1, motor1_KP); + RotateMotor1(motor1Value); +} + +// MOTOR 1 PID-CONTROLLER +void Motor1Controller() +{ + double position1 = radPerPulse * Encoder1.getPulses(); + double motor1Value = PID_Controller(reference1 - position1, motor1_KP, + motor1_KI, motor1_KD, controller_Ts, motor1_err_int, motor1_prev_err, + motor1_f_v1, motor1_f_v2, motor1_f_a1, motor1_f_a2, motor1_f_b0, + motor1_f_b1, motor1_f_b2); + RotateMotor1(motor1Value); +} + +// TURN OFF MOTORS +void TurnMotorsOff() +{ + motor1MagnitudePin = 0; + motor2MagnitudePin = 0; +} + +// STATES void ProcessStateMachine() { switch(currentState) @@ -106,7 +135,7 @@ if(stateChanged) { pc.printf("Entering MOTORS_OFF \r\n"); - TurnMotor1Off(); // Turn motors off + TurnMotorsOff(); // Turn motors off stateChanged = false; } @@ -128,22 +157,15 @@ pc.printf("Entering MOVING \r\n"); stateChanged = false; } - - // Move commands - if(!button1){motor1State = 1;} - else if(!button2){motor1State = 2;} - else{motor1State = 0;} - - /* // Hit command - if(// HIT COMMAND) + if(!button1) { currentState = HITTING; stateChanged = true; break; } - */ + break; } @@ -153,8 +175,8 @@ if(stateChanged) { pc.printf("Entering HITTING \r\n"); + stateChanged = false; //HitBall(); // Hit the ball - stateChanged = false; currentState = MOVING; stateChanged = true; break; @@ -164,13 +186,13 @@ default: { - TurnMotor1Off(); // Turn motors off for safety + TurnMotorsOff(); // Turn motors off for safety break; } } } -// Main function +// MAIN FUNCTION int main() { // Serial communication @@ -178,8 +200,8 @@ pc.printf("START \r\n"); - encoder.attach(EncoderCalc, 0.5); - checkMotorState.attach(CheckMotor1, 0.01); + controllerTicker.attach(&Motor1PController, controller_Ts); // Ticker to control motor 1 + //controllerTicker.attach(&Motor1Controller, controller_Ts); while(true) {