Thom Kuenen / Mbed 2 deprecated Turning_Motor_V3

Project of Biorobotics

Dependencies:   HIDScope MODSERIAL QEI mbed biquadFilter

Fork of TutorialPES by Jurriën Bos

main.cpp@8:e8734a254818, 2018-10-29 (annotated)

Committer:
ThomBMT
Date:
Mon Oct 29 14:02:21 2018 +0000
Revision:
8:e8734a254818
Parent:
7:439940ae1197
Child:
9:355dd95199c3
Calibration AND Inverse kinamatics are added and work (probably)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JurrienBos 0:4591ba678a39 1 #include "mbed.h"
JurrienBos 0:4591ba678a39 2 #include "MODSERIAL.h"
ThomBMT 2:dc9766657afb 3 #include "HIDScope.h"
ThomBMT 2:dc9766657afb 4 #include "QEI.h"
ThomBMT 2:dc9766657afb 5
JurrienBos 0:4591ba678a39 6 MODSERIAL pc(USBTX, USBRX);
JurrienBos 0:4591ba678a39 7 DigitalOut DirectionPin1(D4);
JurrienBos 1:4bf64d003f3a 8 DigitalOut DirectionPin2(D7);
JurrienBos 0:4591ba678a39 9 PwmOut PwmPin1(D5);
JurrienBos 1:4bf64d003f3a 10 PwmOut PwmPin2(D6);
JurrienBos 0:4591ba678a39 11 DigitalIn Knop1(D2);
ThomBMT 4:8f67b8327300 12 DigitalIn Knop2(D3);
ThomBMT 8:e8734a254818 13 DigitalIn Knop3(PTA4);
JurrienBos 0:4591ba678a39 14 AnalogIn pot1 (A5);
JurrienBos 1:4bf64d003f3a 15 AnalogIn pot2 (A4);
ThomBMT 2:dc9766657afb 16 AnalogIn emg0( A0 );
ThomBMT 2:dc9766657afb 17 AnalogIn emg1( A1 );
ThomBMT 2:dc9766657afb 18 AnalogIn emg2( A2 );
ThomBMT 2:dc9766657afb 19 AnalogIn emg3( A3 );
ThomBMT 8:e8734a254818 20 DigitalOut led(LED_GREEN);
JurrienBos 0:4591ba678a39 21
ThomBMT 8:e8734a254818 22 QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
ThomBMT 5:312186a0604d 23 QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
ThomBMT 5:312186a0604d 24
ThomBMT 4:8f67b8327300 25 Ticker StateTicker;
ThomBMT 2:dc9766657afb 26 Ticker printTicker;
ThomBMT 5:312186a0604d 27
ThomBMT 2:dc9766657afb 28 HIDScope scope( 4 );
JurrienBos 0:4591ba678a39 29
ThomBMT 3:c8f0fc045505 30 volatile float Bicep_Right = 0.0;
ThomBMT 3:c8f0fc045505 31 volatile float Bicep_Left = 0.0;
ThomBMT 3:c8f0fc045505 32 volatile float Tricep_Right = 0.0;
ThomBMT 3:c8f0fc045505 33 volatile float Tricep_Left = 0.0;
ThomBMT 8:e8734a254818 34
ThomBMT 3:c8f0fc045505 35 volatile const float maxVelocity = 8.4; // in rad/s
ThomBMT 7:439940ae1197 36 volatile const float pi = 3.1415926;
ThomBMT 7:439940ae1197 37 volatile const float rad_count = 0.0007479; // 2pi/8400;
ThomBMT 8:e8734a254818 38
ThomBMT 3:c8f0fc045505 39 volatile float referenceVelocity1 = 0.5; //dit is de gecentreerde waarde en dus de nulstand
ThomBMT 3:c8f0fc045505 40 volatile float referenceVelocity2 = 0.5;
ThomBMT 2:dc9766657afb 41
ThomBMT 8:e8734a254818 42 volatile float q_1;
ThomBMT 8:e8734a254818 43 volatile float q_2;
ThomBMT 8:e8734a254818 44 volatile float r_1;
ThomBMT 8:e8734a254818 45 volatile float r_2;
ThomBMT 8:e8734a254818 46 volatile const float r_3 = 0.035;
ThomBMT 8:e8734a254818 47 volatile float w_1;
ThomBMT 8:e8734a254818 48 volatile float w_2;
ThomBMT 8:e8734a254818 49
ThomBMT 8:e8734a254818 50 volatile float Flex_Bi_R;
ThomBMT 8:e8734a254818 51 volatile float Flex_Bi_L;
ThomBMT 8:e8734a254818 52 volatile float Flex_Tri_R;
ThomBMT 8:e8734a254818 53 volatile float Flex_Tri_L;
ThomBMT 8:e8734a254818 54
ThomBMT 8:e8734a254818 55
ThomBMT 8:e8734a254818 56 enum states{Starting, Calibration, Homing, Function};
ThomBMT 4:8f67b8327300 57
ThomBMT 8:e8734a254818 58 volatile states Active_State = Starting;
ThomBMT 4:8f67b8327300 59
ThomBMT 5:312186a0604d 60 volatile int counts1 ;
ThomBMT 5:312186a0604d 61 volatile int counts2 ;
ThomBMT 5:312186a0604d 62 volatile float rad_m1;
ThomBMT 5:312186a0604d 63 volatile float rad_m2;
ThomBMT 4:8f67b8327300 64
ThomBMT 3:c8f0fc045505 65 void Encoding()
ThomBMT 3:c8f0fc045505 66 {
ThomBMT 5:312186a0604d 67
ThomBMT 3:c8f0fc045505 68 counts1 = Encoder1.getPulses();
ThomBMT 3:c8f0fc045505 69 counts2 = Encoder2.getPulses();
ThomBMT 4:8f67b8327300 70
ThomBMT 7:439940ae1197 71 rad_m1 = rad_count * (float)counts1;
ThomBMT 7:439940ae1197 72 rad_m2 = rad_count * (float)counts2;
ThomBMT 3:c8f0fc045505 73 }
ThomBMT 2:dc9766657afb 74
ThomBMT 2:dc9766657afb 75 void EMG_Read()
ThomBMT 2:dc9766657afb 76 {
ThomBMT 3:c8f0fc045505 77 Bicep_Right = emg0.read();
ThomBMT 3:c8f0fc045505 78 Bicep_Left = emg1.read();
ThomBMT 3:c8f0fc045505 79 Tricep_Right = emg2.read();
ThomBMT 3:c8f0fc045505 80 Tricep_Left = emg3.read();
ThomBMT 2:dc9766657afb 81 }
ThomBMT 2:dc9766657afb 82
ThomBMT 2:dc9766657afb 83 void sample()
ThomBMT 2:dc9766657afb 84 {
ThomBMT 2:dc9766657afb 85
ThomBMT 2:dc9766657afb 86 scope.set(0, emg0.read() );
ThomBMT 2:dc9766657afb 87 scope.set(1, emg1.read() );
ThomBMT 2:dc9766657afb 88 scope.set(2, emg2.read() );
ThomBMT 2:dc9766657afb 89 scope.set(3, emg3.read() );
ThomBMT 2:dc9766657afb 90
ThomBMT 2:dc9766657afb 91 scope.send();
ThomBMT 2:dc9766657afb 92 }
ThomBMT 2:dc9766657afb 93
ThomBMT 8:e8734a254818 94 void Inverse()
ThomBMT 8:e8734a254818 95 {
ThomBMT 8:e8734a254818 96 q_1= rad_m1; // uit Encoder
ThomBMT 8:e8734a254818 97 q_2= rad_m2+(pi/2.0f); // uit Encoder
ThomBMT 8:e8734a254818 98 r_1= -0.2f;
ThomBMT 8:e8734a254818 99 r_2= -0.2f;
ThomBMT 8:e8734a254818 100
ThomBMT 8:e8734a254818 101 float u = -r_2*sin(q_1)*cos(q_2)-(r_2)*cos(q_1)*sin(q_2);
ThomBMT 8:e8734a254818 102 float z = 2.0f*(r_2*cos(q_1)*cos(q_2))-r_3;
ThomBMT 8:e8734a254818 103 float y = r_2*cos(q_1)*cos(q_2)-r_2*sin(q_1)*sin(q_2)+2.0f*(r_1*cos(q_1))-r_3;
ThomBMT 8:e8734a254818 104 float x = (-2.0f)*r_2*sin(q_1)*cos(q_2);
ThomBMT 8:e8734a254818 105 float D = 1.0f/(u*z-x*y); // Determinant
ThomBMT 8:e8734a254818 106 printf("Determinant is %f\n", D);
ThomBMT 8:e8734a254818 107
ThomBMT 8:e8734a254818 108 float a = D*z; // Inverse jacobian a,b,c,d vormen 2 bij 2 matrix
ThomBMT 8:e8734a254818 109 float b = -D*x; // Inverse jacobian
ThomBMT 8:e8734a254818 110 float c = -D*y; // Inverse jacobian
ThomBMT 8:e8734a254818 111 float d = D*u; // Inverse jacobian
ThomBMT 8:e8734a254818 112
ThomBMT 8:e8734a254818 113 float vx = 0.01f; // uit emg data
ThomBMT 8:e8734a254818 114 float vy = 0.0f; // uit emg data
ThomBMT 8:e8734a254818 115
ThomBMT 8:e8734a254818 116 w_1 = vx*a+vy*b;
ThomBMT 8:e8734a254818 117 w_2 = vx*c+vy*d;
ThomBMT 8:e8734a254818 118
ThomBMT 8:e8734a254818 119 /*
ThomBMT 8:e8734a254818 120 printf("%f\n", w_1);
ThomBMT 8:e8734a254818 121 printf("%f\n", w_2);
ThomBMT 8:e8734a254818 122 */
ThomBMT 8:e8734a254818 123 }
JurrienBos 1:4bf64d003f3a 124
JurrienBos 1:4bf64d003f3a 125 void velocity1()
JurrienBos 0:4591ba678a39 126 {
ThomBMT 3:c8f0fc045505 127 if (pot1.read()>0.5f)
JurrienBos 0:4591ba678a39 128 {
JurrienBos 0:4591ba678a39 129 // Clockwise rotation
JurrienBos 1:4bf64d003f3a 130 referenceVelocity1 = (pot1.read()-0.5f) * 2.0f;
JurrienBos 0:4591ba678a39 131 }
JurrienBos 0:4591ba678a39 132
ThomBMT 5:312186a0604d 133 else if (pot1.read() == 0.5f)
JurrienBos 0:4591ba678a39 134 {
JurrienBos 1:4bf64d003f3a 135 referenceVelocity1 = pot1.read() * 0.0f;
JurrienBos 0:4591ba678a39 136 }
JurrienBos 0:4591ba678a39 137
JurrienBos 0:4591ba678a39 138 else if (pot1.read() < 0.5f)
JurrienBos 0:4591ba678a39 139 {
JurrienBos 0:4591ba678a39 140 // Counterclockwise rotation
JurrienBos 1:4bf64d003f3a 141 referenceVelocity1 = 2.0f * (pot1.read()-0.5f) ;
JurrienBos 0:4591ba678a39 142 }
JurrienBos 0:4591ba678a39 143 }
JurrienBos 0:4591ba678a39 144
JurrienBos 1:4bf64d003f3a 145 void velocity2()
JurrienBos 1:4bf64d003f3a 146 {
JurrienBos 1:4bf64d003f3a 147 if (pot2.read()>0.5f)
JurrienBos 1:4bf64d003f3a 148 {
JurrienBos 1:4bf64d003f3a 149 // Clockwise rotation
JurrienBos 1:4bf64d003f3a 150 referenceVelocity2 = (pot2.read()-0.5f) * 2.0f;
JurrienBos 1:4bf64d003f3a 151 }
JurrienBos 1:4bf64d003f3a 152
JurrienBos 1:4bf64d003f3a 153 else if (pot2.read() == 0.5f)
JurrienBos 1:4bf64d003f3a 154 {
JurrienBos 1:4bf64d003f3a 155 referenceVelocity2 = pot2.read() * 0.0f;
JurrienBos 1:4bf64d003f3a 156 }
JurrienBos 1:4bf64d003f3a 157
JurrienBos 1:4bf64d003f3a 158 else if (pot2.read() < 0.5f)
JurrienBos 1:4bf64d003f3a 159 {
JurrienBos 1:4bf64d003f3a 160 // Counterclockwise rotation
JurrienBos 1:4bf64d003f3a 161 referenceVelocity2 = 2.0f * (pot2.read()-0.5f) ;
JurrienBos 1:4bf64d003f3a 162 }
JurrienBos 1:4bf64d003f3a 163 }
JurrienBos 1:4bf64d003f3a 164
JurrienBos 1:4bf64d003f3a 165 void motor1()
ThomBMT 8:e8734a254818 166 {
ThomBMT 8:e8734a254818 167 float u_v1 = w_1; //referenceVelocity1
ThomBMT 8:e8734a254818 168 float u = u_v1/ (2.0f * pi);
JurrienBos 0:4591ba678a39 169 DirectionPin1 = u < 0.0f;
JurrienBos 0:4591ba678a39 170 PwmPin1 = fabs(u);
JurrienBos 0:4591ba678a39 171 }
JurrienBos 0:4591ba678a39 172
JurrienBos 1:4bf64d003f3a 173 void motor2()
JurrienBos 1:4bf64d003f3a 174 {
ThomBMT 8:e8734a254818 175 float u_v2 = w_2; //referenceVelocity2
ThomBMT 8:e8734a254818 176 float u = u_v2/ (2.0f * pi);
JurrienBos 1:4bf64d003f3a 177 DirectionPin2 = u > 0.0f;
JurrienBos 1:4bf64d003f3a 178 PwmPin2 = fabs(u);
JurrienBos 1:4bf64d003f3a 179 }
ThomBMT 8:e8734a254818 180
ThomBMT 8:e8734a254818 181 void Calibrating()
ThomBMT 8:e8734a254818 182 {
ThomBMT 8:e8734a254818 183 static float n = 0.0;
ThomBMT 8:e8734a254818 184 static float m = 0.0;
ThomBMT 8:e8734a254818 185 static float l = 0.0;
ThomBMT 8:e8734a254818 186 static float k = 0.0;
ThomBMT 8:e8734a254818 187
ThomBMT 8:e8734a254818 188 for(int ii=0; ii<=20000; ii++)
ThomBMT 8:e8734a254818 189 {
ThomBMT 8:e8734a254818 190 if (ii <2500)
ThomBMT 8:e8734a254818 191 {
ThomBMT 8:e8734a254818 192 n = n + emg0.read();
ThomBMT 8:e8734a254818 193 }
ThomBMT 8:e8734a254818 194 else if (ii == 2500)
ThomBMT 8:e8734a254818 195 {
ThomBMT 8:e8734a254818 196 Flex_Bi_R = n / 2500.0f;
ThomBMT 8:e8734a254818 197 }
ThomBMT 8:e8734a254818 198 else if (ii>2500 && ii<=3500)
ThomBMT 8:e8734a254818 199 {
ThomBMT 8:e8734a254818 200 //chillen
ThomBMT 8:e8734a254818 201 }
ThomBMT 8:e8734a254818 202 else if(ii>3500 && ii<6000)
ThomBMT 8:e8734a254818 203 {
ThomBMT 8:e8734a254818 204 m = m + emg1.read();
ThomBMT 8:e8734a254818 205 }
ThomBMT 8:e8734a254818 206 else if (ii == 6000)
ThomBMT 8:e8734a254818 207 {
ThomBMT 8:e8734a254818 208 Flex_Bi_L = m / 2500.0f;
ThomBMT 8:e8734a254818 209 }
ThomBMT 8:e8734a254818 210 else if (ii>6000 && ii<=7000)
ThomBMT 8:e8734a254818 211 {
ThomBMT 8:e8734a254818 212 //chillen
ThomBMT 8:e8734a254818 213 }
ThomBMT 8:e8734a254818 214 else if (ii>7000 && ii<9500)
ThomBMT 8:e8734a254818 215 {
ThomBMT 8:e8734a254818 216 l = l + emg2.read();
ThomBMT 8:e8734a254818 217 }
ThomBMT 8:e8734a254818 218 else if (ii == 9500)
ThomBMT 8:e8734a254818 219 {
ThomBMT 8:e8734a254818 220 Flex_Tri_R = l / 2500.0f;
ThomBMT 8:e8734a254818 221 }
ThomBMT 8:e8734a254818 222 else if (ii>9500 && ii <=10500)
ThomBMT 8:e8734a254818 223 {
ThomBMT 8:e8734a254818 224 //chillen
ThomBMT 8:e8734a254818 225 }
ThomBMT 8:e8734a254818 226 else if (ii>10500 && ii<13000)
ThomBMT 8:e8734a254818 227 {
ThomBMT 8:e8734a254818 228 k = k + emg3.read();
ThomBMT 8:e8734a254818 229 }
ThomBMT 8:e8734a254818 230 else if (ii == 13000)
ThomBMT 8:e8734a254818 231 {
ThomBMT 8:e8734a254818 232 Flex_Tri_L = k / 2500.0f;
ThomBMT 8:e8734a254818 233 }
ThomBMT 8:e8734a254818 234 }
ThomBMT 8:e8734a254818 235 }
ThomBMT 8:e8734a254818 236
ThomBMT 8:e8734a254818 237 void OFF()
ThomBMT 8:e8734a254818 238 {
ThomBMT 8:e8734a254818 239 PwmPin1 = PwmPin2 = 0;
ThomBMT 8:e8734a254818 240 }
JurrienBos 0:4591ba678a39 241
ThomBMT 2:dc9766657afb 242 void Printing()
ThomBMT 2:dc9766657afb 243 {
ThomBMT 8:e8734a254818 244 float v1 = PwmPin1 * maxVelocity;
ThomBMT 8:e8734a254818 245 float v2 = PwmPin2 * maxVelocity;
ThomBMT 8:e8734a254818 246
ThomBMT 8:e8734a254818 247 pc.printf("q1 = %f [rad] \n q2 = %f [rad] \n q1dot = %f [rad/s] \n q2dot = %f [rad/s] \n\n\n\n\n", rad_m1, rad_m2, v1, v2);
ThomBMT 2:dc9766657afb 248 }
ThomBMT 2:dc9766657afb 249
ThomBMT 4:8f67b8327300 250 void StateMachine()
ThomBMT 4:8f67b8327300 251 {
ThomBMT 4:8f67b8327300 252 switch (Active_State)
ThomBMT 4:8f67b8327300 253 {
ThomBMT 8:e8734a254818 254 case Starting:
ThomBMT 8:e8734a254818 255 OFF();
ThomBMT 8:e8734a254818 256 for (int i = 0; i<=20; i++)
ThomBMT 8:e8734a254818 257 {
ThomBMT 8:e8734a254818 258 led = !led;
ThomBMT 8:e8734a254818 259 wait(0.05);
ThomBMT 8:e8734a254818 260
ThomBMT 8:e8734a254818 261 if (i == 0)
ThomBMT 8:e8734a254818 262 {
ThomBMT 8:e8734a254818 263 pc.printf("Starting up..\n");
ThomBMT 8:e8734a254818 264 }
ThomBMT 8:e8734a254818 265 else if (i == 20)
ThomBMT 8:e8734a254818 266 {
ThomBMT 8:e8734a254818 267 Active_State = Calibration;
ThomBMT 8:e8734a254818 268 pc.printf("Entering Calibration state \n");
ThomBMT 8:e8734a254818 269 }
ThomBMT 8:e8734a254818 270 }
ThomBMT 8:e8734a254818 271 break;
ThomBMT 8:e8734a254818 272
ThomBMT 4:8f67b8327300 273 case Calibration:
ThomBMT 5:312186a0604d 274 //calibration actions
ThomBMT 5:312186a0604d 275 //pc.printf("Calibration State");
ThomBMT 8:e8734a254818 276
ThomBMT 8:e8734a254818 277 Calibrating();
ThomBMT 8:e8734a254818 278 OFF();
ThomBMT 5:312186a0604d 279 if (Knop1==false)
ThomBMT 5:312186a0604d 280 {
ThomBMT 5:312186a0604d 281 pc.printf("Entering Homing state \n");
ThomBMT 5:312186a0604d 282 Active_State = Homing;
ThomBMT 5:312186a0604d 283 }
ThomBMT 5:312186a0604d 284
ThomBMT 5:312186a0604d 285 sample();
ThomBMT 5:312186a0604d 286 EMG_Read();
ThomBMT 5:312186a0604d 287 Encoding();
ThomBMT 4:8f67b8327300 288 break;
ThomBMT 4:8f67b8327300 289
ThomBMT 4:8f67b8327300 290 case Homing:
ThomBMT 5:312186a0604d 291 //Homing actions
ThomBMT 5:312186a0604d 292 //pc.printf("Homing State");
ThomBMT 5:312186a0604d 293 if (Knop2==false)
ThomBMT 5:312186a0604d 294 {
ThomBMT 5:312186a0604d 295 pc.printf("Entering Funtioning State \n");
ThomBMT 5:312186a0604d 296 Active_State = Function;
ThomBMT 5:312186a0604d 297 }
ThomBMT 5:312186a0604d 298
ThomBMT 5:312186a0604d 299 sample();
ThomBMT 5:312186a0604d 300 EMG_Read();
ThomBMT 5:312186a0604d 301 Encoding();
ThomBMT 4:8f67b8327300 302 break;
ThomBMT 4:8f67b8327300 303
ThomBMT 4:8f67b8327300 304 case Function:
ThomBMT 5:312186a0604d 305 //pc.printf("Funtioning State");
ThomBMT 8:e8734a254818 306
ThomBMT 8:e8734a254818 307 if (Knop3==false)
ThomBMT 8:e8734a254818 308 {
ThomBMT 5:312186a0604d 309 pc.printf("Re-entering Calibration State \n");
ThomBMT 5:312186a0604d 310 Active_State = Calibration;
ThomBMT 5:312186a0604d 311 }
ThomBMT 8:e8734a254818 312
ThomBMT 5:312186a0604d 313 sample();
ThomBMT 5:312186a0604d 314 EMG_Read();
ThomBMT 5:312186a0604d 315 Encoding();
ThomBMT 5:312186a0604d 316 velocity1();
ThomBMT 5:312186a0604d 317 velocity2();
ThomBMT 5:312186a0604d 318 motor1();
ThomBMT 8:e8734a254818 319 motor2();
ThomBMT 4:8f67b8327300 320 break;
ThomBMT 4:8f67b8327300 321
ThomBMT 4:8f67b8327300 322 default:
ThomBMT 4:8f67b8327300 323 pc.printf("UNKNOWN COMMAND");
ThomBMT 4:8f67b8327300 324 }
ThomBMT 4:8f67b8327300 325 }
ThomBMT 4:8f67b8327300 326
JurrienBos 0:4591ba678a39 327 int main()
JurrienBos 0:4591ba678a39 328 {
JurrienBos 0:4591ba678a39 329 pc.baud(115200);
ThomBMT 4:8f67b8327300 330 PwmPin1.period_us(30); //60 microseconds pwm period, 16.7 kHz
ThomBMT 2:dc9766657afb 331
ThomBMT 8:e8734a254818 332 StateTicker.attach(&StateMachine, 0.002);
ThomBMT 4:8f67b8327300 333
ThomBMT 8:e8734a254818 334 printTicker.attach(&Printing, 4.0);
ThomBMT 2:dc9766657afb 335
JurrienBos 0:4591ba678a39 336 while(true)
ThomBMT 5:312186a0604d 337 {
JurrienBos 0:4591ba678a39 338 }
JurrienBos 1:4bf64d003f3a 339 }