Diff: main.cpp

Revision:

7:757e95b4dc46

Parent:

5:0d3e8694726e

Child:

8:78d8ccf84a38

--- a/main.cpp	Fri Oct 20 11:04:26 2017 +0000
+++ b/main.cpp	Mon Oct 23 09:42:21 2017 +0000
@@ -8,51 +8,68 @@
 
 const double pi = 3.1415926535897;                                              // Definition of pi
 
-// SERIAL COMMUNICATION WITH PC
+// SERIAL COMMUNICATION WITH PC ////////////////////////////////////////////////
 MODSERIAL pc(USBTX, USBRX);
 
-// STATES
+// 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
+// ENCODER /////////////////////////////////////////////////////////////////////
 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
+// PINS ////////////////////////////////////////////////////////////////////////
 DigitalOut motor1DirectionPin(D4);                                              // Value 0: CCW; 1: CW
 PwmOut motor1MagnitudePin(D5);
 DigitalOut motor2DirectionPin(D7);                                              // Value 0: CW or CCW?; 1: CW or CCW?
 PwmOut motor2MagnitudePin(D6);
 InterruptIn button1(D2);                                                        // CONNECT BUT1 TO D2
 InterruptIn button2(D3);                                                        // CONNECT BUT2 TO D3
+AnalogIn emg(A0);
 
-// MOTOR CONTROL
+// 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
+const double radPerPulse = 2*pi/(32*motorRatio);                                // Amount of radians the motor rotates per encoder pulse [rad/pulse]
+
+// MOTOR 1
+double reference1 = 0.0;                                                        // Desired rotation of motor 1 [rad]
 // 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;
+const double motor1_f_a1 = 1.0, motor1_f_a2 = 2.0;                              // Derivative filter coefficients []
+const double motor1_f_b0 = 1.0, motor1_f_b1 = 3.0, motor1_f_b2 = 4.0;           // Derivative filter coefficients []
 // Filter variables
 double motor1_f_v1 = 0, motor1_f_v2 = 0;
-double reference1 = 0.0;                                                        // Wanted rotation of motor 1 [rad]
-// Motor 2
+
+// MOTOR 2
 // Controller gains
 // Derivative filter coefficients
 // Filter variables
 
 
-// FUNCTIONS
-// BIQUAD FILTER FOR DERIVATIVE OF ERROR
+// EMG FILTER //////////////////////////////////////////////////////////////////
+BiQuadChain bqc;
+BiQuad bq1(0, 0, 0, 0, 0);                                                      // EMG filter coefficients []
+BiQuad bq2(0, 0, 0, 0, 0);                                                      // EMG filter coefficients []
+Ticker emgSampleTicker;
+double emg_Ts = 0.01;                                                           // Time step for EMG sampling
+
+
+// FUNCTIONS ///////////////////////////////////////////////////////////////////
+// EMG /////////////////////////////////////////////////////////////////////////
+void EmgSample()
+{
+    double emgFiltered = bqc.step(emg.read());                                  // Filters the EMG signal
+}
+
+// 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)
 {
@@ -62,29 +79,29 @@
     return y;
 }
 
-// P-CONTROLLER
+// P-CONTROLLER ////////////////////////////////////////////////////////////////
 double P_Controller(double error, const double Kp)
 {
     return Kp * error;
 }
 
-// PID-CONTROLLER WITH FILTER
+// 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)
 {
     // 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);
+    double e_der = (e - e_prev)/Ts;                                             // Calculates the derivative of error
+    e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);            // Filter the derivative of error
     e_prev = e;
     // Integral
-    e_int = e_int + Ts*e;
+    e_int = e_int + Ts*e;                                                       // Calculates the integral of error
     // PID
     return Kp*e + Ki*e_int + Kd*e_der;
 }
 
-// MOTOR 1
+// MOTOR 1 /////////////////////////////////////////////////////////////////////
 void RotateMotor1(double motor1Value)
 {
     if(currentState == MOVING)                                                  // Checks if state is MOVING
@@ -98,7 +115,7 @@
     else motor1MagnitudePin = 0;
 }
 
-// MOTOR 1 P-CONTROLLER
+// MOTOR 1 P-CONTROLLER ////////////////////////////////////////////////////////
 void Motor1PController()
 {
     double position1 = radPerPulse * Encoder1.getPulses();                      // Calculates the rotation of motor 1
@@ -106,7 +123,7 @@
     RotateMotor1(motor1Value);
 }
 
-// MOTOR 1 PID-CONTROLLER
+// MOTOR 1 PID-CONTROLLER //////////////////////////////////////////////////////
 void Motor1Controller()
 {
     double position1 = radPerPulse * Encoder1.getPulses();
@@ -117,14 +134,14 @@
     RotateMotor1(motor1Value);
 }
 
-// TURN OFF MOTORS
+// TURN OFF MOTORS /////////////////////////////////////////////////////////////
 void TurnMotorsOff()
 {
     motor1MagnitudePin = 0;
     motor2MagnitudePin = 0;
 }
 
-// STATES
+// STATES //////////////////////////////////////////////////////////////////////
 void ProcessStateMachine()
 {
     switch(currentState)
@@ -134,7 +151,7 @@
             // State initialization
             if(stateChanged)
             {
-                pc.printf("Entering MOTORS_OFF \r\n");
+                pc.printf("Entering MOTORS_OFF \r\n Press button 1 to enter MOVING \r\n");
                 TurnMotorsOff();                                                // Turn motors off
                 stateChanged = false;
             }
@@ -154,12 +171,12 @@
             // State initialization
             if(stateChanged)
             {
-                pc.printf("Entering MOVING \r\n");
+                pc.printf("Entering MOVING \r\n Press button 2 to enter HITTING \r\n");
                 stateChanged = false;
             }
             
             // Hit command    
-            if(!button1)
+            if(!button2)
             {
                 currentState = HITTING;
                 stateChanged = true;
@@ -192,7 +209,7 @@
     }
 }
 
-// MAIN FUNCTION
+// MAIN FUNCTION ///////////////////////////////////////////////////////////////
 int main()
 {
     // Serial communication
@@ -200,8 +217,11 @@
     
     pc.printf("START \r\n");
     
+    bqc.add(&bq1).add(&bq2);
+    
     controllerTicker.attach(&Motor1PController, controller_Ts);                 // Ticker to control motor 1
     //controllerTicker.attach(&Motor1Controller, controller_Ts);
+    emgSampleTicker.attach(&EmgSample, emg_Ts);
     
     while(true)
     {