Esmee Buter / Mbed 2 deprecated Project_script_union_potmeter_1

Nieuwe kinematica + potmeter

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of Project_script_union_potmeter by Marijke Zondag

main.cpp@28:61d1372349c8, 2018-10-31 (annotated)

Committer:
MarijkeZondag
Date:
Wed Oct 31 19:14:56 2018 +0000
Revision:
28:61d1372349c8
Parent:
27:fa493551be99
Child:
29:df10cb76ef26
werkt nog niet;

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>
vsluiter 0:c8f15874531b 6
MarijkeZondag 22:5d956c93b3ae 7 //ATTENTION: set mBed to version 151
MarijkeZondag 22:5d956c93b3ae 8 // set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder)
MarijkeZondag 22:5d956c93b3ae 9 // set MODSERIAL to version 44
MarijkeZondag 22:5d956c93b3ae 10 // set HIDScope to version 7
MarijkeZondag 22:5d956c93b3ae 11 // set biquadFilter to version 7
MarijkeZondag 10:39ec51206c8b 12
MarijkeZondag 28:61d1372349c8 13 //AnalogIn potmeter1 (A0); //First raw EMG signal input
MarijkeZondag 28:61d1372349c8 14 //AnalogIn potmeter2 (A1); //Second raw EMG signal input
MarijkeZondag 22:5d956c93b3ae 15
MarijkeZondag 26:ac5656aa35c7 16 InterruptIn encoderA1 (D9);
MarijkeZondag 26:ac5656aa35c7 17 InterruptIn encoderB1 (D8);
MarijkeZondag 26:ac5656aa35c7 18 InterruptIn encoderA2 (D12);
MarijkeZondag 26:ac5656aa35c7 19 InterruptIn encoderB2 (D13);
MarijkeZondag 26:ac5656aa35c7 20
MarijkeZondag 26:ac5656aa35c7 21 InterruptIn button1 (D10);
MarijkeZondag 16:5f7196ddc77b 22 InterruptIn button2 (D11);
MarijkeZondag 6:f4bbb73f3989 23
MarijkeZondag 23:97a976a8f0fc 24 DigitalOut directionpin1 (D7);
MarijkeZondag 23:97a976a8f0fc 25 DigitalOut directionpin2 (D4);
MarijkeZondag 26:ac5656aa35c7 26
MarijkeZondag 23:97a976a8f0fc 27 PwmOut pwmpin1 (D6);
MarijkeZondag 23:97a976a8f0fc 28 PwmOut pwmpin2 (D5);
MarijkeZondag 22:5d956c93b3ae 29
MarijkeZondag 17:741798018c0d 30 DigitalOut ledr (LED_RED);
MarijkeZondag 17:741798018c0d 31 DigitalOut ledb (LED_BLUE);
MarijkeZondag 17:741798018c0d 32 DigitalOut ledg (LED_GREEN);
MarijkeZondag 13:a3d4b4daf5b4 33
MarijkeZondag 9:c722418997b5 34
MarijkeZondag 23:97a976a8f0fc 35 MODSERIAL pc(USBTX, USBRX); //Serial communication to see if the code works step by step, turn on if hidscope is off
MarijkeZondag 6:f4bbb73f3989 36
MarijkeZondag 23:97a976a8f0fc 37 //HIDScope scope( 6 ); //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered
vsluiter 0:c8f15874531b 38
MarijkeZondag 22:5d956c93b3ae 39 //Tickers
MarijkeZondag 28:61d1372349c8 40 Ticker func_tick;
MarijkeZondag 28:61d1372349c8 41 Ticker engine_control1_tick;
MarijkeZondag 28:61d1372349c8 42 Ticker engine_control2_tick;
MarijkeZondag 9:c722418997b5 43
MarijkeZondag 9:c722418997b5 44 //Global variables
MarijkeZondag 27:fa493551be99 45 const float T = 0.002f; //Ticker period Deze wordt ook gebruikt in de PID, moet die niet anders???
MarijkeZondag 28:61d1372349c8 46 const float T2 = 0.01f;
MarijkeZondag 10:39ec51206c8b 47
MarijkeZondag 26:ac5656aa35c7 48 // Inverse Kinematica variables
Marle 25:bbef09ff226b 49 const double L1 = 0.208; // Hoogte van tafel tot joint 1
MarijkeZondag 28:61d1372349c8 50 //const double L2 = 0.288; // Hoogte van tafel tot joint 2
Marle 25:bbef09ff226b 51 const double L3 = 0.212; // Lengte van de arm
Marle 25:bbef09ff226b 52 const double L4 = 0.089; // Afstand van achterkant base tot joint 1
MarijkeZondag 28:61d1372349c8 53 //const double L5 = 0.030; // Afstand van joint 1 naar joint 2
Marle 25:bbef09ff226b 54 const double r_trans = 0.035; // Kan gebruikt worden om om te rekenen van translation naar shaft rotation
Marle 25:bbef09ff226b 55
Marle 25:bbef09ff226b 56 // Variërende variabelen inverse kinematics:
Marle 25:bbef09ff226b 57 double q1ref = 0; // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is
Marle 25:bbef09ff226b 58 double q2ref = 0; // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is
Marle 25:bbef09ff226b 59 double v_x; // Desired velocity end effector in x direction --> Determined by EMG signals
Marle 25:bbef09ff226b 60 double v_y; // Desired velocity end effector in y direction --> Determined by EMG signals
Marle 25:bbef09ff226b 61
Marle 25:bbef09ff226b 62 double Lq1; // Translatieafstand als gevolg van motor rotation joint 1
Marle 25:bbef09ff226b 63 double Cq2; // Joint angle of the system (corrected for gear ratio 1:5)
Marle 25:bbef09ff226b 64
Marle 25:bbef09ff226b 65 double q1_dot; // Benodigde hoeksnelheid van motor 1 om v_des te bereiken
Marle 25:bbef09ff226b 66 double q2_dot; // Benodigde hoeksnelheid van motor 2 om v_des te bereiken
Marle 25:bbef09ff226b 67
Marle 25:bbef09ff226b 68 double q1_ii; // Reference signal for motorangle q1ref
Marle 25:bbef09ff226b 69 double q2_ii; // Reference signal for motorangle q2ref
Marle 25:bbef09ff226b 70
MarijkeZondag 26:ac5656aa35c7 71 //Variables PID controller
MarijkeZondag 26:ac5656aa35c7 72 double PI = 3.14159;
MarijkeZondag 28:61d1372349c8 73 double Kp1 = 5.0; //Motor 1 eerst 17.5 , nu 5
MarijkeZondag 26:ac5656aa35c7 74 double Ki1 = 1.02;
MarijkeZondag 26:ac5656aa35c7 75 double Kd1 = 23.2;
MarijkeZondag 26:ac5656aa35c7 76 double encoder1 = 0;
MarijkeZondag 26:ac5656aa35c7 77 double encoder_radians1=0;
Marle 25:bbef09ff226b 78
MarijkeZondag 28:61d1372349c8 79 double Kp2 = 5.0; //Motor 2 eerst 17.5, nu 5
MarijkeZondag 26:ac5656aa35c7 80 double Ki2 = 1.02;
MarijkeZondag 26:ac5656aa35c7 81 double Kd2 = 23.2;
MarijkeZondag 26:ac5656aa35c7 82 double encoder2 = 0;
MarijkeZondag 26:ac5656aa35c7 83 double encoder_radians2=0;
MarijkeZondag 26:ac5656aa35c7 84
MarijkeZondag 28:61d1372349c8 85 int start_control = 0;
MarijkeZondag 28:61d1372349c8 86
MarijkeZondag 28:61d1372349c8 87 //double potmeter1s = (potmeter1*2)-1.0f;
MarijkeZondag 28:61d1372349c8 88 //double potmeter2s = (potmeter2*2)-1.0f;
MarijkeZondag 28:61d1372349c8 89 double emg_cal = 1;
MarijkeZondag 27:fa493551be99 90
MarijkeZondag 26:ac5656aa35c7 91
MarijkeZondag 26:ac5656aa35c7 92 //--------------Functions----------------------------------------------------------------------------------------------------------------------------//
MarijkeZondag 26:ac5656aa35c7 93
MarijkeZondag 26:ac5656aa35c7 94
MarijkeZondag 26:ac5656aa35c7 95 //------------------ Encoder motor 1 --------------------------------//
MarijkeZondag 26:ac5656aa35c7 96
MarijkeZondag 26:ac5656aa35c7 97 void encoderA1_rise()
MarijkeZondag 26:ac5656aa35c7 98 {
MarijkeZondag 26:ac5656aa35c7 99 if(encoderB1==false)
MarijkeZondag 26:ac5656aa35c7 100 {
MarijkeZondag 26:ac5656aa35c7 101 encoder1++;
MarijkeZondag 26:ac5656aa35c7 102 }
MarijkeZondag 26:ac5656aa35c7 103 else
MarijkeZondag 26:ac5656aa35c7 104 {
MarijkeZondag 26:ac5656aa35c7 105 encoder1--;
MarijkeZondag 26:ac5656aa35c7 106 }
MarijkeZondag 26:ac5656aa35c7 107 }
MarijkeZondag 26:ac5656aa35c7 108
MarijkeZondag 26:ac5656aa35c7 109 void encoderA1_fall()
MarijkeZondag 26:ac5656aa35c7 110 {
MarijkeZondag 26:ac5656aa35c7 111 if(encoderB1==true)
MarijkeZondag 26:ac5656aa35c7 112 {
MarijkeZondag 26:ac5656aa35c7 113 encoder1++;
MarijkeZondag 26:ac5656aa35c7 114 }
MarijkeZondag 26:ac5656aa35c7 115 else
MarijkeZondag 26:ac5656aa35c7 116 {
MarijkeZondag 26:ac5656aa35c7 117 encoder1--;
MarijkeZondag 26:ac5656aa35c7 118 }
MarijkeZondag 26:ac5656aa35c7 119 }
MarijkeZondag 26:ac5656aa35c7 120
MarijkeZondag 26:ac5656aa35c7 121 void encoderB1_rise()
MarijkeZondag 26:ac5656aa35c7 122 {
MarijkeZondag 26:ac5656aa35c7 123 if(encoderA1==true)
MarijkeZondag 26:ac5656aa35c7 124 {
MarijkeZondag 26:ac5656aa35c7 125 encoder1++;
MarijkeZondag 26:ac5656aa35c7 126 }
MarijkeZondag 26:ac5656aa35c7 127 else
MarijkeZondag 26:ac5656aa35c7 128 {
MarijkeZondag 26:ac5656aa35c7 129 encoder1--;
MarijkeZondag 26:ac5656aa35c7 130 }
MarijkeZondag 26:ac5656aa35c7 131 }
MarijkeZondag 26:ac5656aa35c7 132
MarijkeZondag 26:ac5656aa35c7 133 void encoderB1_fall()
MarijkeZondag 26:ac5656aa35c7 134 {
MarijkeZondag 26:ac5656aa35c7 135 if(encoderA1==false)
MarijkeZondag 26:ac5656aa35c7 136 {
MarijkeZondag 26:ac5656aa35c7 137 encoder1++;
MarijkeZondag 26:ac5656aa35c7 138 }
MarijkeZondag 26:ac5656aa35c7 139 else
MarijkeZondag 26:ac5656aa35c7 140 {
MarijkeZondag 26:ac5656aa35c7 141 encoder1--;
MarijkeZondag 26:ac5656aa35c7 142 }
MarijkeZondag 26:ac5656aa35c7 143 }
MarijkeZondag 26:ac5656aa35c7 144
MarijkeZondag 26:ac5656aa35c7 145 void encoder_count1()
MarijkeZondag 26:ac5656aa35c7 146 {
MarijkeZondag 26:ac5656aa35c7 147 encoderA1.rise(&encoderA1_rise);
MarijkeZondag 26:ac5656aa35c7 148 encoderA1.fall(&encoderA1_fall);
MarijkeZondag 26:ac5656aa35c7 149 encoderB1.rise(&encoderB1_rise);
MarijkeZondag 26:ac5656aa35c7 150 encoderB1.fall(&encoderB1_fall);
MarijkeZondag 26:ac5656aa35c7 151 }
MarijkeZondag 26:ac5656aa35c7 152
MarijkeZondag 26:ac5656aa35c7 153 //------------------ Encoder motor 2 --------------------------------//
MarijkeZondag 26:ac5656aa35c7 154
MarijkeZondag 26:ac5656aa35c7 155 void encoderA2_rise()
MarijkeZondag 26:ac5656aa35c7 156 {
MarijkeZondag 26:ac5656aa35c7 157 if(encoderB2==false)
MarijkeZondag 26:ac5656aa35c7 158 {
MarijkeZondag 26:ac5656aa35c7 159 encoder2++;
MarijkeZondag 26:ac5656aa35c7 160 }
MarijkeZondag 26:ac5656aa35c7 161 else
MarijkeZondag 26:ac5656aa35c7 162 {
MarijkeZondag 26:ac5656aa35c7 163 encoder2--;
MarijkeZondag 26:ac5656aa35c7 164 }
MarijkeZondag 26:ac5656aa35c7 165 }
MarijkeZondag 12:eaed305a76c3 166
MarijkeZondag 26:ac5656aa35c7 167 void encoderA2_fall()
MarijkeZondag 26:ac5656aa35c7 168 {
MarijkeZondag 26:ac5656aa35c7 169 if(encoderB2==true)
MarijkeZondag 26:ac5656aa35c7 170 {
MarijkeZondag 26:ac5656aa35c7 171 encoder2++;
MarijkeZondag 26:ac5656aa35c7 172 }
MarijkeZondag 26:ac5656aa35c7 173 else
MarijkeZondag 26:ac5656aa35c7 174 {
MarijkeZondag 26:ac5656aa35c7 175 encoder2--;
MarijkeZondag 26:ac5656aa35c7 176 }
MarijkeZondag 26:ac5656aa35c7 177 }
MarijkeZondag 26:ac5656aa35c7 178
MarijkeZondag 26:ac5656aa35c7 179 void encoderB2_rise()
MarijkeZondag 26:ac5656aa35c7 180 {
MarijkeZondag 26:ac5656aa35c7 181 if(encoderA2==true)
MarijkeZondag 26:ac5656aa35c7 182 {
MarijkeZondag 26:ac5656aa35c7 183 encoder2++;
MarijkeZondag 26:ac5656aa35c7 184 }
MarijkeZondag 26:ac5656aa35c7 185 else
MarijkeZondag 26:ac5656aa35c7 186 {
MarijkeZondag 26:ac5656aa35c7 187 encoder2--;
MarijkeZondag 26:ac5656aa35c7 188 }
MarijkeZondag 26:ac5656aa35c7 189 }
MarijkeZondag 26:ac5656aa35c7 190
MarijkeZondag 26:ac5656aa35c7 191 void encoderB2_fall()
MarijkeZondag 26:ac5656aa35c7 192 {
MarijkeZondag 26:ac5656aa35c7 193 if(encoderA2==false)
MarijkeZondag 26:ac5656aa35c7 194 {
MarijkeZondag 26:ac5656aa35c7 195 encoder2++;
MarijkeZondag 26:ac5656aa35c7 196 }
MarijkeZondag 26:ac5656aa35c7 197 else
MarijkeZondag 26:ac5656aa35c7 198 {
MarijkeZondag 26:ac5656aa35c7 199 encoder2--;
MarijkeZondag 26:ac5656aa35c7 200 }
MarijkeZondag 26:ac5656aa35c7 201 }
MarijkeZondag 26:ac5656aa35c7 202
MarijkeZondag 26:ac5656aa35c7 203 void encoder_count2()
MarijkeZondag 26:ac5656aa35c7 204 {
MarijkeZondag 26:ac5656aa35c7 205 encoderA2.rise(&encoderA2_rise);
MarijkeZondag 26:ac5656aa35c7 206 encoderA2.fall(&encoderA2_fall);
MarijkeZondag 26:ac5656aa35c7 207 encoderB2.rise(&encoderB2_rise);
MarijkeZondag 26:ac5656aa35c7 208 encoderB2.fall(&encoderB2_fall);
MarijkeZondag 26:ac5656aa35c7 209 }
MarijkeZondag 26:ac5656aa35c7 210
MarijkeZondag 26:ac5656aa35c7 211 //------------------ Filter EMG + Calibration EMG --------------------------------//
MarijkeZondag 26:ac5656aa35c7 212
MarijkeZondag 8:895d941a5910 213
MarijkeZondag 26:ac5656aa35c7 214 //------------------ Inversed Kinematics --------------------------------//
MarijkeZondag 8:895d941a5910 215
MarijkeZondag 28:61d1372349c8 216
MarijkeZondag 28:61d1372349c8 217 //---------PID controller motor 1 + start motor 1 -----------------------------------------------------------//
MarijkeZondag 28:61d1372349c8 218 double PID_controller1(double err1)
MarijkeZondag 28:61d1372349c8 219 {
MarijkeZondag 28:61d1372349c8 220 pc.printf("ik doe het, PDI 1\n\r");
MarijkeZondag 28:61d1372349c8 221
MarijkeZondag 28:61d1372349c8 222 static double err_integral1 = 0;
MarijkeZondag 28:61d1372349c8 223 static double err_prev1 = err1; // initialization with this value only done once!
MarijkeZondag 28:61d1372349c8 224
MarijkeZondag 28:61d1372349c8 225 static BiQuad LowPassFilter1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
MarijkeZondag 28:61d1372349c8 226
MarijkeZondag 28:61d1372349c8 227 // Proportional part:
MarijkeZondag 28:61d1372349c8 228 double u_k1 = Kp1 * err1;
MarijkeZondag 28:61d1372349c8 229
MarijkeZondag 28:61d1372349c8 230 /* Integral part
MarijkeZondag 28:61d1372349c8 231 err_integral1 = err_integral1 + err1 * T;
MarijkeZondag 28:61d1372349c8 232 double u_i1 = Ki1 * err_integral1;
MarijkeZondag 28:61d1372349c8 233
MarijkeZondag 28:61d1372349c8 234 // Derivative part
MarijkeZondag 28:61d1372349c8 235 double err_derivative1 = (err1 - err_prev1)/T;
MarijkeZondag 28:61d1372349c8 236 double filtered_err_derivative1 = LowPassFilter1.step(err_derivative1);
MarijkeZondag 28:61d1372349c8 237 double u_d1 = Kd1 * filtered_err_derivative1;
MarijkeZondag 28:61d1372349c8 238 err_prev1 = err1;
MarijkeZondag 28:61d1372349c8 239 */
MarijkeZondag 28:61d1372349c8 240
MarijkeZondag 28:61d1372349c8 241 // Sum all parts and return it
MarijkeZondag 28:61d1372349c8 242 return u_k1+0; //+ u_i1 + u_d1;
MarijkeZondag 28:61d1372349c8 243 }
MarijkeZondag 28:61d1372349c8 244
MarijkeZondag 28:61d1372349c8 245 void start_your_engines1(double u1)
MarijkeZondag 28:61d1372349c8 246 {
MarijkeZondag 28:61d1372349c8 247 pc.printf("ik doe het, engine start 1\n\r");
MarijkeZondag 28:61d1372349c8 248
MarijkeZondag 28:61d1372349c8 249 if(encoder1<5250 && encoder1>-5250) //limits rotation, in counts
MarijkeZondag 28:61d1372349c8 250 {
MarijkeZondag 28:61d1372349c8 251 pwmpin1 = 1; //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
MarijkeZondag 28:61d1372349c8 252 directionpin1.write(0);
MarijkeZondag 28:61d1372349c8 253 }
MarijkeZondag 28:61d1372349c8 254 else
MarijkeZondag 28:61d1372349c8 255 {
MarijkeZondag 28:61d1372349c8 256 pwmpin1 = 0;
MarijkeZondag 28:61d1372349c8 257 }
MarijkeZondag 28:61d1372349c8 258 }
MarijkeZondag 28:61d1372349c8 259
MarijkeZondag 28:61d1372349c8 260 void engine_control1() //Engine 1 is rotational engine, connected with left side pins
MarijkeZondag 28:61d1372349c8 261 {
MarijkeZondag 28:61d1372349c8 262 //while(start_control == 1)
MarijkeZondag 28:61d1372349c8 263 //{
MarijkeZondag 28:61d1372349c8 264 pc.printf("ik doe het, engine control 1\n\r");
MarijkeZondag 28:61d1372349c8 265 encoder_radians1 = encoder1*(2*PI)/8400;
MarijkeZondag 28:61d1372349c8 266 double err1 = q1ref - encoder_radians1;
MarijkeZondag 28:61d1372349c8 267 double u1 = PID_controller1(err1); //PID controller function call
MarijkeZondag 28:61d1372349c8 268 start_your_engines1(u1);
MarijkeZondag 28:61d1372349c8 269
MarijkeZondag 28:61d1372349c8 270 // break;
MarijkeZondag 28:61d1372349c8 271 //}
MarijkeZondag 28:61d1372349c8 272 }
MarijkeZondag 28:61d1372349c8 273
MarijkeZondag 28:61d1372349c8 274
MarijkeZondag 28:61d1372349c8 275
MarijkeZondag 28:61d1372349c8 276 //---------PID controller motor 2 + start motor 2 -----------------------------------------------------------//
MarijkeZondag 28:61d1372349c8 277 double PID_controller2(double err2)
MarijkeZondag 28:61d1372349c8 278 {
MarijkeZondag 28:61d1372349c8 279 pc.printf("ik doe het, PDI 2\n\r");
MarijkeZondag 28:61d1372349c8 280
MarijkeZondag 28:61d1372349c8 281 static double err_integral2 = 0;
MarijkeZondag 28:61d1372349c8 282 static double err_prev2 = err2; // initialization with this value only done once!
MarijkeZondag 28:61d1372349c8 283
MarijkeZondag 28:61d1372349c8 284 static BiQuad LowPassFilter2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
MarijkeZondag 28:61d1372349c8 285
MarijkeZondag 28:61d1372349c8 286 // Proportional part:
MarijkeZondag 28:61d1372349c8 287 double u_k2 = Kp2 * err2;
MarijkeZondag 28:61d1372349c8 288
MarijkeZondag 28:61d1372349c8 289 /* Integral part
MarijkeZondag 28:61d1372349c8 290 err_integral2 = err_integral2 + err2 * T;
MarijkeZondag 28:61d1372349c8 291 double u_i2 = Ki2 * err_integral2;
MarijkeZondag 28:61d1372349c8 292
MarijkeZondag 28:61d1372349c8 293 // Derivative part
MarijkeZondag 28:61d1372349c8 294 double err_derivative2 = (err2 - err_prev2)/T;
MarijkeZondag 28:61d1372349c8 295 double filtered_err_derivative2 = LowPassFilter2.step(err_derivative2);
MarijkeZondag 28:61d1372349c8 296 double u_d2 = Kd2 * filtered_err_derivative2;
MarijkeZondag 28:61d1372349c8 297 err_prev2 = err2;
MarijkeZondag 28:61d1372349c8 298 */
MarijkeZondag 28:61d1372349c8 299
MarijkeZondag 28:61d1372349c8 300 // Sum all parts and return it
MarijkeZondag 28:61d1372349c8 301 return u_k2+0; //+ u_i2 + u_d2;
MarijkeZondag 28:61d1372349c8 302 }
MarijkeZondag 28:61d1372349c8 303
MarijkeZondag 28:61d1372349c8 304 void start_your_engines2(double u2)
MarijkeZondag 28:61d1372349c8 305 {
MarijkeZondag 28:61d1372349c8 306 pc.printf("ik doe het, engine start 2\n\r");
MarijkeZondag 28:61d1372349c8 307
MarijkeZondag 28:61d1372349c8 308 if(encoder2<12600 && encoder2>-1) //limits translation in counts
MarijkeZondag 28:61d1372349c8 309 {
MarijkeZondag 28:61d1372349c8 310 pwmpin2 = 1; //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
MarijkeZondag 28:61d1372349c8 311 directionpin2.write(0);
MarijkeZondag 28:61d1372349c8 312 }
MarijkeZondag 28:61d1372349c8 313 else
MarijkeZondag 28:61d1372349c8 314 {
MarijkeZondag 28:61d1372349c8 315 pwmpin2 = 0;
MarijkeZondag 28:61d1372349c8 316 }
MarijkeZondag 28:61d1372349c8 317
MarijkeZondag 28:61d1372349c8 318 }
MarijkeZondag 28:61d1372349c8 319
MarijkeZondag 28:61d1372349c8 320 void engine_control2() //Engine 2 is translational engine, connected with right side wires
MarijkeZondag 28:61d1372349c8 321 {
MarijkeZondag 28:61d1372349c8 322 pc.printf("ik doe het, engine control 2\n\r");
MarijkeZondag 28:61d1372349c8 323
MarijkeZondag 28:61d1372349c8 324 encoder_radians2 = encoder2*(2*PI)/8400;
MarijkeZondag 28:61d1372349c8 325 double err2 = q2ref - encoder_radians2;
MarijkeZondag 28:61d1372349c8 326 double u2 = PID_controller2(err2); //PID controller function call
MarijkeZondag 28:61d1372349c8 327 start_your_engines2(u2); //Call start_your_engines function
MarijkeZondag 28:61d1372349c8 328 }
MarijkeZondag 28:61d1372349c8 329
MarijkeZondag 28:61d1372349c8 330
Marle 25:bbef09ff226b 331 void inverse_kinematics()
Marle 25:bbef09ff226b 332 {
MarijkeZondag 28:61d1372349c8 333
MarijkeZondag 28:61d1372349c8 334 pc.printf("ik doe het, inverse kinematics\n\r");
Marle 25:bbef09ff226b 335 Lq1 = q1ref*r_trans;
Marle 25:bbef09ff226b 336 Cq2 = q2ref/5.0;
Marle 25:bbef09ff226b 337
Marle 25:bbef09ff226b 338 q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2));
Marle 25:bbef09ff226b 339 q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2));
Marle 25:bbef09ff226b 340
Marle 25:bbef09ff226b 341 q1_ii = q1ref + q1_dot*T;
Marle 25:bbef09ff226b 342 q2_ii = q2ref + q2_dot*T;
Marle 25:bbef09ff226b 343
Marle 25:bbef09ff226b 344 q1ref = q1_ii;
Marle 25:bbef09ff226b 345 q2ref = q2_ii;
MarijkeZondag 27:fa493551be99 346
MarijkeZondag 28:61d1372349c8 347 //start_control = 1;
MarijkeZondag 28:61d1372349c8 348 engine_control1();
MarijkeZondag 28:61d1372349c8 349 engine_control2();
Marle 25:bbef09ff226b 350 }
Marle 25:bbef09ff226b 351
Marle 25:bbef09ff226b 352 void v_des_calculate_qref()
MarijkeZondag 23:97a976a8f0fc 353 {
MarijkeZondag 28:61d1372349c8 354 if(button1==0) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor 1 will turn
MarijkeZondag 16:5f7196ddc77b 355 {
MarijkeZondag 28:61d1372349c8 356 v_x = 0.5f; //beweging in +x direction
MarijkeZondag 26:ac5656aa35c7 357 ledr = 0; //red
Marle 25:bbef09ff226b 358 ledb = 1;
Marle 25:bbef09ff226b 359 ledg = 1;
MarijkeZondag 16:5f7196ddc77b 360 }
MarijkeZondag 28:61d1372349c8 361 else if(button2==0) //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn
MarijkeZondag 16:5f7196ddc77b 362 {
MarijkeZondag 28:61d1372349c8 363 v_y = 0.5f; //beweging in +y direction
MarijkeZondag 26:ac5656aa35c7 364 ledr = 1; //green
Marle 25:bbef09ff226b 365 ledb = 1;
Marle 25:bbef09ff226b 366 ledg = 0;
MarijkeZondag 24:6d63ad38fef7 367 }
MarijkeZondag 28:61d1372349c8 368 /*
MarijkeZondag 28:61d1372349c8 369 else if(button1==0 && button2==0) //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction
MarijkeZondag 24:6d63ad38fef7 370 {
MarijkeZondag 28:61d1372349c8 371 v_x = -0.5f;
MarijkeZondag 28:61d1372349c8 372 v_y = -0.5f;
MarijkeZondag 26:ac5656aa35c7 373 ledr = 1; //Blue
Marle 25:bbef09ff226b 374 ledb = 0;
Marle 25:bbef09ff226b 375 ledg = 1;
Marle 25:bbef09ff226b 376 }
MarijkeZondag 28:61d1372349c8 377 */
MarijkeZondag 26:ac5656aa35c7 378 else //If not higher than the threshold, motors will not turn at all
Marle 25:bbef09ff226b 379 {
Marle 25:bbef09ff226b 380 v_x = 0;
Marle 25:bbef09ff226b 381 v_y = 0;
MarijkeZondag 26:ac5656aa35c7 382 ledr = 0; //white
MarijkeZondag 24:6d63ad38fef7 383 ledb = 0;
MarijkeZondag 24:6d63ad38fef7 384 ledg = 0;
MarijkeZondag 28:61d1372349c8 385 //pwmpin1 = 0;
MarijkeZondag 28:61d1372349c8 386 //pwmpin2 = 0;
MarijkeZondag 24:6d63ad38fef7 387 }
MarijkeZondag 27:fa493551be99 388
MarijkeZondag 27:fa493551be99 389 inverse_kinematics(); //Call inverse kinematics function
MarijkeZondag 28:61d1372349c8 390
MarijkeZondag 26:ac5656aa35c7 391 }
MarijkeZondag 26:ac5656aa35c7 392
MarijkeZondag 26:ac5656aa35c7 393 //------------------ Start main function --------------------------//
MarijkeZondag 26:ac5656aa35c7 394
MarijkeZondag 26:ac5656aa35c7 395
MarijkeZondag 23:97a976a8f0fc 396 int main()
MarijkeZondag 23:97a976a8f0fc 397 {
MarijkeZondag 23:97a976a8f0fc 398 pc.baud(115200);
MarijkeZondag 26:ac5656aa35c7 399 pc.printf("Hello World!\r\n"); //Serial communication only works if hidscope is turned off.
MarijkeZondag 26:ac5656aa35c7 400 pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
MarijkeZondag 28:61d1372349c8 401
MarijkeZondag 28:61d1372349c8 402 func_tick.attach(&v_des_calculate_qref,T2); //v_des determined every T
MarijkeZondag 28:61d1372349c8 403 //engine_control1_tick.attach(&engine_control1,T2);
MarijkeZondag 28:61d1372349c8 404 //engine_control2_tick.attach(&engine_control2,T2);
MarijkeZondag 28:61d1372349c8 405
MarijkeZondag 28:61d1372349c8 406 // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec.
MarijkeZondag 28:61d1372349c8 407
MarijkeZondag 10:39ec51206c8b 408
MarijkeZondag 23:97a976a8f0fc 409 while(true)
MarijkeZondag 28:61d1372349c8 410 {;}
vsluiter 0:c8f15874531b 411 }