Jordi Luong / Mbed 2 deprecated Project_BioRobotics

Project BioRobotics Group 19

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@15:5d24f832bb7b, 2017-11-02 (annotated)

Committer:
jordiluong
Date:
Thu Nov 02 13:05:59 2017 +0000
Revision:
15:5d24f832bb7b
Parent:
14:95acac6a07c7
Child:
16:2cf8c2705936
Added direction switch

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 13:ec227b229f3d 8
jordiluong 5:0d3e8694726e 9 const double pi = 3.1415926535897; // Definition of pi
jordiluong 14:95acac6a07c7 10
jordiluong 7:757e95b4dc46 11 // SERIAL COMMUNICATION WITH PC ////////////////////////////////////////////////
jordiluong 0:80ac024b84cb 12 MODSERIAL pc(USBTX, USBRX);
jordiluong 12:12b72bd60fd1 13 HIDScope scope(4);
jordiluong 13:ec227b229f3d 14
jordiluong 7:757e95b4dc46 15 // STATES //////////////////////////////////////////////////////////////////////
jordiluong 14:95acac6a07c7 16 enum states{MOTORS_OFF, CALIBRATING, MOVING, HITTING};
jordiluong 0:80ac024b84cb 17 states currentState = MOTORS_OFF; // Start with motors off
jordiluong 0:80ac024b84cb 18 bool stateChanged = true; // Make sure the initialization of first state is executed
jordiluong 13:ec227b229f3d 19
jordiluong 7:757e95b4dc46 20 // ENCODER /////////////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 21 QEI Encoder1(D10,D11,NC,32); // CONNECT ENC1A TO D10, ENC1B TO D11
jordiluong 5:0d3e8694726e 22 QEI Encoder2(D12,D13,NC,32); // CONNECT ENC2A TO D12, ENC2B TO D13
jordiluong 13:ec227b229f3d 23
jordiluong 7:757e95b4dc46 24 // PINS ////////////////////////////////////////////////////////////////////////
jordiluong 4:ea7689bf97e1 25 DigitalOut motor1DirectionPin(D4); // Value 0: CCW; 1: CW
jordiluong 3:5c3edcd29448 26 PwmOut motor1MagnitudePin(D5);
jordiluong 3:5c3edcd29448 27 DigitalOut motor2DirectionPin(D7); // Value 0: CW or CCW?; 1: CW or CCW?
jordiluong 3:5c3edcd29448 28 PwmOut motor2MagnitudePin(D6);
jordiluong 4:ea7689bf97e1 29 InterruptIn button1(D2); // CONNECT BUT1 TO D2
jordiluong 4:ea7689bf97e1 30 InterruptIn button2(D3); // CONNECT BUT2 TO D3
jordiluong 14:95acac6a07c7 31 InterruptIn button3(SW2);
jordiluong 10:a9e344e440b8 32 InterruptIn button4(SW3);
jordiluong 14:95acac6a07c7 33 AnalogIn potmeter1(A0); // CONNECT POT1 TO A0
jordiluong 14:95acac6a07c7 34 AnalogIn potmeter2(A1); // CONNECT POT2 TO A1
jordiluong 14:95acac6a07c7 35 DigitalOut led1(LED_RED);
jordiluong 14:95acac6a07c7 36 DigitalOut led2(LED_BLUE);
jordiluong 14:95acac6a07c7 37 DigitalOut led3(LED_GREEN);
jordiluong 15:5d24f832bb7b 38 DigitalOut led4(D8); // CONNECT LED1 TO D8
jordiluong 15:5d24f832bb7b 39 DigitalOut led5(D9); // CONNECT LED2 TO D9
jordiluong 14:95acac6a07c7 40 AnalogIn emg_r(A2); // CONNECT EMG TO A2
jordiluong 14:95acac6a07c7 41 AnalogIn emg_l(A3); // CONNECT EMG TO A3
jordiluong 13:ec227b229f3d 42
jordiluong 7:757e95b4dc46 43 // MOTOR CONTROL ///////////////////////////////////////////////////////////////
jordiluong 14:95acac6a07c7 44 Ticker controllerTicker; // Ticker for the controller
jordiluong 14:95acac6a07c7 45 const double controller_Ts = 1/200.1; // Time step for controllerTicker [s]
jordiluong 5:0d3e8694726e 46 const double motorRatio = 131.25; // Ratio of the gearbox in the motors []
jordiluong 7:757e95b4dc46 47 const double radPerPulse = 2*pi/(32*motorRatio); // Amount of radians the motor rotates per encoder pulse [rad/pulse]
jordiluong 14:95acac6a07c7 48 volatile double xVelocity = 0, yVelocity = 0; // X and Y velocities of the end effector at the start
jordiluong 14:95acac6a07c7 49 double velocity = 0.01; // X and Y velocity of the end effector when desired
jordiluong 13:ec227b229f3d 50
jordiluong 7:757e95b4dc46 51 // MOTOR 1
jordiluong 14:95acac6a07c7 52 volatile double position1; // Position of motor 1 [rad]
jordiluong 14:95acac6a07c7 53 volatile double reference1 = 0; // Desired rotation of motor 1 [rad]
jordiluong 14:95acac6a07c7 54 double motor1Max = 0; // Maximum rotation of motor 1 [rad]
jordiluong 14:95acac6a07c7 55 double motor1Min = 2*pi*-40/360; // Minimum rotation of motor 1 [rad]
jordiluong 5:0d3e8694726e 56 // Controller gains
jordiluong 14:95acac6a07c7 57 const double motor1_KP = 13; // Position gain []
jordiluong 13:ec227b229f3d 58 const double motor1_KI = 7; // Integral gain []
jordiluong 13:ec227b229f3d 59 const double motor1_KD = 0.3; // Derivative gain []
jordiluong 5:0d3e8694726e 60 double motor1_err_int = 0, motor1_prev_err = 0;
jordiluong 5:0d3e8694726e 61 // Derivative filter coefficients
jordiluong 14:95acac6a07c7 62 const double motor1_f_a1 = 0.25, motor1_f_a2 = 0.8; // Derivative filter coefficients []
jordiluong 14:95acac6a07c7 63 const double motor1_f_b0 = 1, motor1_f_b1 = 2, motor1_f_b2 = 0.8; // Derivative filter coefficients []
jordiluong 5:0d3e8694726e 64 // Filter variables
jordiluong 5:0d3e8694726e 65 double motor1_f_v1 = 0, motor1_f_v2 = 0;
jordiluong 13:ec227b229f3d 66
jordiluong 7:757e95b4dc46 67 // MOTOR 2
jordiluong 14:95acac6a07c7 68 volatile double position2; // Position of motor 2 [rad]
jordiluong 14:95acac6a07c7 69 volatile double reference2 = 0; // Desired rotation of motor 2 [rad]
jordiluong 13:ec227b229f3d 70 double motor2Max = 2*pi*25/360; // Maximum rotation of motor 2 [rad]
jordiluong 13:ec227b229f3d 71 double motor2Min = 2*pi*-28/360; // Minimum rotation of motor 2 [rad]
jordiluong 5:0d3e8694726e 72 // Controller gains
jordiluong 14:95acac6a07c7 73 //const double motor2_KP = potmeter1*10; // Position gain []
jordiluong 14:95acac6a07c7 74 //const double motor2_KI = potmeter2*20; // Integral gain []
jordiluong 14:95acac6a07c7 75 const double motor2_KP = 13; // Position gain []
jordiluong 13:ec227b229f3d 76 const double motor2_KI = 5; // Integral gain []
jordiluong 13:ec227b229f3d 77 const double motor2_KD = 0.1; // Derivative gain []
jordiluong 10:a9e344e440b8 78 double motor2_err_int = 0, motor2_prev_err = 0;
jordiluong 5:0d3e8694726e 79 // Derivative filter coefficients
jordiluong 14:95acac6a07c7 80 const double motor2_f_a1 = 0.25, motor2_f_a2 = 0.8; // Derivative filter coefficients []
jordiluong 14:95acac6a07c7 81 const double motor2_f_b0 = 1, motor2_f_b1 = 2, motor2_f_b2 = 0.8; // Derivative filter coefficients []
jordiluong 5:0d3e8694726e 82 // Filter variables
jordiluong 10:a9e344e440b8 83 double motor2_f_v1 = 0, motor2_f_v2 = 0;
jordiluong 13:ec227b229f3d 84
jordiluong 14:95acac6a07c7 85 // EMG //////////////////////////////////////////////////////////////////
jordiluong 14:95acac6a07c7 86 Ticker emgLeft;
jordiluong 14:95acac6a07c7 87 Ticker emgRight;
jordiluong 14:95acac6a07c7 88 double emgTs = 0.5;
jordiluong 14:95acac6a07c7 89 // Filters
jordiluong 14:95acac6a07c7 90 BiQuadChain bqc;
jordiluong 14:95acac6a07c7 91 BiQuad bq2_high(0.875182, -1.750364, 0.87518, -1.73472, 0.766004);
jordiluong 14:95acac6a07c7 92 BiQuad bq3_notch(-1.1978e-16, 0.9561, 0.9780, -1.1978e-16, 0.9780);
jordiluong 14:95acac6a07c7 93 BiQuad bq1_low(3.65747e-2, 7.31495e-2, 3.65747e-2, -1.390892, 0.537191);
jordiluong 14:95acac6a07c7 94 // Right arm
jordiluong 14:95acac6a07c7 95 volatile double emgFiltered_r;
jordiluong 14:95acac6a07c7 96 volatile double filteredAbs_r;
jordiluong 14:95acac6a07c7 97 volatile double emg_value_r;
jordiluong 14:95acac6a07c7 98 volatile double onoffsignal_r;
jordiluong 14:95acac6a07c7 99 volatile bool check_calibration_r=0;
jordiluong 14:95acac6a07c7 100 volatile double avg_emg_r;
jordiluong 14:95acac6a07c7 101 volatile bool active_r = false;
jordiluong 14:95acac6a07c7 102 // Left arm
jordiluong 14:95acac6a07c7 103 volatile double emgFiltered_l;
jordiluong 14:95acac6a07c7 104 volatile double filteredAbs_l;
jordiluong 14:95acac6a07c7 105 volatile double emg_value_l;
jordiluong 14:95acac6a07c7 106 volatile double onoffsignal_l;
jordiluong 14:95acac6a07c7 107 volatile bool check_calibration_l=0;
jordiluong 14:95acac6a07c7 108 volatile double avg_emg_l;
jordiluong 14:95acac6a07c7 109 volatile bool active_l = false;
jordiluong 13:ec227b229f3d 110
jordiluong 14:95acac6a07c7 111 Ticker sampleTicker;
jordiluong 14:95acac6a07c7 112 const double sampleTs = 0.05;
jordiluong 15:5d24f832bb7b 113
jordiluong 15:5d24f832bb7b 114 volatile bool xdir = true, ydir = false;
jordiluong 15:5d24f832bb7b 115 volatile bool timer = false;
jordiluong 13:ec227b229f3d 116
jordiluong 7:757e95b4dc46 117 // FUNCTIONS ///////////////////////////////////////////////////////////////////
jordiluong 7:757e95b4dc46 118 // BIQUAD FILTER FOR DERIVATIVE OF ERROR ///////////////////////////////////////
jordiluong 5:0d3e8694726e 119 double biquad(double u, double &v1, double &v2, const double a1,
jordiluong 5:0d3e8694726e 120 const double a2, const double b0, const double b1, const double b2)
jordiluong 0:80ac024b84cb 121 {
jordiluong 5:0d3e8694726e 122 double v = u - a1*v1 - a2*v2;
jordiluong 5:0d3e8694726e 123 double y = b0*v + b1*v1 + b2*v2;
jordiluong 5:0d3e8694726e 124 v2 = v1; v1 = v;
jordiluong 5:0d3e8694726e 125 return y;
jordiluong 0:80ac024b84cb 126 }
jordiluong 13:ec227b229f3d 127
jordiluong 7:757e95b4dc46 128 // PID-CONTROLLER WITH FILTER //////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 129 double PID_Controller(double e, const double Kp, const double Ki,
jordiluong 5:0d3e8694726e 130 const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1,
jordiluong 5:0d3e8694726e 131 double &f_v2, const double f_a1, const double f_a2, const double f_b0,
jordiluong 5:0d3e8694726e 132 const double f_b1, const double f_b2)
jordiluong 0:80ac024b84cb 133 {
jordiluong 5:0d3e8694726e 134 // Derivative
jordiluong 8:78d8ccf84a38 135 double e_der = (e - e_prev)/Ts; // Calculate the derivative of error
jordiluong 12:12b72bd60fd1 136 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 12:12b72bd60fd1 137 //e_der = bq1.step(e_der);
jordiluong 5:0d3e8694726e 138 e_prev = e;
jordiluong 5:0d3e8694726e 139 // Integral
jordiluong 8:78d8ccf84a38 140 e_int = e_int + Ts*e; // Calculate the integral of error
jordiluong 5:0d3e8694726e 141 // PID
jordiluong 12:12b72bd60fd1 142 //pc.printf("%f --> %f \r\n", e_der, e_derf);
jordiluong 11:5c06c97c3673 143 return Kp*e + Ki*e_int + Kd*e_der;
jordiluong 3:5c3edcd29448 144 }
jordiluong 13:ec227b229f3d 145
jordiluong 7:757e95b4dc46 146 // MOTOR 1 /////////////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 147 void RotateMotor1(double motor1Value)
jordiluong 3:5c3edcd29448 148 {
jordiluong 10:a9e344e440b8 149 if(currentState == MOVING || currentState == HITTING) // Check if state is MOVING
jordiluong 5:0d3e8694726e 150 {
jordiluong 10:a9e344e440b8 151 if(motor1Value >= 0) motor1DirectionPin = 0; // Rotate motor 1 CW
jordiluong 10:a9e344e440b8 152 else motor1DirectionPin = 1; // Rotate motor 1 CCW
jordiluong 5:0d3e8694726e 153
jordiluong 5:0d3e8694726e 154 if(fabs(motor1Value) > 1) motor1MagnitudePin = 1;
jordiluong 5:0d3e8694726e 155 else motor1MagnitudePin = fabs(motor1Value);
jordiluong 5:0d3e8694726e 156 }
jordiluong 5:0d3e8694726e 157 else motor1MagnitudePin = 0;
jordiluong 3:5c3edcd29448 158 }
jordiluong 13:ec227b229f3d 159
jordiluong 10:a9e344e440b8 160 // MOTOR 2 /////////////////////////////////////////////////////////////////////
jordiluong 10:a9e344e440b8 161 void RotateMotor2(double motor2Value)
jordiluong 10:a9e344e440b8 162 {
jordiluong 10:a9e344e440b8 163 if(currentState == MOVING || currentState == HITTING) // Check if state is MOVING
jordiluong 10:a9e344e440b8 164 {
jordiluong 10:a9e344e440b8 165 if(motor2Value >= 0) motor2DirectionPin = 1; // Rotate motor 1 CW
jordiluong 10:a9e344e440b8 166 else motor2DirectionPin = 0; // Rotate motor 1 CCW
jordiluong 10:a9e344e440b8 167
jordiluong 10:a9e344e440b8 168 if(fabs(motor2Value) > 1) motor2MagnitudePin = 1;
jordiluong 10:a9e344e440b8 169 else motor2MagnitudePin = fabs(motor2Value);
jordiluong 10:a9e344e440b8 170 }
jordiluong 10:a9e344e440b8 171 else motor2MagnitudePin = 0;
jordiluong 10:a9e344e440b8 172 }
jordiluong 13:ec227b229f3d 173
jordiluong 7:757e95b4dc46 174 // MOTOR 1 PID-CONTROLLER //////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 175 void Motor1Controller()
jordiluong 5:0d3e8694726e 176 {
jordiluong 14:95acac6a07c7 177 if(currentState == MOVING)
jordiluong 14:95acac6a07c7 178 {
jordiluong 14:95acac6a07c7 179 position1 = radPerPulse * Encoder1.getPulses();
jordiluong 14:95acac6a07c7 180 position2 = radPerPulse * Encoder2.getPulses();
jordiluong 14:95acac6a07c7 181 //pc.printf("error %f \r\n", reference1 - position1);
jordiluong 14:95acac6a07c7 182 double motor1Value = PID_Controller(reference1 - position1, motor1_KP,
jordiluong 14:95acac6a07c7 183 motor1_KI, motor1_KD, controller_Ts, motor1_err_int, motor1_prev_err,
jordiluong 14:95acac6a07c7 184 motor1_f_v1, motor1_f_v2, motor1_f_a1, motor1_f_a2, motor1_f_b0,
jordiluong 14:95acac6a07c7 185 motor1_f_b1, motor1_f_b2);
jordiluong 14:95acac6a07c7 186 //pc.printf("motor value %f \r\n",motor1Value);
jordiluong 14:95acac6a07c7 187
jordiluong 14:95acac6a07c7 188 double motor2Value = PID_Controller(reference2 - position2, motor2_KP,
jordiluong 14:95acac6a07c7 189 motor2_KI, motor2_KD, controller_Ts, motor2_err_int, motor2_prev_err,
jordiluong 14:95acac6a07c7 190 motor2_f_v1, motor2_f_v2, motor2_f_a1, motor2_f_a2, motor2_f_b0,
jordiluong 14:95acac6a07c7 191 motor2_f_b1, motor2_f_b2);
jordiluong 14:95acac6a07c7 192
jordiluong 14:95acac6a07c7 193 //pc.printf("%f, %f \r\n", motor1Value, motor2Value);
jordiluong 14:95acac6a07c7 194
jordiluong 14:95acac6a07c7 195 RotateMotor1(motor1Value);
jordiluong 14:95acac6a07c7 196 RotateMotor2(motor2Value);
jordiluong 14:95acac6a07c7 197 }
jordiluong 10:a9e344e440b8 198 }
jordiluong 13:ec227b229f3d 199
jordiluong 12:12b72bd60fd1 200 // MOTOR 2 PID-CONTROLLER //////////////////////////////////////////////////////
jordiluong 12:12b72bd60fd1 201 /*void Motor2Controller()
jordiluong 12:12b72bd60fd1 202 {
jordiluong 12:12b72bd60fd1 203 position2 = radPerPulse * Encoder2.getPulses();
jordiluong 12:12b72bd60fd1 204 double motor2Value = PID_Controller(reference2 - position2, motor2_KP,
jordiluong 12:12b72bd60fd1 205 motor2_KI, motor2_KD, controller2_Ts, motor2_err_int, motor2_prev_err,
jordiluong 12:12b72bd60fd1 206 motor2_f_v1, motor2_f_v2, motor2_f_a1, motor2_f_a2, motor2_f_b0,
jordiluong 12:12b72bd60fd1 207 motor2_f_b1, motor2_f_b2);
jordiluong 12:12b72bd60fd1 208 RotateMotor2(motor2Value);
jordiluong 12:12b72bd60fd1 209 }
jordiluong 12:12b72bd60fd1 210 */
jordiluong 7:757e95b4dc46 211 // TURN OFF MOTORS /////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 212 void TurnMotorsOff()
jordiluong 5:0d3e8694726e 213 {
jordiluong 5:0d3e8694726e 214 motor1MagnitudePin = 0;
jordiluong 5:0d3e8694726e 215 motor2MagnitudePin = 0;
jordiluong 5:0d3e8694726e 216 }
jordiluong 13:ec227b229f3d 217
jordiluong 14:95acac6a07c7 218
jordiluong 14:95acac6a07c7 219 // EMG /////////////////////////////////////////////////////////////////////////
jordiluong 14:95acac6a07c7 220 // Filter EMG signal of right arm
jordiluong 13:ec227b229f3d 221
jordiluong 14:95acac6a07c7 222 void filter_r(){
jordiluong 14:95acac6a07c7 223 if(check_calibration_r==1){
jordiluong 14:95acac6a07c7 224 emg_value_r = emg_r.read();
jordiluong 14:95acac6a07c7 225 emgFiltered_r = bqc.step( emg_value_r );
jordiluong 14:95acac6a07c7 226 filteredAbs_r = abs( emgFiltered_r );
jordiluong 14:95acac6a07c7 227 if (avg_emg_r != 0){
jordiluong 14:95acac6a07c7 228 onoffsignal_r=filteredAbs_r/avg_emg_r; //divide the emg_r signal by the max emg_r to calibrate the signal per person
jordiluong 14:95acac6a07c7 229 }
jordiluong 14:95acac6a07c7 230 }
jordiluong 14:95acac6a07c7 231 }
jordiluong 14:95acac6a07c7 232
jordiluong 14:95acac6a07c7 233 // Filter EMG signal of left arm
jordiluong 14:95acac6a07c7 234 void filter_l(){
jordiluong 14:95acac6a07c7 235 if(check_calibration_l==1){
jordiluong 14:95acac6a07c7 236 emg_value_l = emg_l.read();
jordiluong 14:95acac6a07c7 237 emgFiltered_l = bqc.step( emg_value_l );
jordiluong 14:95acac6a07c7 238 filteredAbs_l = abs( emgFiltered_l );
jordiluong 14:95acac6a07c7 239 if (avg_emg_l != 0){
jordiluong 14:95acac6a07c7 240 onoffsignal_l=filteredAbs_l/avg_emg_l; //divide the emg_r signal by the avg_emg_l wat staat voor avg emg in gespannen toestand
jordiluong 14:95acac6a07c7 241 }
jordiluong 10:a9e344e440b8 242 }
jordiluong 14:95acac6a07c7 243 }
jordiluong 14:95acac6a07c7 244
jordiluong 14:95acac6a07c7 245 // Check threshold right arm
jordiluong 14:95acac6a07c7 246 void check_emg_r(){
jordiluong 14:95acac6a07c7 247 double filteredAbs_temp_r;
jordiluong 14:95acac6a07c7 248
jordiluong 14:95acac6a07c7 249 if((check_calibration_l==1) &&(check_calibration_r==1)){
jordiluong 14:95acac6a07c7 250 for( int i = 0; i<250;i++){
jordiluong 14:95acac6a07c7 251 filter_r();
jordiluong 14:95acac6a07c7 252 filteredAbs_temp_r = filteredAbs_temp_r + onoffsignal_r;
jordiluong 14:95acac6a07c7 253 wait(0.0004); // 0.0004
jordiluong 14:95acac6a07c7 254 }
jordiluong 14:95acac6a07c7 255 filteredAbs_temp_r = filteredAbs_temp_r/250;
jordiluong 14:95acac6a07c7 256 if(filteredAbs_temp_r<=0.3){ //if signal is lower then 0.5 the blue light goes on
jordiluong 14:95acac6a07c7 257 led1.write(1); //led 1 is rood en uit
jordiluong 14:95acac6a07c7 258
jordiluong 14:95acac6a07c7 259 active_r = false;
jordiluong 14:95acac6a07c7 260 }
jordiluong 14:95acac6a07c7 261 else if(filteredAbs_temp_r > 0.3){ //if signal does not pass threshold value, blue light goes on
jordiluong 14:95acac6a07c7 262 led1.write(0);
jordiluong 14:95acac6a07c7 263 active_r = true;
jordiluong 14:95acac6a07c7 264 }
jordiluong 10:a9e344e440b8 265 }
jordiluong 14:95acac6a07c7 266 }
jordiluong 14:95acac6a07c7 267 // Check threshold left arm
jordiluong 14:95acac6a07c7 268 void check_emg_l(){
jordiluong 14:95acac6a07c7 269 double filteredAbs_temp_l;
jordiluong 14:95acac6a07c7 270
jordiluong 14:95acac6a07c7 271 if((check_calibration_l)==1 &&(check_calibration_r==1) ){
jordiluong 14:95acac6a07c7 272 for( int i = 0; i<250;i++){
jordiluong 14:95acac6a07c7 273 filter_l();
jordiluong 14:95acac6a07c7 274 filteredAbs_temp_l = filteredAbs_temp_l + onoffsignal_l;
jordiluong 14:95acac6a07c7 275 wait(0.0004); // 0.0004
jordiluong 14:95acac6a07c7 276 }
jordiluong 14:95acac6a07c7 277 filteredAbs_temp_l = filteredAbs_temp_l/250;
jordiluong 14:95acac6a07c7 278 if(filteredAbs_temp_l<=0.3){ //if signal is lower then 0.5 the blue light goes on
jordiluong 14:95acac6a07c7 279 // led1.write(1); //led 1 is rood en uit
jordiluong 14:95acac6a07c7 280 led2.write(1); //led 2 is blauw en aan
jordiluong 14:95acac6a07c7 281 active_l = false;
jordiluong 14:95acac6a07c7 282 }
jordiluong 14:95acac6a07c7 283 else if(filteredAbs_temp_l > 0.3){ //if signal does not pass threshold value, blue light goes on
jordiluong 14:95acac6a07c7 284 // led1.write(0);
jordiluong 14:95acac6a07c7 285 led2.write(0);
jordiluong 14:95acac6a07c7 286 active_l = true;
jordiluong 14:95acac6a07c7 287 }
jordiluong 14:95acac6a07c7 288 }
jordiluong 14:95acac6a07c7 289
jordiluong 10:a9e344e440b8 290 }
jordiluong 13:ec227b229f3d 291
jordiluong 14:95acac6a07c7 292 // Calibrate right arm
jordiluong 14:95acac6a07c7 293 int calibration_r(){
jordiluong 14:95acac6a07c7 294 led3.write(0);
jordiluong 14:95acac6a07c7 295
jordiluong 14:95acac6a07c7 296 double signal_verzameling_r = 0;
jordiluong 14:95acac6a07c7 297 for(int n =0; n<5000;n++){
jordiluong 14:95acac6a07c7 298 filter_r();
jordiluong 14:95acac6a07c7 299 //read for 5000 samples as calibration
jordiluong 14:95acac6a07c7 300 emg_value_r = emg_r.read();
jordiluong 14:95acac6a07c7 301 emgFiltered_r = bqc.step( emg_value_r );
jordiluong 14:95acac6a07c7 302 filteredAbs_r = abs(emgFiltered_r);
jordiluong 14:95acac6a07c7 303
jordiluong 14:95acac6a07c7 304
jordiluong 14:95acac6a07c7 305 // signal_verzameling[n]= filteredAbs_r;
jordiluong 14:95acac6a07c7 306 signal_verzameling_r = signal_verzameling_r + filteredAbs_r ;
jordiluong 14:95acac6a07c7 307 wait(0.0004);
jordiluong 14:95acac6a07c7 308
jordiluong 14:95acac6a07c7 309 if (n == 4999){
jordiluong 14:95acac6a07c7 310 avg_emg_r = signal_verzameling_r / n;
jordiluong 14:95acac6a07c7 311
jordiluong 14:95acac6a07c7 312 }
jordiluong 14:95acac6a07c7 313 }
jordiluong 14:95acac6a07c7 314
jordiluong 14:95acac6a07c7 315 led3.write(1);
jordiluong 14:95acac6a07c7 316 //pc.printf("calibratie rechts compleet\n\r");
jordiluong 14:95acac6a07c7 317
jordiluong 14:95acac6a07c7 318 check_calibration_r=1;
jordiluong 14:95acac6a07c7 319 return 0;
jordiluong 14:95acac6a07c7 320 }
jordiluong 14:95acac6a07c7 321
jordiluong 14:95acac6a07c7 322 // Calibrate left arm
jordiluong 14:95acac6a07c7 323 int calibration_l(){
jordiluong 14:95acac6a07c7 324 led3.write(0);
jordiluong 14:95acac6a07c7 325
jordiluong 14:95acac6a07c7 326 double signal_verzameling_l= 0;
jordiluong 14:95acac6a07c7 327 for(int n =0; n<5000;n++){
jordiluong 14:95acac6a07c7 328 filter_l();
jordiluong 14:95acac6a07c7 329 //read for 5000 samples as calibration
jordiluong 14:95acac6a07c7 330 emg_value_l = emg_l.read();
jordiluong 14:95acac6a07c7 331 emgFiltered_l = bqc.step( emg_value_l );
jordiluong 14:95acac6a07c7 332 filteredAbs_l = abs(emgFiltered_l);
jordiluong 14:95acac6a07c7 333
jordiluong 14:95acac6a07c7 334
jordiluong 14:95acac6a07c7 335 // signal_verzameling[n]= filteredAbs_r;
jordiluong 14:95acac6a07c7 336 signal_verzameling_l = signal_verzameling_l + filteredAbs_l ;
jordiluong 14:95acac6a07c7 337 wait(0.0004);
jordiluong 14:95acac6a07c7 338
jordiluong 14:95acac6a07c7 339 if (n == 4999){
jordiluong 14:95acac6a07c7 340 avg_emg_l = signal_verzameling_l / n;
jordiluong 14:95acac6a07c7 341
jordiluong 14:95acac6a07c7 342 }
jordiluong 14:95acac6a07c7 343 }
jordiluong 14:95acac6a07c7 344 led3.write(1);
jordiluong 14:95acac6a07c7 345 wait(1);
jordiluong 14:95acac6a07c7 346 check_calibration_l=1;
jordiluong 14:95acac6a07c7 347
jordiluong 14:95acac6a07c7 348 //pc.printf("calibratie links compleet\n\r");
jordiluong 14:95acac6a07c7 349 // }
jordiluong 14:95acac6a07c7 350 return 0;
jordiluong 14:95acac6a07c7 351 }
jordiluong 14:95acac6a07c7 352
jordiluong 14:95acac6a07c7 353 // DETERMINE JOINT VELOCITIES //////////////////////////////////////////////////
jordiluong 14:95acac6a07c7 354 void JointVelocities()
jordiluong 10:a9e344e440b8 355 {
jordiluong 10:a9e344e440b8 356 if(currentState == MOVING)
jordiluong 10:a9e344e440b8 357 {
jordiluong 14:95acac6a07c7 358 position1 = radPerPulse * Encoder1.getPulses();
jordiluong 14:95acac6a07c7 359 position2 = radPerPulse * Encoder2.getPulses();
jordiluong 14:95acac6a07c7 360
jordiluong 15:5d24f832bb7b 361 if(xdir)
jordiluong 14:95acac6a07c7 362 {
jordiluong 15:5d24f832bb7b 363 if(active_r && reference1 > motor1Min && reference2 < motor2Max)
jordiluong 15:5d24f832bb7b 364 {
jordiluong 15:5d24f832bb7b 365 xVelocity = velocity;
jordiluong 15:5d24f832bb7b 366 pc.printf("x positive \r\n");
jordiluong 15:5d24f832bb7b 367 }
jordiluong 15:5d24f832bb7b 368 else if(active_l && reference1 < motor1Max && reference2 > motor2Min)
jordiluong 15:5d24f832bb7b 369 {
jordiluong 15:5d24f832bb7b 370 xVelocity = -velocity;
jordiluong 15:5d24f832bb7b 371 pc.printf("x negative \r\n");
jordiluong 15:5d24f832bb7b 372 }
jordiluong 15:5d24f832bb7b 373 else xVelocity = 0;
jordiluong 14:95acac6a07c7 374 }
jordiluong 15:5d24f832bb7b 375 else if(ydir)
jordiluong 14:95acac6a07c7 376 {
jordiluong 15:5d24f832bb7b 377 if(active_r && reference2 < motor2Max )//&& reference1 > motor2Min)
jordiluong 15:5d24f832bb7b 378 {
jordiluong 15:5d24f832bb7b 379 yVelocity = velocity;
jordiluong 15:5d24f832bb7b 380 pc.printf("y positive \r\n");
jordiluong 15:5d24f832bb7b 381 }
jordiluong 15:5d24f832bb7b 382 else if(active_l && reference2 > motor2Min )//&& reference1 > motor2Min)
jordiluong 15:5d24f832bb7b 383 {
jordiluong 15:5d24f832bb7b 384 yVelocity = -velocity;
jordiluong 15:5d24f832bb7b 385 pc.printf("y negative \r\n");
jordiluong 15:5d24f832bb7b 386 }
jordiluong 15:5d24f832bb7b 387 else yVelocity = 0;
jordiluong 14:95acac6a07c7 388 }
jordiluong 14:95acac6a07c7 389
jordiluong 14:95acac6a07c7 390 //pc.printf("x %f, y %f \r\n", xVelocity, yVelocity);
jordiluong 14:95acac6a07c7 391
jordiluong 14:95acac6a07c7 392 // Construct Jacobian
jordiluong 14:95acac6a07c7 393 double q[4];
jordiluong 14:95acac6a07c7 394 q[0] = position1, q[1] = -position1;
jordiluong 14:95acac6a07c7 395 q[2] = position2, q[3] = -position2;
jordiluong 14:95acac6a07c7 396
jordiluong 14:95acac6a07c7 397 double T2[3]; // Second column of the jacobian
jordiluong 14:95acac6a07c7 398 double T3[3]; // Third column of the jacobian
jordiluong 14:95acac6a07c7 399 double T4[3]; // Fourth column of the jacobian
jordiluong 14:95acac6a07c7 400 double T1[6];
jordiluong 14:95acac6a07c7 401 static const signed char b_T1[3] = { 1, 0, 0 };
jordiluong 14:95acac6a07c7 402 double J_data[6];
jordiluong 14:95acac6a07c7 403
jordiluong 14:95acac6a07c7 404 T2[0] = 1.0;
jordiluong 14:95acac6a07c7 405 T2[1] = 0.365 * cos(q[0]);
jordiluong 14:95acac6a07c7 406 T2[2] = 0.365 * sin(q[0]);
jordiluong 14:95acac6a07c7 407 T3[0] = 1.0;
jordiluong 14:95acac6a07c7 408 T3[1] = 0.365 * cos(q[0]) + 0.2353720459187964 * sin((0.21406068356382149 +
jordiluong 14:95acac6a07c7 409 q[0]) + q[1]);
jordiluong 14:95acac6a07c7 410 T3[2] = 0.365 * sin(q[0]) - 0.2353720459187964 * cos((0.21406068356382149 +
jordiluong 14:95acac6a07c7 411 q[0]) + q[1]);
jordiluong 14:95acac6a07c7 412 T4[0] = 1.0;
jordiluong 14:95acac6a07c7 413 T4[1] = (0.365 * cos(q[0]) + 0.2353720459187964 * sin((0.21406068356382149 +
jordiluong 14:95acac6a07c7 414 q[0]) + q[1])) + 0.265 * sin((q[0] + q[1]) + q[2]);
jordiluong 14:95acac6a07c7 415 T4[2] = (0.365 * sin(q[0]) - 0.2353720459187964 * cos((0.21406068356382149 +
jordiluong 14:95acac6a07c7 416 q[0]) + q[1])) - 0.265 * cos((q[0] + q[1]) + q[2]);
jordiluong 14:95acac6a07c7 417
jordiluong 14:95acac6a07c7 418 for (int i = 0; i < 3; i++)
jordiluong 14:95acac6a07c7 419 {
jordiluong 14:95acac6a07c7 420 T1[i] = (double)b_T1[i] - T2[i];
jordiluong 14:95acac6a07c7 421 T1[3 + i] = T3[i] - T4[i];
jordiluong 14:95acac6a07c7 422 }
jordiluong 14:95acac6a07c7 423
jordiluong 14:95acac6a07c7 424 for (int i = 0; i < 2; i++)
jordiluong 14:95acac6a07c7 425 {
jordiluong 14:95acac6a07c7 426 for (int j = 0; j < 3; j++)
jordiluong 14:95acac6a07c7 427 {
jordiluong 14:95acac6a07c7 428 J_data[j + 3 * i] = T1[j + 3 * i];
jordiluong 14:95acac6a07c7 429 }
jordiluong 14:95acac6a07c7 430 }
jordiluong 14:95acac6a07c7 431
jordiluong 14:95acac6a07c7 432 // Here the first row of the Jacobian is cut off, so we do not take rotation into consideration
jordiluong 14:95acac6a07c7 433 // Note: the matrices from now on will we constructed rowwise
jordiluong 14:95acac6a07c7 434 double Jvelocity[4];
jordiluong 14:95acac6a07c7 435 Jvelocity[0] = J_data[1];
jordiluong 14:95acac6a07c7 436 Jvelocity[1] = J_data[4];
jordiluong 14:95acac6a07c7 437 Jvelocity[2] = J_data[2];
jordiluong 14:95acac6a07c7 438 Jvelocity[3] = J_data[5];
jordiluong 14:95acac6a07c7 439
jordiluong 14:95acac6a07c7 440 // Creating the inverse Jacobian
jordiluong 14:95acac6a07c7 441 double Jvelocity_inv[4]; // The inverse matrix of the jacobian
jordiluong 14:95acac6a07c7 442 double determ = Jvelocity[0]*Jvelocity[3]-Jvelocity[1]*Jvelocity[2]; // The determinant of the matrix
jordiluong 14:95acac6a07c7 443 Jvelocity_inv[0] = Jvelocity[3]/determ;
jordiluong 14:95acac6a07c7 444 Jvelocity_inv[1] = -Jvelocity[1]/determ;
jordiluong 14:95acac6a07c7 445 Jvelocity_inv[2] = -Jvelocity[2]/determ;
jordiluong 14:95acac6a07c7 446 Jvelocity_inv[3] = Jvelocity[0]/determ;
jordiluong 14:95acac6a07c7 447
jordiluong 14:95acac6a07c7 448 // Now the velocity of the joints are found by giving the velocity of the end-effector and the inverse jacobian
jordiluong 14:95acac6a07c7 449 double msh[2]; // This is the velocity the joints have to have
jordiluong 14:95acac6a07c7 450 msh[0] = xVelocity*Jvelocity_inv[0] + yVelocity*Jvelocity_inv[1];
jordiluong 14:95acac6a07c7 451 msh[1] = xVelocity*Jvelocity_inv[2] + yVelocity*Jvelocity_inv[3];
jordiluong 14:95acac6a07c7 452
jordiluong 14:95acac6a07c7 453 if(reference1 + msh[0]*sampleTs > motor1Max) reference1 = motor1Max;
jordiluong 14:95acac6a07c7 454 else if(reference1 + msh[0]*sampleTs < motor1Min) reference1 = motor1Min;
jordiluong 14:95acac6a07c7 455 else reference1 = reference1 + msh[0]*sampleTs;
jordiluong 14:95acac6a07c7 456
jordiluong 14:95acac6a07c7 457 if(reference2 + msh[1]*sampleTs > motor2Max) reference2 = motor2Max;
jordiluong 14:95acac6a07c7 458 else if(reference2 + msh[1]*sampleTs < motor2Min) reference2 = motor2Min;
jordiluong 14:95acac6a07c7 459 else reference2 = reference2 + msh[1]*sampleTs;
jordiluong 14:95acac6a07c7 460
jordiluong 14:95acac6a07c7 461 scope.set(0,reference1);
jordiluong 14:95acac6a07c7 462 scope.set(1,position1);
jordiluong 14:95acac6a07c7 463 scope.set(2,reference2);
jordiluong 14:95acac6a07c7 464 scope.set(3,position2);
jordiluong 14:95acac6a07c7 465 scope.send();
jordiluong 14:95acac6a07c7 466
jordiluong 14:95acac6a07c7 467 pc.printf("position 1 %f, 2 %f \r\n", position1/2/pi*360, position2/2/pi*360);
jordiluong 14:95acac6a07c7 468 pc.printf("reference 1 %f, 2 %f \r\n", reference1/2/pi*360, reference2/2/pi*360);
jordiluong 14:95acac6a07c7 469 //pc.printf("msh*Ts 1 %f, 2 %f \r\n\n", msh[0]*emg_Ts, msh[1]*emg_Ts);
jordiluong 14:95acac6a07c7 470
jordiluong 10:a9e344e440b8 471 }
jordiluong 10:a9e344e440b8 472 }
jordiluong 15:5d24f832bb7b 473
jordiluong 15:5d24f832bb7b 474 void StartTimer()
jordiluong 15:5d24f832bb7b 475 {
jordiluong 15:5d24f832bb7b 476 timer = true;
jordiluong 15:5d24f832bb7b 477 wait(3);
jordiluong 15:5d24f832bb7b 478 timer = false;
jordiluong 15:5d24f832bb7b 479 }
jordiluong 13:ec227b229f3d 480
jordiluong 7:757e95b4dc46 481 // STATES //////////////////////////////////////////////////////////////////////
jordiluong 0:80ac024b84cb 482 void ProcessStateMachine()
jordiluong 0:80ac024b84cb 483 {
jordiluong 0:80ac024b84cb 484 switch(currentState)
jordiluong 0:80ac024b84cb 485 {
jordiluong 0:80ac024b84cb 486 case MOTORS_OFF:
jordiluong 0:80ac024b84cb 487 {
jordiluong 0:80ac024b84cb 488 // State initialization
jordiluong 0:80ac024b84cb 489 if(stateChanged)
jordiluong 0:80ac024b84cb 490 {
jordiluong 15:5d24f832bb7b 491 pc.printf("Entering MOTORS_OFF \r\n"
jordiluong 15:5d24f832bb7b 492 "Press button 1 to enter CALIBRATING \r\n");
jordiluong 5:0d3e8694726e 493 TurnMotorsOff(); // Turn motors off
jordiluong 0:80ac024b84cb 494 stateChanged = false;
jordiluong 0:80ac024b84cb 495 }
jordiluong 0:80ac024b84cb 496
jordiluong 0:80ac024b84cb 497 // Home command
jordiluong 4:ea7689bf97e1 498 if(!button1)
jordiluong 0:80ac024b84cb 499 {
jordiluong 14:95acac6a07c7 500 currentState = CALIBRATING;
jordiluong 14:95acac6a07c7 501 stateChanged = true;
jordiluong 14:95acac6a07c7 502 break;
jordiluong 14:95acac6a07c7 503 }
jordiluong 14:95acac6a07c7 504 break;
jordiluong 14:95acac6a07c7 505 }
jordiluong 14:95acac6a07c7 506
jordiluong 14:95acac6a07c7 507 case CALIBRATING:
jordiluong 14:95acac6a07c7 508 {
jordiluong 14:95acac6a07c7 509 // State initialization
jordiluong 14:95acac6a07c7 510 if(stateChanged)
jordiluong 14:95acac6a07c7 511 {
jordiluong 15:5d24f832bb7b 512 pc.printf("Entering CALIBRATING \r\n"
jordiluong 15:5d24f832bb7b 513 "Press button 1 to enter MOVING \r\n");
jordiluong 14:95acac6a07c7 514 stateChanged = false;
jordiluong 14:95acac6a07c7 515 calibration_r();
jordiluong 14:95acac6a07c7 516 calibration_l();
jordiluong 14:95acac6a07c7 517 currentState = MOVING;
jordiluong 14:95acac6a07c7 518 stateChanged = true;
jordiluong 14:95acac6a07c7 519 }
jordiluong 14:95acac6a07c7 520 /*
jordiluong 14:95acac6a07c7 521 // Home command
jordiluong 14:95acac6a07c7 522 if(!button1)
jordiluong 14:95acac6a07c7 523 {
jordiluong 0:80ac024b84cb 524 currentState = MOVING;
jordiluong 0:80ac024b84cb 525 stateChanged = true;
jordiluong 0:80ac024b84cb 526 break;
jordiluong 0:80ac024b84cb 527 }
jordiluong 14:95acac6a07c7 528 */
jordiluong 4:ea7689bf97e1 529 break;
jordiluong 0:80ac024b84cb 530 }
jordiluong 0:80ac024b84cb 531
jordiluong 0:80ac024b84cb 532 case MOVING:
jordiluong 0:80ac024b84cb 533 {
jordiluong 0:80ac024b84cb 534 // State initialization
jordiluong 0:80ac024b84cb 535 if(stateChanged)
jordiluong 0:80ac024b84cb 536 {
jordiluong 15:5d24f832bb7b 537 pc.printf("Entering MOVING \r\n");
jordiluong 12:12b72bd60fd1 538 //"Press button 2 to enter HITTING \r\n");
jordiluong 0:80ac024b84cb 539 stateChanged = false;
jordiluong 0:80ac024b84cb 540 }
jordiluong 0:80ac024b84cb 541
jordiluong 15:5d24f832bb7b 542 if(active_l && active_r)
jordiluong 15:5d24f832bb7b 543 {
jordiluong 15:5d24f832bb7b 544 if(!timer)
jordiluong 15:5d24f832bb7b 545 {
jordiluong 15:5d24f832bb7b 546 active_l = false;
jordiluong 15:5d24f832bb7b 547 active_r = false;
jordiluong 15:5d24f832bb7b 548 xdir = !xdir;
jordiluong 15:5d24f832bb7b 549 ydir = !ydir;
jordiluong 15:5d24f832bb7b 550 led4 = !led4;
jordiluong 15:5d24f832bb7b 551 led5 = !led5;
jordiluong 15:5d24f832bb7b 552 if(active_l)pc.printf("links active");
jordiluong 15:5d24f832bb7b 553 else pc.printf("links inactive");
jordiluong 15:5d24f832bb7b 554 StartTimer();
jordiluong 15:5d24f832bb7b 555 }
jordiluong 15:5d24f832bb7b 556 }
jordiluong 15:5d24f832bb7b 557
jordiluong 0:80ac024b84cb 558 // Hit command
jordiluong 10:a9e344e440b8 559 /*if(!button2)
jordiluong 0:80ac024b84cb 560 {
jordiluong 0:80ac024b84cb 561 currentState = HITTING;
jordiluong 0:80ac024b84cb 562 stateChanged = true;
jordiluong 0:80ac024b84cb 563 break;
jordiluong 0:80ac024b84cb 564 }
jordiluong 10:a9e344e440b8 565 */
jordiluong 4:ea7689bf97e1 566 break;
jordiluong 0:80ac024b84cb 567 }
jordiluong 0:80ac024b84cb 568
jordiluong 0:80ac024b84cb 569 case HITTING:
jordiluong 0:80ac024b84cb 570 {
jordiluong 0:80ac024b84cb 571 // State initialization
jordiluong 0:80ac024b84cb 572 if(stateChanged)
jordiluong 0:80ac024b84cb 573 {
jordiluong 12:12b72bd60fd1 574 //pc.printf("Entering HITTING \r\n");
jordiluong 5:0d3e8694726e 575 stateChanged = false;
jordiluong 4:ea7689bf97e1 576 //HitBall(); // Hit the ball
jordiluong 0:80ac024b84cb 577 currentState = MOVING;
jordiluong 0:80ac024b84cb 578 stateChanged = true;
jordiluong 0:80ac024b84cb 579 break;
jordiluong 0:80ac024b84cb 580 }
jordiluong 4:ea7689bf97e1 581 break;
jordiluong 0:80ac024b84cb 582 }
jordiluong 0:80ac024b84cb 583
jordiluong 0:80ac024b84cb 584 default:
jordiluong 0:80ac024b84cb 585 {
jordiluong 5:0d3e8694726e 586 TurnMotorsOff(); // Turn motors off for safety
jordiluong 4:ea7689bf97e1 587 break;
jordiluong 0:80ac024b84cb 588 }
jordiluong 0:80ac024b84cb 589 }
jordiluong 0:80ac024b84cb 590 }
jordiluong 13:ec227b229f3d 591
jordiluong 7:757e95b4dc46 592 // MAIN FUNCTION ///////////////////////////////////////////////////////////////
jordiluong 0:80ac024b84cb 593 int main()
jordiluong 0:80ac024b84cb 594 {
jordiluong 0:80ac024b84cb 595 // Serial communication
jordiluong 0:80ac024b84cb 596 pc.baud(115200);
jordiluong 0:80ac024b84cb 597
jordiluong 14:95acac6a07c7 598 led1.write(1);
jordiluong 14:95acac6a07c7 599 led2.write(1);
jordiluong 14:95acac6a07c7 600 led3.write(1);
jordiluong 15:5d24f832bb7b 601 led4.write(1);
jordiluong 15:5d24f832bb7b 602 led5.write(0);
jordiluong 15:5d24f832bb7b 603
jordiluong 4:ea7689bf97e1 604 pc.printf("START \r\n");
jordiluong 4:ea7689bf97e1 605
jordiluong 14:95acac6a07c7 606 bqc.add( &bq1_low ).add( &bq2_high ).add( &bq3_notch );
jordiluong 7:757e95b4dc46 607
jordiluong 14:95acac6a07c7 608 sampleTicker.attach(&JointVelocities, sampleTs); // Ticker to sample EMG
jordiluong 12:12b72bd60fd1 609 controllerTicker.attach(&Motor1Controller, controller_Ts); // Ticker to control motor 1 (PID)
jordiluong 14:95acac6a07c7 610 emgRight.attach(&check_emg_r, emgTs); //continously execute the motor controller
jordiluong 14:95acac6a07c7 611 emgLeft.attach(&check_emg_l, emgTs);
jordiluong 12:12b72bd60fd1 612
jordiluong 12:12b72bd60fd1 613 motor1MagnitudePin.period_ms(1);
jordiluong 12:12b72bd60fd1 614 motor2MagnitudePin.period_ms(1);
jordiluong 12:12b72bd60fd1 615 TurnMotorsOff();
jordiluong 4:ea7689bf97e1 616
jordiluong 0:80ac024b84cb 617 while(true)
jordiluong 0:80ac024b84cb 618 {
jordiluong 0:80ac024b84cb 619 ProcessStateMachine(); // Execute states function
jordiluong 0:80ac024b84cb 620 }
jordiluong 0:80ac024b84cb 621 }