Henk van Gansen / Mbed 2 deprecated Project_BioRobotics

Vanmorgen aan gewerkt

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

Fork of Project_BioRobotics by Jordi Luong

main.cpp@11:5c06c97c3673, 2017-10-30 (annotated)

Committer:
jordiluong
Date:
Mon Oct 30 12:11:42 2017 +0000
Revision:
11:5c06c97c3673
Parent:
10:a9e344e440b8
Child:
12:12b72bd60fd1
Minor update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jordiluong 0:80ac024b84cb 1 #include "BiQuad.h"
jordiluong 0:80ac024b84cb 2 #include "FastPWM.h"
jordiluong 0:80ac024b84cb 3 #include "HIDScope.h"
jordiluong 5:0d3e8694726e 4 #include <math.h>
jordiluong 0:80ac024b84cb 5 #include "mbed.h"
jordiluong 5:0d3e8694726e 6 #include "MODSERIAL.h"
jordiluong 0:80ac024b84cb 7 #include "QEI.h"
jordiluong 0:80ac024b84cb 8
jordiluong 5:0d3e8694726e 9 const double pi = 3.1415926535897; // Definition of pi
jordiluong 5:0d3e8694726e 10
jordiluong 7:757e95b4dc46 11 // SERIAL COMMUNICATION WITH PC ////////////////////////////////////////////////
jordiluong 0:80ac024b84cb 12 MODSERIAL pc(USBTX, USBRX);
jordiluong 0:80ac024b84cb 13
jordiluong 7:757e95b4dc46 14 // STATES //////////////////////////////////////////////////////////////////////
jordiluong 3:5c3edcd29448 15 enum states{MOTORS_OFF, MOVING, HITTING};
jordiluong 0:80ac024b84cb 16 states currentState = MOTORS_OFF; // Start with motors off
jordiluong 0:80ac024b84cb 17 bool stateChanged = true; // Make sure the initialization of first state is executed
jordiluong 0:80ac024b84cb 18
jordiluong 7:757e95b4dc46 19 // ENCODER /////////////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 20 QEI Encoder1(D10,D11,NC,32); // CONNECT ENC1A TO D10, ENC1B TO D11
jordiluong 5:0d3e8694726e 21 QEI Encoder2(D12,D13,NC,32); // CONNECT ENC2A TO D12, ENC2B TO D13
jordiluong 4:ea7689bf97e1 22
jordiluong 7:757e95b4dc46 23 // PINS ////////////////////////////////////////////////////////////////////////
jordiluong 4:ea7689bf97e1 24 DigitalOut motor1DirectionPin(D4); // Value 0: CCW; 1: CW
jordiluong 3:5c3edcd29448 25 PwmOut motor1MagnitudePin(D5);
jordiluong 3:5c3edcd29448 26 DigitalOut motor2DirectionPin(D7); // Value 0: CW or CCW?; 1: CW or CCW?
jordiluong 3:5c3edcd29448 27 PwmOut motor2MagnitudePin(D6);
jordiluong 4:ea7689bf97e1 28 InterruptIn button1(D2); // CONNECT BUT1 TO D2
jordiluong 4:ea7689bf97e1 29 InterruptIn button2(D3); // CONNECT BUT2 TO D3
jordiluong 10:a9e344e440b8 30 InterruptIn button3(SW2);
jordiluong 10:a9e344e440b8 31 InterruptIn button4(SW3);
jordiluong 7:757e95b4dc46 32 AnalogIn emg(A0);
jordiluong 3:5c3edcd29448 33
jordiluong 7:757e95b4dc46 34 // MOTOR CONTROL ///////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 35 Ticker controllerTicker;
jordiluong 10:a9e344e440b8 36 const double controller_Ts = 0.01; // Time step for controllerTicker [s]; Should be between 5kHz and 10kHz
jordiluong 5:0d3e8694726e 37 const double motorRatio = 131.25; // Ratio of the gearbox in the motors []
jordiluong 7:757e95b4dc46 38 const double radPerPulse = 2*pi/(32*motorRatio); // Amount of radians the motor rotates per encoder pulse [rad/pulse]
jordiluong 11:5c06c97c3673 39 double xVelocity = 0, yVelocity = 0; // X and Y velocities of the end effector at the start
jordiluong 11:5c06c97c3673 40 double velocity = 0.3; // X and Y velocity of the end effector when desired
jordiluong 7:757e95b4dc46 41
jordiluong 7:757e95b4dc46 42 // MOTOR 1
jordiluong 10:a9e344e440b8 43 double position1; // Position of motor 1 [rad]
jordiluong 11:5c06c97c3673 44 double reference1 = 0; // Desired rotation of motor 1 [rad]
jordiluong 10:a9e344e440b8 45 double motor1Max = 2*pi*45/360; // Maximum rotation of motor 1 [rad]
jordiluong 10:a9e344e440b8 46 double motor1Min = 0; // Minimum rotation of motor 1 [rad]
jordiluong 5:0d3e8694726e 47 // Controller gains
jordiluong 10:a9e344e440b8 48 const double motor1_KP = 5; // Position gain []
jordiluong 11:5c06c97c3673 49 const double motor1_KI = 5; // Integral gain []
jordiluong 11:5c06c97c3673 50 const double motor1_KD = 0.0; // Derivative gain []
jordiluong 5:0d3e8694726e 51 double motor1_err_int = 0, motor1_prev_err = 0;
jordiluong 5:0d3e8694726e 52 // Derivative filter coefficients
jordiluong 7:757e95b4dc46 53 const double motor1_f_a1 = 1.0, motor1_f_a2 = 2.0; // Derivative filter coefficients []
jordiluong 7:757e95b4dc46 54 const double motor1_f_b0 = 1.0, motor1_f_b1 = 3.0, motor1_f_b2 = 4.0; // Derivative filter coefficients []
jordiluong 5:0d3e8694726e 55 // Filter variables
jordiluong 5:0d3e8694726e 56 double motor1_f_v1 = 0, motor1_f_v2 = 0;
jordiluong 7:757e95b4dc46 57
jordiluong 7:757e95b4dc46 58 // MOTOR 2
jordiluong 10:a9e344e440b8 59 double position2; // Position of motor 2 [rad]
jordiluong 10:a9e344e440b8 60 volatile double reference2 = 0; // Desired rotation of motor 2 [rad]
jordiluong 10:a9e344e440b8 61 double motor2Max = 2*pi*45/360; // Maximum rotation of motor 2 [rad]
jordiluong 10:a9e344e440b8 62 double motor2Min = 2*pi*-45/360; // Minimum rotation of motor 2 [rad]
jordiluong 5:0d3e8694726e 63 // Controller gains
jordiluong 10:a9e344e440b8 64 const double motor2_KP = 3; // Position gain []
jordiluong 11:5c06c97c3673 65 const double motor2_KI = 3; // Integral gain []
jordiluong 11:5c06c97c3673 66 const double motor2_KD = 0.0; // Derivative gain []
jordiluong 10:a9e344e440b8 67 double motor2_err_int = 0, motor2_prev_err = 0;
jordiluong 5:0d3e8694726e 68 // Derivative filter coefficients
jordiluong 10:a9e344e440b8 69 const double motor2_f_a1 = 1.0, motor2_f_a2 = 2.0; // Derivative filter coefficients []
jordiluong 10:a9e344e440b8 70 const double motor2_f_b0 = 1.0, motor2_f_b1 = 3.0, motor2_f_b2 = 4.0; // Derivative filter coefficients []
jordiluong 5:0d3e8694726e 71 // Filter variables
jordiluong 10:a9e344e440b8 72 double motor2_f_v1 = 0, motor2_f_v2 = 0;
jordiluong 0:80ac024b84cb 73
jordiluong 7:757e95b4dc46 74 // EMG FILTER //////////////////////////////////////////////////////////////////
jordiluong 7:757e95b4dc46 75 BiQuadChain bqc;
jordiluong 7:757e95b4dc46 76 BiQuad bq1(0, 0, 0, 0, 0); // EMG filter coefficients []
jordiluong 7:757e95b4dc46 77 BiQuad bq2(0, 0, 0, 0, 0); // EMG filter coefficients []
jordiluong 7:757e95b4dc46 78 Ticker emgSampleTicker;
jordiluong 10:a9e344e440b8 79 double emg_Ts = 0.1; // Time step for EMG sampling [s]
jordiluong 7:757e95b4dc46 80
jordiluong 7:757e95b4dc46 81
jordiluong 7:757e95b4dc46 82 // FUNCTIONS ///////////////////////////////////////////////////////////////////
jordiluong 7:757e95b4dc46 83 // BIQUAD FILTER FOR DERIVATIVE OF ERROR ///////////////////////////////////////
jordiluong 5:0d3e8694726e 84 double biquad(double u, double &v1, double &v2, const double a1,
jordiluong 5:0d3e8694726e 85 const double a2, const double b0, const double b1, const double b2)
jordiluong 0:80ac024b84cb 86 {
jordiluong 5:0d3e8694726e 87 double v = u - a1*v1 - a2*v2;
jordiluong 5:0d3e8694726e 88 double y = b0*v + b1*v1 + b2*v2;
jordiluong 5:0d3e8694726e 89 v2 = v1; v1 = v;
jordiluong 5:0d3e8694726e 90 return y;
jordiluong 0:80ac024b84cb 91 }
jordiluong 0:80ac024b84cb 92
jordiluong 7:757e95b4dc46 93 // PID-CONTROLLER WITH FILTER //////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 94 double PID_Controller(double e, const double Kp, const double Ki,
jordiluong 5:0d3e8694726e 95 const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1,
jordiluong 5:0d3e8694726e 96 double &f_v2, const double f_a1, const double f_a2, const double f_b0,
jordiluong 5:0d3e8694726e 97 const double f_b1, const double f_b2)
jordiluong 0:80ac024b84cb 98 {
jordiluong 5:0d3e8694726e 99 // Derivative
jordiluong 8:78d8ccf84a38 100 double e_der = (e - e_prev)/Ts; // Calculate the derivative of error
jordiluong 11:5c06c97c3673 101 //pc.printf("derivative error %f \r\n", e_der);
jordiluong 10:a9e344e440b8 102 //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2); // Filter the derivative of error
jordiluong 5:0d3e8694726e 103 e_prev = e;
jordiluong 5:0d3e8694726e 104 // Integral
jordiluong 8:78d8ccf84a38 105 e_int = e_int + Ts*e; // Calculate the integral of error
jordiluong 5:0d3e8694726e 106 // PID
jordiluong 11:5c06c97c3673 107 //pc.printf("Na derivative error %f \r\n", e_der);
jordiluong 11:5c06c97c3673 108 return Kp*e + Ki*e_int + Kd*e_der;
jordiluong 3:5c3edcd29448 109 }
jordiluong 3:5c3edcd29448 110
jordiluong 7:757e95b4dc46 111 // MOTOR 1 /////////////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 112 void RotateMotor1(double motor1Value)
jordiluong 3:5c3edcd29448 113 {
jordiluong 10:a9e344e440b8 114 if(currentState == MOVING || currentState == HITTING) // Check if state is MOVING
jordiluong 5:0d3e8694726e 115 {
jordiluong 10:a9e344e440b8 116 if(motor1Value >= 0) motor1DirectionPin = 0; // Rotate motor 1 CW
jordiluong 10:a9e344e440b8 117 else motor1DirectionPin = 1; // Rotate motor 1 CCW
jordiluong 5:0d3e8694726e 118
jordiluong 5:0d3e8694726e 119 if(fabs(motor1Value) > 1) motor1MagnitudePin = 1;
jordiluong 5:0d3e8694726e 120 else motor1MagnitudePin = fabs(motor1Value);
jordiluong 5:0d3e8694726e 121 }
jordiluong 5:0d3e8694726e 122 else motor1MagnitudePin = 0;
jordiluong 3:5c3edcd29448 123 }
jordiluong 0:80ac024b84cb 124
jordiluong 10:a9e344e440b8 125 // MOTOR 2 /////////////////////////////////////////////////////////////////////
jordiluong 10:a9e344e440b8 126 void RotateMotor2(double motor2Value)
jordiluong 10:a9e344e440b8 127 {
jordiluong 10:a9e344e440b8 128 if(currentState == MOVING || currentState == HITTING) // Check if state is MOVING
jordiluong 10:a9e344e440b8 129 {
jordiluong 10:a9e344e440b8 130 if(motor2Value >= 0) motor2DirectionPin = 1; // Rotate motor 1 CW
jordiluong 10:a9e344e440b8 131 else motor2DirectionPin = 0; // Rotate motor 1 CCW
jordiluong 10:a9e344e440b8 132
jordiluong 10:a9e344e440b8 133 if(fabs(motor2Value) > 1) motor2MagnitudePin = 1;
jordiluong 10:a9e344e440b8 134 else motor2MagnitudePin = fabs(motor2Value);
jordiluong 10:a9e344e440b8 135 }
jordiluong 10:a9e344e440b8 136 else motor2MagnitudePin = 0;
jordiluong 10:a9e344e440b8 137 }
jordiluong 10:a9e344e440b8 138
jordiluong 7:757e95b4dc46 139 // MOTOR 1 PID-CONTROLLER //////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 140 void Motor1Controller()
jordiluong 5:0d3e8694726e 141 {
jordiluong 10:a9e344e440b8 142 position1 = radPerPulse * Encoder1.getPulses();
jordiluong 10:a9e344e440b8 143 pc.printf("error %f \r\n", reference1 - position1);
jordiluong 5:0d3e8694726e 144 double motor1Value = PID_Controller(reference1 - position1, motor1_KP,
jordiluong 5:0d3e8694726e 145 motor1_KI, motor1_KD, controller_Ts, motor1_err_int, motor1_prev_err,
jordiluong 5:0d3e8694726e 146 motor1_f_v1, motor1_f_v2, motor1_f_a1, motor1_f_a2, motor1_f_b0,
jordiluong 5:0d3e8694726e 147 motor1_f_b1, motor1_f_b2);
jordiluong 10:a9e344e440b8 148 //pc.printf("motor value %f \r\n",motor1Value);
jordiluong 5:0d3e8694726e 149 RotateMotor1(motor1Value);
jordiluong 5:0d3e8694726e 150 }
jordiluong 5:0d3e8694726e 151
jordiluong 10:a9e344e440b8 152 // MOTOR 2 PID-CONTROLLER //////////////////////////////////////////////////////
jordiluong 10:a9e344e440b8 153 void Motor2Controller()
jordiluong 10:a9e344e440b8 154 {
jordiluong 10:a9e344e440b8 155 position2 = radPerPulse * Encoder2.getPulses();
jordiluong 10:a9e344e440b8 156 //pc.printf("%f", position2);
jordiluong 10:a9e344e440b8 157 double motor2Value = PID_Controller(reference2 - position2, motor2_KP,
jordiluong 10:a9e344e440b8 158 motor2_KI, motor2_KD, controller_Ts, motor2_err_int, motor2_prev_err,
jordiluong 10:a9e344e440b8 159 motor2_f_v1, motor2_f_v2, motor2_f_a1, motor2_f_a2, motor2_f_b0,
jordiluong 10:a9e344e440b8 160 motor2_f_b1, motor2_f_b2);
jordiluong 10:a9e344e440b8 161 pc.printf("motor value %f \r\n",motor2Value);
jordiluong 10:a9e344e440b8 162 RotateMotor2(motor2Value);
jordiluong 10:a9e344e440b8 163 }
jordiluong 10:a9e344e440b8 164
jordiluong 7:757e95b4dc46 165 // TURN OFF MOTORS /////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 166 void TurnMotorsOff()
jordiluong 5:0d3e8694726e 167 {
jordiluong 5:0d3e8694726e 168 motor1MagnitudePin = 0;
jordiluong 5:0d3e8694726e 169 motor2MagnitudePin = 0;
jordiluong 5:0d3e8694726e 170 }
jordiluong 5:0d3e8694726e 171
jordiluong 10:a9e344e440b8 172 // DETERMINE JOINT VELOCITIES //////////////////////////////////////////////////
jordiluong 10:a9e344e440b8 173 void jointVelocities()
jordiluong 10:a9e344e440b8 174 {
jordiluong 10:a9e344e440b8 175 position1 = radPerPulse * Encoder1.getPulses();
jordiluong 10:a9e344e440b8 176 position2 = radPerPulse * Encoder2.getPulses();
jordiluong 10:a9e344e440b8 177
jordiluong 11:5c06c97c3673 178 if(!button1) xVelocity = velocity;//&& position1 > motor1Min && position2 > motor2Min
jordiluong 11:5c06c97c3673 179 else if(!button2) xVelocity = -velocity;//&& position1 < motor1Max && position2 < motor2Max
jordiluong 10:a9e344e440b8 180 else xVelocity = 0;
jordiluong 10:a9e344e440b8 181
jordiluong 11:5c06c97c3673 182 if(!button3) yVelocity = velocity;//&& position1 < motor1Max && position2 < motor2Max
jordiluong 11:5c06c97c3673 183 else if(!button4) yVelocity = -velocity;//&& position1 < motor1Max && position2 > motor2Min
jordiluong 10:a9e344e440b8 184 else yVelocity = 0;
jordiluong 10:a9e344e440b8 185
jordiluong 10:a9e344e440b8 186 //pc.printf("x %f, y %f \r\n", xVelocity, yVelocity);
jordiluong 10:a9e344e440b8 187
jordiluong 10:a9e344e440b8 188 // Construct Jacobian
jordiluong 10:a9e344e440b8 189 double q[4];
jordiluong 10:a9e344e440b8 190 q[0] = position1, q[1] = -position1;
jordiluong 10:a9e344e440b8 191 q[2] = position2, q[3] = -position2;
jordiluong 10:a9e344e440b8 192
jordiluong 10:a9e344e440b8 193 double T2[3]; // Second column of the jacobian
jordiluong 10:a9e344e440b8 194 double T3[3]; // Third column of the jacobian
jordiluong 10:a9e344e440b8 195 double T4[3]; // Fourth column of the jacobian
jordiluong 10:a9e344e440b8 196 double T1[6];
jordiluong 10:a9e344e440b8 197 static const signed char b_T1[3] = { 1, 0, 0 };
jordiluong 10:a9e344e440b8 198 double J_data[6];
jordiluong 10:a9e344e440b8 199
jordiluong 10:a9e344e440b8 200 T2[0] = 1.0;
jordiluong 10:a9e344e440b8 201 T2[1] = 0.365 * cos(q[0]);
jordiluong 10:a9e344e440b8 202 T2[2] = 0.365 * sin(q[0]);
jordiluong 10:a9e344e440b8 203 T3[0] = 1.0;
jordiluong 10:a9e344e440b8 204 T3[1] = 0.365 * cos(q[0]) + 0.2353720459187964 * sin((0.21406068356382149 +
jordiluong 10:a9e344e440b8 205 q[0]) + q[1]);
jordiluong 10:a9e344e440b8 206 T3[2] = 0.365 * sin(q[0]) - 0.2353720459187964 * cos((0.21406068356382149 +
jordiluong 10:a9e344e440b8 207 q[0]) + q[1]);
jordiluong 10:a9e344e440b8 208 T4[0] = 1.0;
jordiluong 10:a9e344e440b8 209 T4[1] = (0.365 * cos(q[0]) + 0.2353720459187964 * sin((0.21406068356382149 +
jordiluong 10:a9e344e440b8 210 q[0]) + q[1])) + 0.265 * sin((q[0] + q[1]) + q[2]);
jordiluong 10:a9e344e440b8 211 T4[2] = (0.365 * sin(q[0]) - 0.2353720459187964 * cos((0.21406068356382149 +
jordiluong 10:a9e344e440b8 212 q[0]) + q[1])) - 0.265 * cos((q[0] + q[1]) + q[2]);
jordiluong 10:a9e344e440b8 213
jordiluong 10:a9e344e440b8 214 for (int i = 0; i < 3; i++)
jordiluong 10:a9e344e440b8 215 {
jordiluong 10:a9e344e440b8 216 T1[i] = (double)b_T1[i] - T2[i];
jordiluong 10:a9e344e440b8 217 T1[3 + i] = T3[i] - T4[i];
jordiluong 10:a9e344e440b8 218 }
jordiluong 10:a9e344e440b8 219
jordiluong 10:a9e344e440b8 220 for (int i = 0; i < 2; i++)
jordiluong 10:a9e344e440b8 221 {
jordiluong 10:a9e344e440b8 222 for (int j = 0; j < 3; j++)
jordiluong 10:a9e344e440b8 223 {
jordiluong 10:a9e344e440b8 224 J_data[j + 3 * i] = T1[j + 3 * i];
jordiluong 10:a9e344e440b8 225 }
jordiluong 10:a9e344e440b8 226 }
jordiluong 10:a9e344e440b8 227
jordiluong 10:a9e344e440b8 228 // Here the first row of the Jacobian is cut off, so we do not take rotation into consideration
jordiluong 10:a9e344e440b8 229 // Note: the matrices from now on will we constructed rowwise
jordiluong 10:a9e344e440b8 230 double Jvelocity[4];
jordiluong 10:a9e344e440b8 231 Jvelocity[0] = J_data[1];
jordiluong 10:a9e344e440b8 232 Jvelocity[1] = J_data[4];
jordiluong 10:a9e344e440b8 233 Jvelocity[2] = J_data[2];
jordiluong 10:a9e344e440b8 234 Jvelocity[3] = J_data[5];
jordiluong 10:a9e344e440b8 235
jordiluong 10:a9e344e440b8 236 // Creating the inverse Jacobian
jordiluong 10:a9e344e440b8 237 double Jvelocity_inv[4]; // The inverse matrix of the jacobian
jordiluong 10:a9e344e440b8 238 double determ = Jvelocity[0]*Jvelocity[3]-Jvelocity[1]*Jvelocity[2]; // The determinant of the matrix
jordiluong 10:a9e344e440b8 239 Jvelocity_inv[0] = Jvelocity[3]/determ;
jordiluong 10:a9e344e440b8 240 Jvelocity_inv[1] = -Jvelocity[1]/determ;
jordiluong 10:a9e344e440b8 241 Jvelocity_inv[2] = -Jvelocity[2]/determ;
jordiluong 10:a9e344e440b8 242 Jvelocity_inv[3] = Jvelocity[0]/determ;
jordiluong 10:a9e344e440b8 243
jordiluong 10:a9e344e440b8 244 // Now the velocity of the joints are found by giving the velocity of the end-effector and the inverse jacobian
jordiluong 10:a9e344e440b8 245 double msh[2]; // This is the velocity the joints have to have
jordiluong 10:a9e344e440b8 246 msh[0] = xVelocity*Jvelocity_inv[0] + yVelocity*Jvelocity_inv[1];
jordiluong 10:a9e344e440b8 247 msh[1] = xVelocity*Jvelocity_inv[2] + yVelocity*Jvelocity_inv[3];
jordiluong 10:a9e344e440b8 248
jordiluong 10:a9e344e440b8 249 if(position1 + msh[0]*emg_Ts > motor1Max) reference1 = motor1Max;
jordiluong 10:a9e344e440b8 250 else if(position1 + msh[0]*emg_Ts < motor1Min) reference1 = motor1Min;
jordiluong 10:a9e344e440b8 251 else reference1 = position1 + msh[0]*emg_Ts;
jordiluong 10:a9e344e440b8 252
jordiluong 10:a9e344e440b8 253 if(position2 + msh[1]*emg_Ts > motor2Max) reference2 = motor2Max;
jordiluong 10:a9e344e440b8 254 else if(position2 + msh[1]*emg_Ts < motor2Min) reference2 = motor2Min;
jordiluong 10:a9e344e440b8 255 else reference2 = position2 + msh[1]*emg_Ts;
jordiluong 10:a9e344e440b8 256
jordiluong 10:a9e344e440b8 257 Motor1Controller(), Motor2Controller();
jordiluong 10:a9e344e440b8 258
jordiluong 10:a9e344e440b8 259 pc.printf("position 1 %f, 2 %f \r\n", position1/2/pi*360, position2/2/pi*360);
jordiluong 10:a9e344e440b8 260 pc.printf("reference 1 %f, 2 %f \r\n", reference1/2/pi*360, reference2/2/pi*360);
jordiluong 10:a9e344e440b8 261 pc.printf("msh*Ts 1 %f, 2 %f \r\n\n", msh[0]*emg_Ts, msh[1]*emg_Ts);
jordiluong 10:a9e344e440b8 262 }
jordiluong 10:a9e344e440b8 263
jordiluong 10:a9e344e440b8 264 // EMG /////////////////////////////////////////////////////////////////////////
jordiluong 10:a9e344e440b8 265 void EmgSample()
jordiluong 10:a9e344e440b8 266 {
jordiluong 10:a9e344e440b8 267 if(currentState == MOVING)
jordiluong 10:a9e344e440b8 268 {
jordiluong 10:a9e344e440b8 269 //double emgFiltered = bqc.step(emg.read()); // Filter the EMG signal
jordiluong 10:a9e344e440b8 270 /*
jordiluong 10:a9e344e440b8 271 If EMG signal 1 --> xVelocity / yVelocity = velocity
jordiluong 11:5c06c97c3673 272 Else if EMG signal 2 --> xVelocity / yVelocity = -velocity
jordiluong 10:a9e344e440b8 273 Else xVelocity / yVelocity = 0
jordiluong 11:5c06c97c3673 274 If both signal 1 and 2 --> Switch
jordiluong 10:a9e344e440b8 275 */
jordiluong 10:a9e344e440b8 276 jointVelocities();
jordiluong 10:a9e344e440b8 277 }
jordiluong 10:a9e344e440b8 278 }
jordiluong 10:a9e344e440b8 279
jordiluong 7:757e95b4dc46 280 // STATES //////////////////////////////////////////////////////////////////////
jordiluong 0:80ac024b84cb 281 void ProcessStateMachine()
jordiluong 0:80ac024b84cb 282 {
jordiluong 0:80ac024b84cb 283 switch(currentState)
jordiluong 0:80ac024b84cb 284 {
jordiluong 0:80ac024b84cb 285 case MOTORS_OFF:
jordiluong 0:80ac024b84cb 286 {
jordiluong 0:80ac024b84cb 287 // State initialization
jordiluong 0:80ac024b84cb 288 if(stateChanged)
jordiluong 0:80ac024b84cb 289 {
jordiluong 10:a9e344e440b8 290 pc.printf("Entering MOTORS_OFF \r\n"
jordiluong 10:a9e344e440b8 291 "Press button 1 to enter MOVING \r\n");
jordiluong 5:0d3e8694726e 292 TurnMotorsOff(); // Turn motors off
jordiluong 0:80ac024b84cb 293 stateChanged = false;
jordiluong 0:80ac024b84cb 294 }
jordiluong 0:80ac024b84cb 295
jordiluong 0:80ac024b84cb 296 // Home command
jordiluong 4:ea7689bf97e1 297 if(!button1)
jordiluong 0:80ac024b84cb 298 {
jordiluong 0:80ac024b84cb 299 currentState = MOVING;
jordiluong 0:80ac024b84cb 300 stateChanged = true;
jordiluong 0:80ac024b84cb 301 break;
jordiluong 0:80ac024b84cb 302 }
jordiluong 4:ea7689bf97e1 303 break;
jordiluong 0:80ac024b84cb 304 }
jordiluong 0:80ac024b84cb 305
jordiluong 0:80ac024b84cb 306 case MOVING:
jordiluong 0:80ac024b84cb 307 {
jordiluong 0:80ac024b84cb 308 // State initialization
jordiluong 0:80ac024b84cb 309 if(stateChanged)
jordiluong 0:80ac024b84cb 310 {
jordiluong 10:a9e344e440b8 311 pc.printf("Entering MOVING \r\n"
jordiluong 10:a9e344e440b8 312 "Press button 2 to enter HITTING \r\n");
jordiluong 0:80ac024b84cb 313 stateChanged = false;
jordiluong 0:80ac024b84cb 314 }
jordiluong 0:80ac024b84cb 315
jordiluong 0:80ac024b84cb 316 // Hit command
jordiluong 10:a9e344e440b8 317 /*if(!button2)
jordiluong 0:80ac024b84cb 318 {
jordiluong 0:80ac024b84cb 319 currentState = HITTING;
jordiluong 0:80ac024b84cb 320 stateChanged = true;
jordiluong 0:80ac024b84cb 321 break;
jordiluong 0:80ac024b84cb 322 }
jordiluong 10:a9e344e440b8 323 */
jordiluong 4:ea7689bf97e1 324 break;
jordiluong 0:80ac024b84cb 325 }
jordiluong 0:80ac024b84cb 326
jordiluong 0:80ac024b84cb 327 case HITTING:
jordiluong 0:80ac024b84cb 328 {
jordiluong 0:80ac024b84cb 329 // State initialization
jordiluong 0:80ac024b84cb 330 if(stateChanged)
jordiluong 0:80ac024b84cb 331 {
jordiluong 0:80ac024b84cb 332 pc.printf("Entering HITTING \r\n");
jordiluong 5:0d3e8694726e 333 stateChanged = false;
jordiluong 4:ea7689bf97e1 334 //HitBall(); // Hit the ball
jordiluong 0:80ac024b84cb 335 currentState = MOVING;
jordiluong 0:80ac024b84cb 336 stateChanged = true;
jordiluong 0:80ac024b84cb 337 break;
jordiluong 0:80ac024b84cb 338 }
jordiluong 4:ea7689bf97e1 339 break;
jordiluong 0:80ac024b84cb 340 }
jordiluong 0:80ac024b84cb 341
jordiluong 0:80ac024b84cb 342 default:
jordiluong 0:80ac024b84cb 343 {
jordiluong 5:0d3e8694726e 344 TurnMotorsOff(); // Turn motors off for safety
jordiluong 4:ea7689bf97e1 345 break;
jordiluong 0:80ac024b84cb 346 }
jordiluong 0:80ac024b84cb 347 }
jordiluong 0:80ac024b84cb 348 }
jordiluong 0:80ac024b84cb 349
jordiluong 7:757e95b4dc46 350 // MAIN FUNCTION ///////////////////////////////////////////////////////////////
jordiluong 0:80ac024b84cb 351 int main()
jordiluong 0:80ac024b84cb 352 {
jordiluong 0:80ac024b84cb 353 // Serial communication
jordiluong 0:80ac024b84cb 354 pc.baud(115200);
jordiluong 0:80ac024b84cb 355
jordiluong 4:ea7689bf97e1 356 pc.printf("START \r\n");
jordiluong 4:ea7689bf97e1 357
jordiluong 8:78d8ccf84a38 358 bqc.add(&bq1).add(&bq2); // Make BiQuad Chain
jordiluong 7:757e95b4dc46 359
jordiluong 11:5c06c97c3673 360 //controllerTicker.attach(&Motor1Controller, controller_Ts); // Ticker to control motor 1 (PID)
jordiluong 8:78d8ccf84a38 361 emgSampleTicker.attach(&EmgSample, emg_Ts); // Ticker to sample EMG
jordiluong 4:ea7689bf97e1 362
jordiluong 0:80ac024b84cb 363 while(true)
jordiluong 0:80ac024b84cb 364 {
jordiluong 0:80ac024b84cb 365 ProcessStateMachine(); // Execute states function
jordiluong 0:80ac024b84cb 366 }
jordiluong 0:80ac024b84cb 367 }