Diff: main.cpp

Revision:

5:0d3e8694726e

Parent:

4:ea7689bf97e1

Child:

7:757e95b4dc46

--- 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)
     {