Marijke Zondag / Mbed 2 deprecated Project_script_union_potmeter

werkt nog niet

Dependencies:   HIDScope MODSERIAL biquadFilter mbed QEI

Fork of Project_script_union by Marijke Zondag

main.cpp@30:8191e8541a0a, 2018-11-01 (annotated)

Committer:
MarijkeZondag
Date:
Thu Nov 01 12:46:20 2018 +0000
Revision:
30:8191e8541a0a
Parent:
29:df10cb76ef26
Child:
31:481ad25a40c3
aangepast 13:46 uur

Who changed what in which revision?

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