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

Committer:
MarijkeZondag
Date:
Thu Nov 01 16:09:00 2018 +0000
Revision:
33:7b94346bcead
Parent:
32:137d0f27e5a8
Child:
34:bece2820b63b
PID controller smoother

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 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 33:7b94346bcead 43 const float T = 0.002f; //Ticker period Deze wordt ook gebruikt in de PID, moet die niet anders???
MarijkeZondag 33:7b94346bcead 44 const float T2 = 0.002f;
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 33:7b94346bcead 71 double Kp1 = 1.0; //Motor 1 eerst 17.5 , nu 1
MarijkeZondag 33:7b94346bcead 72 double Ki1 = 1.0;
MarijkeZondag 33:7b94346bcead 73 double Kd1 = 23.0;
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);
MarijkeZondag 33:7b94346bcead 81 //BiQuad LowPassFilterDer1( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 );
MarijkeZondag 33:7b94346bcead 82 //BiQuad LowPassFilterDer2( 1.12160e-01, 1.12160e-01, 0.00000e+00, -7.75680e-01, 0.00000e+00 );
Marle 25:bbef09ff226b 83
MarijkeZondag 29:df10cb76ef26 84
MarijkeZondag 33:7b94346bcead 85 double Kp2 = 1.0; //Motor 2 eerst 17.5, nu 1
MarijkeZondag 33:7b94346bcead 86 double Ki2 = 1.0;
MarijkeZondag 33:7b94346bcead 87 double Kd2 = 23.0;
MarijkeZondag 26:ac5656aa35c7 88 double encoder_radians2=0;
MarijkeZondag 29:df10cb76ef26 89 double err_integral2 = 0;
MarijkeZondag 31:481ad25a40c3 90 double u1, u2;
MarijkeZondag 31:481ad25a40c3 91
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 31:481ad25a40c3 104 void PID_control1()
MarijkeZondag 28:61d1372349c8 105 {
MarijkeZondag 31:481ad25a40c3 106 //pc.printf("ik doe het, PDI \n\r");
MarijkeZondag 28:61d1372349c8 107
MarijkeZondag 30:8191e8541a0a 108 // Proportional part:
MarijkeZondag 31:481ad25a40c3 109 double u_k1 = Kp1 * err1;
MarijkeZondag 28:61d1372349c8 110
MarijkeZondag 30:8191e8541a0a 111 //Integral part
MarijkeZondag 31:481ad25a40c3 112 err_integral1 = err_integral1 + err1 * T;
MarijkeZondag 31:481ad25a40c3 113 double u_i1 = Ki1 * err_integral1;
MarijkeZondag 30:8191e8541a0a 114
MarijkeZondag 30:8191e8541a0a 115 // Derivative part
MarijkeZondag 31:481ad25a40c3 116 double err_derivative1 = (err1 - err_prev1)/T;
MarijkeZondag 31:481ad25a40c3 117 double filtered_err_derivative1 = LowPassFilterDer1.step(err_derivative1);
MarijkeZondag 31:481ad25a40c3 118 double u_d1 = Kd1 * filtered_err_derivative1;
MarijkeZondag 31:481ad25a40c3 119 err_prev1 = err1;
MarijkeZondag 30:8191e8541a0a 120
MarijkeZondag 30:8191e8541a0a 121
MarijkeZondag 30:8191e8541a0a 122 // Sum all parts and return it
MarijkeZondag 31:481ad25a40c3 123 u1 = u_k1 + u_i1 + u_d1;
MarijkeZondag 31:481ad25a40c3 124 }
MarijkeZondag 31:481ad25a40c3 125
MarijkeZondag 31:481ad25a40c3 126 void PID_control2()
MarijkeZondag 31:481ad25a40c3 127 {
MarijkeZondag 31:481ad25a40c3 128 //pc.printf("ik doe het, PDI \n\r");
MarijkeZondag 31:481ad25a40c3 129
MarijkeZondag 31:481ad25a40c3 130 // Proportional part:
MarijkeZondag 31:481ad25a40c3 131 double u_k2 = Kp2 * err2;
MarijkeZondag 31:481ad25a40c3 132
MarijkeZondag 31:481ad25a40c3 133 //Integral part
MarijkeZondag 31:481ad25a40c3 134 err_integral2 = err_integral2 + err2 * T;
MarijkeZondag 31:481ad25a40c3 135 double u_i2 = Ki2 * err_integral2;
MarijkeZondag 31:481ad25a40c3 136
MarijkeZondag 31:481ad25a40c3 137 // Derivative part
MarijkeZondag 31:481ad25a40c3 138 double err_derivative2 = (err2 - err_prev2)/T;
MarijkeZondag 31:481ad25a40c3 139 double filtered_err_derivative2 = LowPassFilterDer2.step(err_derivative2);
MarijkeZondag 31:481ad25a40c3 140 double u_d2 = Kd2 * filtered_err_derivative2;
MarijkeZondag 31:481ad25a40c3 141 err_prev2 = err2;
MarijkeZondag 31:481ad25a40c3 142
MarijkeZondag 31:481ad25a40c3 143
MarijkeZondag 31:481ad25a40c3 144 // Sum all parts and return it
MarijkeZondag 31:481ad25a40c3 145 u2 = u_k2 + u_i2 + u_d2;
MarijkeZondag 31:481ad25a40c3 146 }
MarijkeZondag 28:61d1372349c8 147
MarijkeZondag 30:8191e8541a0a 148 void engine_control1() //Engine 1 is translational engine, connected with left side pins
MarijkeZondag 28:61d1372349c8 149 {
MarijkeZondag 31:481ad25a40c3 150 //pc.printf("ik doe het, engine control 1\n\r");
MarijkeZondag 31:481ad25a40c3 151 encoder_radians1 = (double)encoder1.getPulses()*(2.0*PI)/8400.0;
MarijkeZondag 31:481ad25a40c3 152 //pc.printf("encoder1 %f \n\r", (float)encoder1.getPulses());
MarijkeZondag 31:481ad25a40c3 153 //pc.printf("encoder_radians1 %f \n\r",(float) encoder_radians1);
MarijkeZondag 31:481ad25a40c3 154 err1 = q1ref - encoder_radians1;
MarijkeZondag 31:481ad25a40c3 155 //pc.printf("err1 = %f\n\r",err1);
MarijkeZondag 31:481ad25a40c3 156 PID_control1(); //PID controller function call
MarijkeZondag 29:df10cb76ef26 157
MarijkeZondag 31:481ad25a40c3 158 //pc.printf("u1 = %f\n\r",u1);
MarijkeZondag 30:8191e8541a0a 159
MarijkeZondag 31:481ad25a40c3 160 //if(encoder1.getPulses()<12000 && encoder1.getPulses()>-1) //limits translation in counts, eerst 12600
MarijkeZondag 31:481ad25a40c3 161 //{
MarijkeZondag 30:8191e8541a0a 162 pwmpin1 = fabs(u1); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
MarijkeZondag 30:8191e8541a0a 163 directionpin1.write(u1<0);
MarijkeZondag 31:481ad25a40c3 164 //}
MarijkeZondag 31:481ad25a40c3 165 //else
MarijkeZondag 31:481ad25a40c3 166 // {
MarijkeZondag 31:481ad25a40c3 167 // pwmpin1 = 0;
MarijkeZondag 31:481ad25a40c3 168 // }
MarijkeZondag 28:61d1372349c8 169 }
MarijkeZondag 28:61d1372349c8 170
MarijkeZondag 30:8191e8541a0a 171 void engine_control2() //Engine 2 is rotational engine, connected with right side wires
MarijkeZondag 28:61d1372349c8 172 {
MarijkeZondag 31:481ad25a40c3 173 encoder_radians2 = (float)encoder2.getPulses()*(2.0*PI)/8400.0;
MarijkeZondag 31:481ad25a40c3 174 //pc.printf("encoder2 %f \n\r",(float)encoder2.getPulses());
MarijkeZondag 31:481ad25a40c3 175 //pc.printf("encoder_radians2 %f \n\r",(float)encoder_radians2);
MarijkeZondag 31:481ad25a40c3 176 err2 = q2ref - encoder_radians2;
MarijkeZondag 31:481ad25a40c3 177 //pc.printf("err2 = %f\n\r",err2);
MarijkeZondag 31:481ad25a40c3 178 PID_control2(); //PID controller function call
MarijkeZondag 31:481ad25a40c3 179 //pc.printf("u2 = %f\n\r",u2);
MarijkeZondag 30:8191e8541a0a 180
MarijkeZondag 31:481ad25a40c3 181 //if(encoder2.getPulses()<-5250 && encoder2.getPulses()>5250) //limits rotation, in counts
MarijkeZondag 31:481ad25a40c3 182 // {
MarijkeZondag 30:8191e8541a0a 183 pwmpin2 = fabs(u2); //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
MarijkeZondag 30:8191e8541a0a 184 directionpin2.write(u2>0);
MarijkeZondag 31:481ad25a40c3 185 // }
MarijkeZondag 31:481ad25a40c3 186 //else
MarijkeZondag 31:481ad25a40c3 187 // {
MarijkeZondag 31:481ad25a40c3 188 // pwmpin2 = 0;
MarijkeZondag 31:481ad25a40c3 189 // }
MarijkeZondag 28:61d1372349c8 190 }
MarijkeZondag 28:61d1372349c8 191
MarijkeZondag 28:61d1372349c8 192
MarijkeZondag 29:df10cb76ef26 193 //------------------ Inversed Kinematics --------------------------------//
MarijkeZondag 29:df10cb76ef26 194
MarijkeZondag 29:df10cb76ef26 195
MarijkeZondag 31:481ad25a40c3 196 void inverse_kinematics()
Marle 25:bbef09ff226b 197 {
MarijkeZondag 28:61d1372349c8 198
MarijkeZondag 31:481ad25a40c3 199 //pc.printf("v_x is %f en v_y is %f\n\r",v_x, v_y);
MarijkeZondag 30:8191e8541a0a 200
Marle 25:bbef09ff226b 201 Lq1 = q1ref*r_trans;
Marle 25:bbef09ff226b 202 Cq2 = q2ref/5.0;
Marle 25:bbef09ff226b 203
MarijkeZondag 31:481ad25a40c3 204 q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2)); //RKI systeem
MarijkeZondag 31:481ad25a40c3 205 q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2)); //
Marle 25:bbef09ff226b 206
MarijkeZondag 31:481ad25a40c3 207 q1_ii = q1ref + (q1_dot/r_trans)*T; //Omgezet naar motorhoeken
MarijkeZondag 29:df10cb76ef26 208 q2_ii = q2ref + (q2_dot*5.0)*T;
Marle 25:bbef09ff226b 209
Marle 25:bbef09ff226b 210 q1ref = q1_ii;
Marle 25:bbef09ff226b 211 q2ref = q2_ii;
MarijkeZondag 27:fa493551be99 212
MarijkeZondag 30:8191e8541a0a 213
MarijkeZondag 31:481ad25a40c3 214 //pc.printf("q1ref is %f en q2ref is %f\n\r",q1ref, q2ref);
MarijkeZondag 30:8191e8541a0a 215
MarijkeZondag 30:8191e8541a0a 216
MarijkeZondag 28:61d1372349c8 217 //start_control = 1;
MarijkeZondag 29:df10cb76ef26 218 engine_control1();
MarijkeZondag 28:61d1372349c8 219 engine_control2();
Marle 25:bbef09ff226b 220 }
Marle 25:bbef09ff226b 221
Marle 25:bbef09ff226b 222 void v_des_calculate_qref()
MarijkeZondag 23:97a976a8f0fc 223 {
MarijkeZondag 29:df10cb76ef26 224 double m1 = (potmeter1*2.0)-1.0;
MarijkeZondag 29:df10cb76ef26 225 double m2 = (potmeter2*2.0)-1.0;
MarijkeZondag 29:df10cb76ef26 226
MarijkeZondag 29:df10cb76ef26 227 if(m1>0.5) //If the filtered EMG signal of muscle 1 is higher than the threshold, motor 1 will turn
MarijkeZondag 16:5f7196ddc77b 228 {
MarijkeZondag 30:8191e8541a0a 229 v_x = 0.5; //beweging in +x direction
MarijkeZondag 30:8191e8541a0a 230 v_y = 0.0;
MarijkeZondag 26:ac5656aa35c7 231 ledr = 0; //red
Marle 25:bbef09ff226b 232 ledb = 1;
Marle 25:bbef09ff226b 233 ledg = 1;
MarijkeZondag 30:8191e8541a0a 234
MarijkeZondag 16:5f7196ddc77b 235 }
MarijkeZondag 29:df10cb76ef26 236 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 237 {
MarijkeZondag 30:8191e8541a0a 238 v_y = 0.5; //beweging in +y direction
MarijkeZondag 30:8191e8541a0a 239 v_x = 0.0;
MarijkeZondag 26:ac5656aa35c7 240 ledr = 1; //green
Marle 25:bbef09ff226b 241 ledb = 1;
Marle 25:bbef09ff226b 242 ledg = 0;
MarijkeZondag 24:6d63ad38fef7 243 }
MarijkeZondag 29:df10cb76ef26 244
MarijkeZondag 29:df10cb76ef26 245 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 246 {
MarijkeZondag 30:8191e8541a0a 247 v_x = -0.5;
MarijkeZondag 29:df10cb76ef26 248 v_y = 0;
MarijkeZondag 29:df10cb76ef26 249 ledr = 1; //Blue
MarijkeZondag 29:df10cb76ef26 250 ledb = 0;
MarijkeZondag 29:df10cb76ef26 251 ledg = 1;
MarijkeZondag 29:df10cb76ef26 252 }
MarijkeZondag 29:df10cb76ef26 253
MarijkeZondag 29:df10cb76ef26 254 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 255 {
MarijkeZondag 29:df10cb76ef26 256 v_x = 0;
MarijkeZondag 30:8191e8541a0a 257 v_y = -0.5;
MarijkeZondag 26:ac5656aa35c7 258 ledr = 1; //Blue
Marle 25:bbef09ff226b 259 ledb = 0;
Marle 25:bbef09ff226b 260 ledg = 1;
MarijkeZondag 29:df10cb76ef26 261 }
MarijkeZondag 29:df10cb76ef26 262
MarijkeZondag 30:8191e8541a0a 263
MarijkeZondag 26:ac5656aa35c7 264 else //If not higher than the threshold, motors will not turn at all
Marle 25:bbef09ff226b 265 {
Marle 25:bbef09ff226b 266 v_x = 0;
Marle 25:bbef09ff226b 267 v_y = 0;
MarijkeZondag 26:ac5656aa35c7 268 ledr = 0; //white
MarijkeZondag 24:6d63ad38fef7 269 ledb = 0;
MarijkeZondag 24:6d63ad38fef7 270 ledg = 0;
MarijkeZondag 28:61d1372349c8 271 //pwmpin1 = 0;
MarijkeZondag 28:61d1372349c8 272 //pwmpin2 = 0;
MarijkeZondag 24:6d63ad38fef7 273 }
MarijkeZondag 30:8191e8541a0a 274
MarijkeZondag 31:481ad25a40c3 275 inverse_kinematics(); //Call inverse kinematics function
MarijkeZondag 28:61d1372349c8 276
MarijkeZondag 26:ac5656aa35c7 277 }
MarijkeZondag 26:ac5656aa35c7 278
MarijkeZondag 31:481ad25a40c3 279 void printFunctie()
MarijkeZondag 31:481ad25a40c3 280 {
MarijkeZondag 31:481ad25a40c3 281 pc.printf("%f, %f\n\r", u1, u2);
MarijkeZondag 31:481ad25a40c3 282 }
MarijkeZondag 31:481ad25a40c3 283
MarijkeZondag 26:ac5656aa35c7 284 //------------------ Start main function --------------------------//
MarijkeZondag 26:ac5656aa35c7 285
MarijkeZondag 26:ac5656aa35c7 286
MarijkeZondag 31:481ad25a40c3 287
MarijkeZondag 23:97a976a8f0fc 288 int main()
MarijkeZondag 29:df10cb76ef26 289 {
MarijkeZondag 23:97a976a8f0fc 290 pc.baud(115200);
MarijkeZondag 26:ac5656aa35c7 291 pc.printf("Hello World!\r\n"); //Serial communication only works if hidscope is turned off.
MarijkeZondag 26:ac5656aa35c7 292 pwmpin1.period_us(60); //60 microseconds PWM period, 16.7 kHz
MarijkeZondag 28:61d1372349c8 293
MarijkeZondag 28:61d1372349c8 294 func_tick.attach(&v_des_calculate_qref,T2); //v_des determined every T
MarijkeZondag 31:481ad25a40c3 295 printTicker.attach(&printFunctie, 0.01);
MarijkeZondag 30:8191e8541a0a 296
MarijkeZondag 30:8191e8541a0a 297
MarijkeZondag 30:8191e8541a0a 298 while(true)
MarijkeZondag 30:8191e8541a0a 299 {
MarijkeZondag 28:61d1372349c8 300 //engine_control1_tick.attach(&engine_control1,T2);
MarijkeZondag 28:61d1372349c8 301 //engine_control2_tick.attach(&engine_control2,T2);
MarijkeZondag 28:61d1372349c8 302
MarijkeZondag 28:61d1372349c8 303 // HIDScope_tick.attach(&HIDScope_sample,T); //EMG signals raw + filtered to HIDScope every T sec.
MarijkeZondag 30:8191e8541a0a 304
MarijkeZondag 31:481ad25a40c3 305 //pc.printf("Encoder engine 1 %d\n\r",encoder2.getPulses());
MarijkeZondag 31:481ad25a40c3 306 //pc.printf("Encoder engine 2 %d\n\r",encoder1.getPulses());
MarijkeZondag 31:481ad25a40c3 307 //wait(0.1f);
MarijkeZondag 31:481ad25a40c3 308 ;
MarijkeZondag 29:df10cb76ef26 309 }
vsluiter 0:c8f15874531b 310 }