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@32:137d0f27e5a8, 2018-11-01 (annotated)

Committer:
MarijkeZondag
Date:
Thu Nov 01 14:40:34 2018 +0000
Revision:
32:137d0f27e5a8
Parent:
31:481ad25a40c3
Child:
33:7b94346bcead
iets werkt!!! 15:40

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 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 32:137d0f27e5a8 71 double Kp1 = 30.0; //Motor 1 eerst 17.5 , nu 1
MarijkeZondag 32:137d0f27e5a8 72 double Ki1 = 1.3;
MarijkeZondag 32:137d0f27e5a8 73 double Kd1 = 2.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);
Marle 25:bbef09ff226b 81
MarijkeZondag 29:df10cb76ef26 82
MarijkeZondag 32:137d0f27e5a8 83 double Kp2 = 30.0; //Motor 2 eerst 17.5, nu 1
MarijkeZondag 32:137d0f27e5a8 84 double Ki2 = 1.3;
MarijkeZondag 32:137d0f27e5a8 85 double Kd2 = 2.0;
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 }