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@29:df10cb76ef26, 2018-11-01 (annotated)

Committer:
MarijkeZondag
Date:
Thu Nov 01 08:21:31 2018 +0000
Revision:
29:df10cb76ef26
Parent:
28:61d1372349c8
Child:
30:8191e8541a0a
Potmeter motor aansturing met inversed kinematics en PID control, werkt nog niet denk ik

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