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