![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
werkt nog niet
Dependencies: HIDScope MODSERIAL biquadFilter mbed QEI
Fork of Project_script_union by
main.cpp@31:481ad25a40c3, 2018-11-01 (annotated)
- Committer:
- MarijkeZondag
- Date:
- Thu Nov 01 14:14:03 2018 +0000
- Revision:
- 31:481ad25a40c3
- Parent:
- 30:8191e8541a0a
- Child:
- 32:137d0f27e5a8
Update 15:13 uur
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
vsluiter | 0:c8f15874531b | 1 | #include "mbed.h" |
vsluiter | 0:c8f15874531b | 2 | #include "MODSERIAL.h" |
MarijkeZondag | 10:39ec51206c8b | 3 | #include "BiQuad.h" |
MarijkeZondag | 10:39ec51206c8b | 4 | #include "HIDScope.h" |
MarijkeZondag | 10:39ec51206c8b | 5 | #include <math.h> |
MarijkeZondag | 29:df10cb76ef26 | 6 | #include "QEI.h" |
vsluiter | 0:c8f15874531b | 7 | |
MarijkeZondag | 22:5d956c93b3ae | 8 | //ATTENTION: set mBed to version 151 |
MarijkeZondag | 22:5d956c93b3ae | 9 | // set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder) |
MarijkeZondag | 22:5d956c93b3ae | 10 | // set MODSERIAL to version 44 |
MarijkeZondag | 22:5d956c93b3ae | 11 | // set HIDScope to version 7 |
MarijkeZondag | 22:5d956c93b3ae | 12 | // set biquadFilter to version 7 |
MarijkeZondag | 10:39ec51206c8b | 13 | |
MarijkeZondag | 29:df10cb76ef26 | 14 | AnalogIn potmeter1 (A0); //First raw EMG signal input |
MarijkeZondag | 29:df10cb76ef26 | 15 | AnalogIn potmeter2 (A1); //Second raw EMG signal input |
MarijkeZondag | 22:5d956c93b3ae | 16 | |
MarijkeZondag | 26:ac5656aa35c7 | 17 | InterruptIn button1 (D10); |
MarijkeZondag | 16:5f7196ddc77b | 18 | InterruptIn button2 (D11); |
MarijkeZondag | 6:f4bbb73f3989 | 19 | |
MarijkeZondag | 30:8191e8541a0a | 20 | DigitalOut directionpin1 (D4); |
MarijkeZondag | 30:8191e8541a0a | 21 | DigitalOut directionpin2 (D7); |
MarijkeZondag | 26:ac5656aa35c7 | 22 | |
MarijkeZondag | 30:8191e8541a0a | 23 | PwmOut pwmpin1 (D5); |
MarijkeZondag | 30:8191e8541a0a | 24 | PwmOut pwmpin2 (D6); |
MarijkeZondag | 22:5d956c93b3ae | 25 | |
MarijkeZondag | 17:741798018c0d | 26 | DigitalOut ledr (LED_RED); |
MarijkeZondag | 17:741798018c0d | 27 | DigitalOut ledb (LED_BLUE); |
MarijkeZondag | 17:741798018c0d | 28 | DigitalOut ledg (LED_GREEN); |
MarijkeZondag | 13:a3d4b4daf5b4 | 29 | |
MarijkeZondag | 9:c722418997b5 | 30 | |
MarijkeZondag | 23:97a976a8f0fc | 31 | MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off |
MarijkeZondag | 31:481ad25a40c3 | 32 | QEI encoder2 (D9, D8, NC, 8400,QEI::X4_ENCODING); |
MarijkeZondag | 31:481ad25a40c3 | 33 | QEI encoder1 (D12, D13, NC, 8400,QEI::X4_ENCODING); |
MarijkeZondag | 6:f4bbb73f3989 | 34 | |
MarijkeZondag | 23:97a976a8f0fc | 35 | //HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered |
vsluiter | 0:c8f15874531b | 36 | |
MarijkeZondag | 22:5d956c93b3ae | 37 | //Tickers |
MarijkeZondag | 31:481ad25a40c3 | 38 | Ticker func_tick; |
MarijkeZondag | 31:481ad25a40c3 | 39 | Ticker printTicker; |
MarijkeZondag | 30:8191e8541a0a | 40 | |
MarijkeZondag | 9:c722418997b5 | 41 | |
MarijkeZondag | 9:c722418997b5 | 42 | //Global variables |
MarijkeZondag | 30:8191e8541a0a | 43 | const float T = 0.02f; //Ticker period Deze wordt ook gebruikt in de PID, moet die niet anders??? |
MarijkeZondag | 30:8191e8541a0a | 44 | const float T2 = 0.02f; |
MarijkeZondag | 10:39ec51206c8b | 45 | |
MarijkeZondag | 26:ac5656aa35c7 | 46 | // Inverse Kinematica variables |
Marle | 25:bbef09ff226b | 47 | const double L1 = 0.208; // Hoogte van tafel tot joint 1 |
MarijkeZondag | 28:61d1372349c8 | 48 | //const double L2 = 0.288; // Hoogte van tafel tot joint 2 |
Marle | 25:bbef09ff226b | 49 | const double L3 = 0.212; // Lengte van de arm |
Marle | 25:bbef09ff226b | 50 | const double L4 = 0.089; // Afstand van achterkant base tot joint 1 |
MarijkeZondag | 28:61d1372349c8 | 51 | //const double L5 = 0.030; // Afstand van joint 1 naar joint 2 |
Marle | 25:bbef09ff226b | 52 | const double r_trans = 0.035; // Kan gebruikt worden om om te rekenen van translation naar shaft rotation |
Marle | 25:bbef09ff226b | 53 | |
Marle | 25:bbef09ff226b | 54 | // Variërende variabelen inverse kinematics: |
MarijkeZondag | 29:df10cb76ef26 | 55 | double q1ref = 0.0; // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is |
MarijkeZondag | 29:df10cb76ef26 | 56 | double q2ref = 0.0; // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is |
MarijkeZondag | 30:8191e8541a0a | 57 | double v_x; // Desired velocity end effector in x direction --> Determined by EMG signals |
MarijkeZondag | 30:8191e8541a0a | 58 | double v_y; // Desired velocity end effector in y direction --> Determined by EMG signals |
Marle | 25:bbef09ff226b | 59 | |
Marle | 25:bbef09ff226b | 60 | double Lq1; // Translatieafstand als gevolg van motor rotation joint 1 |
Marle | 25:bbef09ff226b | 61 | double Cq2; // Joint angle of the system (corrected for gear ratio 1:5) |
Marle | 25:bbef09ff226b | 62 | |
MarijkeZondag | 29:df10cb76ef26 | 63 | double q1_dot=0.0; // Benodigde hoeksnelheid van motor 1 om v_des te bereiken |
MarijkeZondag | 29:df10cb76ef26 | 64 | double q2_dot=0.0; // Benodigde hoeksnelheid van motor 2 om v_des te bereiken |
Marle | 25:bbef09ff226b | 65 | |
MarijkeZondag | 29:df10cb76ef26 | 66 | double q1_ii=0.0; // Reference signal for motorangle q1ref |
MarijkeZondag | 29:df10cb76ef26 | 67 | double q2_ii=0.0; // Reference signal for motorangle q2ref |
Marle | 25:bbef09ff226b | 68 | |
MarijkeZondag | 26:ac5656aa35c7 | 69 | //Variables PID controller |
MarijkeZondag | 26:ac5656aa35c7 | 70 | double PI = 3.14159; |
MarijkeZondag | 30:8191e8541a0a | 71 | double Kp1 = 17.5; //Motor 1 eerst 17.5 , nu 1 |
MarijkeZondag | 26:ac5656aa35c7 | 72 | double Ki1 = 1.02; |
MarijkeZondag | 31:481ad25a40c3 | 73 | double Kd1 = 15.5; |
MarijkeZondag | 26:ac5656aa35c7 | 74 | double encoder_radians1=0; |
MarijkeZondag | 29:df10cb76ef26 | 75 | double err_integral1 = 0; |
MarijkeZondag | 31:481ad25a40c3 | 76 | double err_prev1, err_prev2; |
MarijkeZondag | 31:481ad25a40c3 | 77 | double err1, err2; |
MarijkeZondag | 31:481ad25a40c3 | 78 | |
MarijkeZondag | 31:481ad25a40c3 | 79 | BiQuad LowPassFilterDer1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); |
MarijkeZondag | 31:481ad25a40c3 | 80 | BiQuad LowPassFilterDer2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); |
Marle | 25:bbef09ff226b | 81 | |
MarijkeZondag | 29:df10cb76ef26 | 82 | |
MarijkeZondag | 30:8191e8541a0a | 83 | double Kp2 = 17.5; //Motor 2 eerst 17.5, nu 1 |
MarijkeZondag | 26:ac5656aa35c7 | 84 | double Ki2 = 1.02; |
MarijkeZondag | 31:481ad25a40c3 | 85 | double Kd2 = 15.5; |
MarijkeZondag | 26:ac5656aa35c7 | 86 | double encoder_radians2=0; |
MarijkeZondag | 29:df10cb76ef26 | 87 | double err_integral2 = 0; |
MarijkeZondag | 31:481ad25a40c3 | 88 | double u1, u2; |
MarijkeZondag | 31:481ad25a40c3 | 89 | |
MarijkeZondag | 26:ac5656aa35c7 | 90 | |
MarijkeZondag | 28:61d1372349c8 | 91 | int start_control = 0; |
MarijkeZondag | 28:61d1372349c8 | 92 | double emg_cal = 1; |
MarijkeZondag | 27:fa493551be99 | 93 | |
MarijkeZondag | 26:ac5656aa35c7 | 94 | |
MarijkeZondag | 26:ac5656aa35c7 | 95 | //--------------Functions----------------------------------------------------------------------------------------------------------------------------// |
MarijkeZondag | 26:ac5656aa35c7 | 96 | |
MarijkeZondag | 26:ac5656aa35c7 | 97 | |
MarijkeZondag | 26:ac5656aa35c7 | 98 | //------------------ Filter EMG + Calibration EMG --------------------------------// |
MarijkeZondag | 26:ac5656aa35c7 | 99 | |
MarijkeZondag | 8:895d941a5910 | 100 | |
MarijkeZondag | 29:df10cb76ef26 | 101 | //---------PID controller motor 1 + motor control 1 & 2-----------------------------------------------------------// |
MarijkeZondag | 31:481ad25a40c3 | 102 | void PID_control1() |
MarijkeZondag | 28:61d1372349c8 | 103 | { |
MarijkeZondag | 31:481ad25a40c3 | 104 | //pc.printf("ik doe het, PDI \n\r"); |
MarijkeZondag | 28:61d1372349c8 | 105 | |
MarijkeZondag | 30:8191e8541a0a | 106 | // Proportional part: |
MarijkeZondag | 31:481ad25a40c3 | 107 | double u_k1 = Kp1 * err1; |
MarijkeZondag | 28:61d1372349c8 | 108 | |
MarijkeZondag | 30:8191e8541a0a | 109 | //Integral part |
MarijkeZondag | 31:481ad25a40c3 | 110 | err_integral1 = err_integral1 + err1 * T; |
MarijkeZondag | 31:481ad25a40c3 | 111 | double u_i1 = Ki1 * err_integral1; |
MarijkeZondag | 30:8191e8541a0a | 112 | |
MarijkeZondag | 30:8191e8541a0a | 113 | // Derivative part |
MarijkeZondag | 31:481ad25a40c3 | 114 | double err_derivative1 = (err1 - err_prev1)/T; |
MarijkeZondag | 31:481ad25a40c3 | 115 | double filtered_err_derivative1 = LowPassFilterDer1.step(err_derivative1); |
MarijkeZondag | 31:481ad25a40c3 | 116 | double u_d1 = Kd1 * filtered_err_derivative1; |
MarijkeZondag | 31:481ad25a40c3 | 117 | err_prev1 = err1; |
MarijkeZondag | 30:8191e8541a0a | 118 | |
MarijkeZondag | 30:8191e8541a0a | 119 | |
MarijkeZondag | 30:8191e8541a0a | 120 | // Sum all parts and return it |
MarijkeZondag | 31:481ad25a40c3 | 121 | u1 = u_k1 + u_i1 + u_d1; |
MarijkeZondag | 31:481ad25a40c3 | 122 | } |
MarijkeZondag | 31:481ad25a40c3 | 123 | |
MarijkeZondag | 31:481ad25a40c3 | 124 | void PID_control2() |
MarijkeZondag | 31:481ad25a40c3 | 125 | { |
MarijkeZondag | 31:481ad25a40c3 | 126 | //pc.printf("ik doe het, PDI \n\r"); |
MarijkeZondag | 31:481ad25a40c3 | 127 | |
MarijkeZondag | 31:481ad25a40c3 | 128 | // Proportional part: |
MarijkeZondag | 31:481ad25a40c3 | 129 | double u_k2 = Kp2 * err2; |
MarijkeZondag | 31:481ad25a40c3 | 130 | |
MarijkeZondag | 31:481ad25a40c3 | 131 | //Integral part |
MarijkeZondag | 31:481ad25a40c3 | 132 | err_integral2 = err_integral2 + err2 * T; |
MarijkeZondag | 31:481ad25a40c3 | 133 | double u_i2 = Ki2 * err_integral2; |
MarijkeZondag | 31:481ad25a40c3 | 134 | |
MarijkeZondag | 31:481ad25a40c3 | 135 | // Derivative part |
MarijkeZondag | 31:481ad25a40c3 | 136 | double err_derivative2 = (err2 - err_prev2)/T; |
MarijkeZondag | 31:481ad25a40c3 | 137 | double filtered_err_derivative2 = LowPassFilterDer2.step(err_derivative2); |
MarijkeZondag | 31:481ad25a40c3 | 138 | double u_d2 = Kd2 * filtered_err_derivative2; |
MarijkeZondag | 31:481ad25a40c3 | 139 | err_prev2 = err2; |
MarijkeZondag | 31:481ad25a40c3 | 140 | |
MarijkeZondag | 31:481ad25a40c3 | 141 | |
MarijkeZondag | 31:481ad25a40c3 | 142 | // Sum all parts and return it |
MarijkeZondag | 31:481ad25a40c3 | 143 | u2 = u_k2 + u_i2 + u_d2; |
MarijkeZondag | 31:481ad25a40c3 | 144 | } |
MarijkeZondag | 28:61d1372349c8 | 145 | |
MarijkeZondag | 30:8191e8541a0a | 146 | void engine_control1() //Engine 1 is translational engine, connected with left side pins |
MarijkeZondag | 28:61d1372349c8 | 147 | { |
MarijkeZondag | 31:481ad25a40c3 | 148 | //pc.printf("ik doe het, engine control 1\n\r"); |
MarijkeZondag | 31:481ad25a40c3 | 149 | encoder_radians1 = (double)encoder1.getPulses()*(2.0*PI)/8400.0; |
MarijkeZondag | 31:481ad25a40c3 | 150 | //pc.printf("encoder1 %f \n\r", (float)encoder1.getPulses()); |
MarijkeZondag | 31:481ad25a40c3 | 151 | //pc.printf("encoder_radians1 %f \n\r",(float) encoder_radians1); |
MarijkeZondag | 31:481ad25a40c3 | 152 | err1 = q1ref - encoder_radians1; |
MarijkeZondag | 31:481ad25a40c3 | 153 | //pc.printf("err1 = %f\n\r",err1); |
MarijkeZondag | 31:481ad25a40c3 | 154 | PID_control1(); //PID controller function call |
MarijkeZondag | 29:df10cb76ef26 | 155 | |
MarijkeZondag | 31:481ad25a40c3 | 156 | //pc.printf("u1 = %f\n\r",u1); |
MarijkeZondag | 30:8191e8541a0a | 157 | |
MarijkeZondag | 31:481ad25a40c3 | 158 | //if(encoder1.getPulses()<12000 && encoder1.getPulses()>-1) //limits translation in counts, eerst 12600 |
MarijkeZondag | 31:481ad25a40c3 | 159 | //{ |
MarijkeZondag | 30:8191e8541a0a | 160 | pwmpin1 = fabs(u1); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! |
MarijkeZondag | 30:8191e8541a0a | 161 | directionpin1.write(u1<0); |
MarijkeZondag | 31:481ad25a40c3 | 162 | //} |
MarijkeZondag | 31:481ad25a40c3 | 163 | //else |
MarijkeZondag | 31:481ad25a40c3 | 164 | // { |
MarijkeZondag | 31:481ad25a40c3 | 165 | // pwmpin1 = 0; |
MarijkeZondag | 31:481ad25a40c3 | 166 | // } |
MarijkeZondag | 28:61d1372349c8 | 167 | } |
MarijkeZondag | 28:61d1372349c8 | 168 | |
MarijkeZondag | 30:8191e8541a0a | 169 | void engine_control2() //Engine 2 is rotational engine, connected with right side wires |
MarijkeZondag | 28:61d1372349c8 | 170 | { |
MarijkeZondag | 31:481ad25a40c3 | 171 | encoder_radians2 = (float)encoder2.getPulses()*(2.0*PI)/8400.0; |
MarijkeZondag | 31:481ad25a40c3 | 172 | //pc.printf("encoder2 %f \n\r",(float)encoder2.getPulses()); |
MarijkeZondag | 31:481ad25a40c3 | 173 | //pc.printf("encoder_radians2 %f \n\r",(float)encoder_radians2); |
MarijkeZondag | 31:481ad25a40c3 | 174 | err2 = q2ref - encoder_radians2; |
MarijkeZondag | 31:481ad25a40c3 | 175 | //pc.printf("err2 = %f\n\r",err2); |
MarijkeZondag | 31:481ad25a40c3 | 176 | PID_control2(); //PID controller function call |
MarijkeZondag | 31:481ad25a40c3 | 177 | //pc.printf("u2 = %f\n\r",u2); |
MarijkeZondag | 30:8191e8541a0a | 178 | |
MarijkeZondag | 31:481ad25a40c3 | 179 | //if(encoder2.getPulses()<-5250 && encoder2.getPulses()>5250) //limits rotation, in counts |
MarijkeZondag | 31:481ad25a40c3 | 180 | // { |
MarijkeZondag | 30:8191e8541a0a | 181 | pwmpin2 = fabs(u2); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!! |
MarijkeZondag | 30:8191e8541a0a | 182 | directionpin2.write(u2>0); |
MarijkeZondag | 31:481ad25a40c3 | 183 | // } |
MarijkeZondag | 31:481ad25a40c3 | 184 | //else |
MarijkeZondag | 31:481ad25a40c3 | 185 | // { |
MarijkeZondag | 31:481ad25a40c3 | 186 | // pwmpin2 = 0; |
MarijkeZondag | 31:481ad25a40c3 | 187 | // } |
MarijkeZondag | 28:61d1372349c8 | 188 | } |
MarijkeZondag | 28:61d1372349c8 | 189 | |
MarijkeZondag | 28:61d1372349c8 | 190 | |
MarijkeZondag | 29:df10cb76ef26 | 191 | //------------------ Inversed Kinematics --------------------------------// |
MarijkeZondag | 29:df10cb76ef26 | 192 | |
MarijkeZondag | 29:df10cb76ef26 | 193 | |
MarijkeZondag | 31:481ad25a40c3 | 194 | void inverse_kinematics() |
Marle | 25:bbef09ff226b | 195 | { |
MarijkeZondag | 28:61d1372349c8 | 196 | |
MarijkeZondag | 31:481ad25a40c3 | 197 | //pc.printf("v_x is %f en v_y is %f\n\r",v_x, v_y); |
MarijkeZondag | 30:8191e8541a0a | 198 | |
Marle | 25:bbef09ff226b | 199 | Lq1 = q1ref*r_trans; |
Marle | 25:bbef09ff226b | 200 | Cq2 = q2ref/5.0; |
Marle | 25:bbef09ff226b | 201 | |
MarijkeZondag | 31:481ad25a40c3 | 202 | q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2)); //RKI systeem |
MarijkeZondag | 31:481ad25a40c3 | 203 | q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2)); // |
Marle | 25:bbef09ff226b | 204 | |
MarijkeZondag | 31:481ad25a40c3 | 205 | q1_ii = q1ref + (q1_dot/r_trans)*T; //Omgezet naar motorhoeken |
MarijkeZondag | 29:df10cb76ef26 | 206 | q2_ii = q2ref + (q2_dot*5.0)*T; |
Marle | 25:bbef09ff226b | 207 | |
Marle | 25:bbef09ff226b | 208 | q1ref = q1_ii; |
Marle | 25:bbef09ff226b | 209 | q2ref = q2_ii; |
MarijkeZondag | 27:fa493551be99 | 210 | |
MarijkeZondag | 30:8191e8541a0a | 211 | |
MarijkeZondag | 31:481ad25a40c3 | 212 | //pc.printf("q1ref is %f en q2ref is %f\n\r",q1ref, q2ref); |
MarijkeZondag | 30:8191e8541a0a | 213 | |
MarijkeZondag | 30:8191e8541a0a | 214 | |
MarijkeZondag | 28:61d1372349c8 | 215 | //start_control = 1; |
MarijkeZondag | 29:df10cb76ef26 | 216 | engine_control1(); |
MarijkeZondag | 28:61d1372349c8 | 217 | engine_control2(); |
Marle | 25:bbef09ff226b | 218 | } |
Marle | 25:bbef09ff226b | 219 | |
Marle | 25:bbef09ff226b | 220 | void v_des_calculate_qref() |
MarijkeZondag | 23:97a976a8f0fc | 221 | { |
MarijkeZondag | 29:df10cb76ef26 | 222 | double m1 = (potmeter1*2.0)-1.0; |
MarijkeZondag | 29:df10cb76ef26 | 223 | double m2 = (potmeter2*2.0)-1.0; |
MarijkeZondag | 29:df10cb76ef26 | 224 | |
MarijkeZondag | 29:df10cb76ef26 | 225 | if(m1>0.5) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor 1 will turn |
MarijkeZondag | 16:5f7196ddc77b | 226 | { |
MarijkeZondag | 30:8191e8541a0a | 227 | v_x = 0.5; //beweging in +x direction |
MarijkeZondag | 30:8191e8541a0a | 228 | v_y = 0.0; |
MarijkeZondag | 26:ac5656aa35c7 | 229 | ledr = 0; //red |
Marle | 25:bbef09ff226b | 230 | ledb = 1; |
Marle | 25:bbef09ff226b | 231 | ledg = 1; |
MarijkeZondag | 30:8191e8541a0a | 232 | |
MarijkeZondag | 16:5f7196ddc77b | 233 | } |
MarijkeZondag | 29:df10cb76ef26 | 234 | else if(m2>0.5) //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn |
MarijkeZondag | 16:5f7196ddc77b | 235 | { |
MarijkeZondag | 30:8191e8541a0a | 236 | v_y = 0.5; //beweging in +y direction |
MarijkeZondag | 30:8191e8541a0a | 237 | v_x = 0.0; |
MarijkeZondag | 26:ac5656aa35c7 | 238 | ledr = 1; //green |
Marle | 25:bbef09ff226b | 239 | ledb = 1; |
Marle | 25:bbef09ff226b | 240 | ledg = 0; |
MarijkeZondag | 24:6d63ad38fef7 | 241 | } |
MarijkeZondag | 29:df10cb76ef26 | 242 | |
MarijkeZondag | 29:df10cb76ef26 | 243 | else if(m1 < -0.5) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction |
MarijkeZondag | 24:6d63ad38fef7 | 244 | { |
MarijkeZondag | 30:8191e8541a0a | 245 | v_x = -0.5; |
MarijkeZondag | 29:df10cb76ef26 | 246 | v_y = 0; |
MarijkeZondag | 29:df10cb76ef26 | 247 | ledr = 1; //Blue |
MarijkeZondag | 29:df10cb76ef26 | 248 | ledb = 0; |
MarijkeZondag | 29:df10cb76ef26 | 249 | ledg = 1; |
MarijkeZondag | 29:df10cb76ef26 | 250 | } |
MarijkeZondag | 29:df10cb76ef26 | 251 | |
MarijkeZondag | 29:df10cb76ef26 | 252 | else if(m2 < -0.5) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction |
MarijkeZondag | 29:df10cb76ef26 | 253 | { |
MarijkeZondag | 29:df10cb76ef26 | 254 | v_x = 0; |
MarijkeZondag | 30:8191e8541a0a | 255 | v_y = -0.5; |
MarijkeZondag | 26:ac5656aa35c7 | 256 | ledr = 1; //Blue |
Marle | 25:bbef09ff226b | 257 | ledb = 0; |
Marle | 25:bbef09ff226b | 258 | ledg = 1; |
MarijkeZondag | 29:df10cb76ef26 | 259 | } |
MarijkeZondag | 29:df10cb76ef26 | 260 | |
MarijkeZondag | 30:8191e8541a0a | 261 | |
MarijkeZondag | 26:ac5656aa35c7 | 262 | else //If not higher than the threshold, motors will not turn at all |
Marle | 25:bbef09ff226b | 263 | { |
Marle | 25:bbef09ff226b | 264 | v_x = 0; |
Marle | 25:bbef09ff226b | 265 | v_y = 0; |
MarijkeZondag | 26:ac5656aa35c7 | 266 | ledr = 0; //white |
MarijkeZondag | 24:6d63ad38fef7 | 267 | ledb = 0; |
MarijkeZondag | 24:6d63ad38fef7 | 268 | ledg = 0; |
MarijkeZondag | 28:61d1372349c8 | 269 | //pwmpin1 = 0; |
MarijkeZondag | 28:61d1372349c8 | 270 | //pwmpin2 = 0; |
MarijkeZondag | 24:6d63ad38fef7 | 271 | } |
MarijkeZondag | 30:8191e8541a0a | 272 | |
MarijkeZondag | 31:481ad25a40c3 | 273 | inverse_kinematics(); //Call inverse kinematics function |
MarijkeZondag | 28:61d1372349c8 | 274 | |
MarijkeZondag | 26:ac5656aa35c7 | 275 | } |
MarijkeZondag | 26:ac5656aa35c7 | 276 | |
MarijkeZondag | 31:481ad25a40c3 | 277 | void printFunctie() |
MarijkeZondag | 31:481ad25a40c3 | 278 | { |
MarijkeZondag | 31:481ad25a40c3 | 279 | pc.printf("%f, %f\n\r", u1, u2); |
MarijkeZondag | 31:481ad25a40c3 | 280 | } |
MarijkeZondag | 31:481ad25a40c3 | 281 | |
MarijkeZondag | 26:ac5656aa35c7 | 282 | //------------------ Start main function --------------------------// |
MarijkeZondag | 26:ac5656aa35c7 | 283 | |
MarijkeZondag | 26:ac5656aa35c7 | 284 | |
MarijkeZondag | 31:481ad25a40c3 | 285 | |
MarijkeZondag | 23:97a976a8f0fc | 286 | int main() |
MarijkeZondag | 29:df10cb76ef26 | 287 | { |
MarijkeZondag | 23:97a976a8f0fc | 288 | pc.baud(115200); |
MarijkeZondag | 26:ac5656aa35c7 | 289 | pc.printf("Hello World!\r\n"); //Serial communication only works if hidscope is turned off. |
MarijkeZondag | 26:ac5656aa35c7 | 290 | pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz |
MarijkeZondag | 28:61d1372349c8 | 291 | |
MarijkeZondag | 28:61d1372349c8 | 292 | func_tick.attach(&v_des_calculate_qref,T2); //v_des determined every T |
MarijkeZondag | 31:481ad25a40c3 | 293 | printTicker.attach(&printFunctie, 0.01); |
MarijkeZondag | 30:8191e8541a0a | 294 | |
MarijkeZondag | 30:8191e8541a0a | 295 | |
MarijkeZondag | 30:8191e8541a0a | 296 | while(true) |
MarijkeZondag | 30:8191e8541a0a | 297 | { |
MarijkeZondag | 28:61d1372349c8 | 298 | //engine_control1_tick.attach(&engine_control1,T2); |
MarijkeZondag | 28:61d1372349c8 | 299 | //engine_control2_tick.attach(&engine_control2,T2); |
MarijkeZondag | 28:61d1372349c8 | 300 | |
MarijkeZondag | 28:61d1372349c8 | 301 | // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec. |
MarijkeZondag | 30:8191e8541a0a | 302 | |
MarijkeZondag | 31:481ad25a40c3 | 303 | //pc.printf("Encoder engine 1 %d\n\r",encoder2.getPulses()); |
MarijkeZondag | 31:481ad25a40c3 | 304 | //pc.printf("Encoder engine 2 %d\n\r",encoder1.getPulses()); |
MarijkeZondag | 31:481ad25a40c3 | 305 | //wait(0.1f); |
MarijkeZondag | 31:481ad25a40c3 | 306 | ; |
MarijkeZondag | 29:df10cb76ef26 | 307 | } |
vsluiter | 0:c8f15874531b | 308 | } |