Jordi Luong / Mbed 2 deprecated Project_BioRobotics

Project BioRobotics Group 19

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@16:2cf8c2705936, 2017-11-02 (annotated)

Committer:
jordiluong
Date:
Thu Nov 02 13:47:48 2017 +0000
Revision:
16:2cf8c2705936
Parent:
15:5d24f832bb7b
Child:
17:f8dd5b8e4b52
Direction switch working properly

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