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@35:01aa6cb4a35f, 2018-11-02 (annotated)

Committer:
MarijkeZondag
Date:
Fri Nov 02 07:59:57 2018 +0000
Revision:
35:01aa6cb4a35f
Parent:
34:bece2820b63b 
..

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