Matthew Maat / Mbed 2 deprecated template_biorobotics

fancy lampje

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FXOS8700Q FastPWM

main.cpp@23:b0222fa7c131, 2019-10-25 (annotated)

Committer:
MatthewMaat
Date:
Fri Oct 25 09:52:56 2019 +0000
Revision:
23:b0222fa7c131
Parent:
22:335a30b0825d
Child:
24:3c9ac44890f0
Volledig programma, operating is nog niet getest

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
MatthewMaat 13:ec4708dab45d 2 #include "HIDScope.h"
MatthewMaat 17:d1acb6888b82 3 #include "QEI.h"
RobertoO 1:b862262a9d14 4 #include "MODSERIAL.h"
MatthewMaat 17:d1acb6888b82 5 #include "BiQuad.h"
MatthewMaat 8:ec3c634390c7 6 #include "FastPWM.h"
MatthewMaat 2:626688c21b6f 7 #include <iostream>
MatthewMaat 5:cee5f898b350 8 MODSERIAL pc(USBTX, USBRX);
MatthewMaat 18:8002c75b8e20 9 QEI motor2_pos (D8, D9, NC, 32);
MatthewMaat 18:8002c75b8e20 10 QEI motor1_pos (D12, D13, NC, 32);
MatthewMaat 13:ec4708dab45d 11 AnalogIn ain2(A2);
MatthewMaat 13:ec4708dab45d 12 AnalogIn ain1(A3);
MatthewMaat 13:ec4708dab45d 13 DigitalOut dir2(D4);
MatthewMaat 13:ec4708dab45d 14 DigitalOut dir1(D7);
MatthewMaat 13:ec4708dab45d 15 //D4,D7 direction of motors 2,1 on board, D5,D6- PWM of motors 2,1 on board
MatthewMaat 13:ec4708dab45d 16 PwmOut motor2_pwm(D5);
MatthewMaat 13:ec4708dab45d 17 PwmOut motor1_pwm(D6);
MatthewMaat 13:ec4708dab45d 18 AnalogIn emg0( A0 );
MatthewMaat 13:ec4708dab45d 19 AnalogIn emg1( A1 );
MatthewMaat 13:ec4708dab45d 20
MatthewMaat 15:c4799ad02cdc 21 Ticker ticktick;
MatthewMaat 15:c4799ad02cdc 22 Timer state_time;
MatthewMaat 23:b0222fa7c131 23 Timeout EMG_peakx;
MatthewMaat 23:b0222fa7c131 24 Timeout turnx;
MatthewMaat 23:b0222fa7c131 25 Timeout EMG_peaky;
MatthewMaat 23:b0222fa7c131 26 Timeout turny;
MatthewMaat 13:ec4708dab45d 27 Ticker sample_timer;
MatthewMaat 18:8002c75b8e20 28 HIDScope scope( 4);
MatthewMaat 14:dc89250ebc52 29 DigitalOut ledred(LED_RED);
MatthewMaat 14:dc89250ebc52 30 DigitalOut ledblue(LED_BLUE);
MatthewMaat 14:dc89250ebc52 31 DigitalOut ledgreen(LED_GREEN);
MatthewMaat 15:c4799ad02cdc 32 InterruptIn err(SW2);
MatthewMaat 15:c4799ad02cdc 33 InterruptIn button(SW3);
MatthewMaat 15:c4799ad02cdc 34
MatthewMaat 15:c4799ad02cdc 35 volatile float P0;
MatthewMaat 15:c4799ad02cdc 36 volatile float P1;
MatthewMaat 15:c4799ad02cdc 37 volatile float EMG_min0=1;
MatthewMaat 15:c4799ad02cdc 38 volatile float EMG_max0=0;
MatthewMaat 15:c4799ad02cdc 39 volatile float EMG_min1=1;
MatthewMaat 15:c4799ad02cdc 40 volatile float EMG_max1=0;
MatthewMaat 23:b0222fa7c131 41 volatile bool ignore_peaksx=false;
MatthewMaat 23:b0222fa7c131 42 volatile bool ignore_turnx=true;
MatthewMaat 23:b0222fa7c131 43 volatile bool ignore_peaksy=false;
MatthewMaat 23:b0222fa7c131 44 volatile bool ignore_turny=true;
MatthewMaat 14:dc89250ebc52 45 enum states{Waiting,Position_calibration,EMG_calibration,Homing,Operating,Demo,Failure};
MatthewMaat 15:c4799ad02cdc 46 states currentState=Waiting;
MatthewMaat 21:a316452da8cd 47 volatile bool motor1_calibrated=false;
MatthewMaat 10:e1eb73e19540 48
MatthewMaat 23:b0222fa7c131 49 volatile float x_ref=0.175;
MatthewMaat 23:b0222fa7c131 50 volatile float y_ref=0.175;
jobjansen 20:0f6a88f29a71 51 static float v_refx; //reference speeds
jobjansen 20:0f6a88f29a71 52 static float v_refy;
jobjansen 20:0f6a88f29a71 53 static float T = 0.002; //measuring period
jobjansen 20:0f6a88f29a71 54 static float L = 0.35; //distance between the motor axles, has to be checked
jobjansen 20:0f6a88f29a71 55 volatile float theta1_old = 0.0; //feedback variables
jobjansen 20:0f6a88f29a71 56 volatile float theta2_old = 0.0;
jobjansen 20:0f6a88f29a71 57
jobjansen 20:0f6a88f29a71 58
MatthewMaat 21:a316452da8cd 59 float error_1;
MatthewMaat 21:a316452da8cd 60 float error_2;
jobjansen 20:0f6a88f29a71 61
jobjansen 20:0f6a88f29a71 62 //PID controller variables
jobjansen 20:0f6a88f29a71 63 bool q = true;
jobjansen 20:0f6a88f29a71 64 float error_prev;
MatthewMaat 23:b0222fa7c131 65 static float Kp = 24.6;
MatthewMaat 23:b0222fa7c131 66 static float Ki = 28.3;
jobjansen 20:0f6a88f29a71 67 static float Kd = 0.1;
jobjansen 20:0f6a88f29a71 68 float u;
jobjansen 20:0f6a88f29a71 69 float u_p;
jobjansen 20:0f6a88f29a71 70 float u_i;
jobjansen 20:0f6a88f29a71 71 float u_d;
MatthewMaat 21:a316452da8cd 72 float error_integral1 = 0.0;
MatthewMaat 21:a316452da8cd 73 float error_integral2 = 0.0;
jobjansen 20:0f6a88f29a71 74
jobjansen 20:0f6a88f29a71 75 float u_1;
jobjansen 20:0f6a88f29a71 76 float u_2;
MatthewMaat 18:8002c75b8e20 77 const float angle2_offset=asin(0.2);
MatthewMaat 22:335a30b0825d 78 const float angle1_offset=atan(2.2/32.8);
MatthewMaat 18:8002c75b8e20 79 const double pi=3.1415926535897932384626;
MatthewMaat 18:8002c75b8e20 80 volatile float theta1;
MatthewMaat 18:8002c75b8e20 81 volatile float theta2;
MatthewMaat 14:dc89250ebc52 82
MatthewMaat 14:dc89250ebc52 83 void read_emg()
MatthewMaat 13:ec4708dab45d 84 {
MatthewMaat 15:c4799ad02cdc 85 //EMG signal 0
MatthewMaat 15:c4799ad02cdc 86 static int count0=0;
MatthewMaat 15:c4799ad02cdc 87 static float RMS_value0=0;
MatthewMaat 15:c4799ad02cdc 88 static float HighPass_value0=0;
MatthewMaat 15:c4799ad02cdc 89 count0+=1;
MatthewMaat 15:c4799ad02cdc 90 static float RMS0[150];
MatthewMaat 15:c4799ad02cdc 91 static float HighPass0[30];
MatthewMaat 17:d1acb6888b82 92 static BiQuad Notch0(0.9695f,-1.5695f,0.9695f,-1.5695f,0.9391f);
MatthewMaat 17:d1acb6888b82 93 static BiQuad Notch1(0.9695f,-1.5695f,0.9695f,-1.5695f,0.9391f);
MatthewMaat 15:c4799ad02cdc 94 float I0;
MatthewMaat 15:c4799ad02cdc 95 float If0;
MatthewMaat 15:c4799ad02cdc 96 //signal 1
MatthewMaat 15:c4799ad02cdc 97 static int count1=0;
MatthewMaat 15:c4799ad02cdc 98 static float RMS_value1=0;
MatthewMaat 15:c4799ad02cdc 99 static float HighPass_value1=0;
MatthewMaat 15:c4799ad02cdc 100 count1+=1;
MatthewMaat 15:c4799ad02cdc 101 static float RMS1[150];
MatthewMaat 15:c4799ad02cdc 102 static float HighPass1[30];
MatthewMaat 13:ec4708dab45d 103 float I1;
MatthewMaat 15:c4799ad02cdc 104 float If1;
MatthewMaat 15:c4799ad02cdc 105 I0=emg0.read(); //read signal
MatthewMaat 17:d1acb6888b82 106 double notched0=Notch0.step(I0);
MatthewMaat 17:d1acb6888b82 107 HighPass_value0+=(notched0-HighPass0[count0%30])/30.0;
MatthewMaat 17:d1acb6888b82 108 HighPass0[count0%30]=notched0;
MatthewMaat 15:c4799ad02cdc 109 If0=pow(I0-HighPass_value0,2.0f); // Highpass-filtered value squared
MatthewMaat 15:c4799ad02cdc 110 RMS_value0+=(If0-RMS0[count0%150])/150.0;
MatthewMaat 15:c4799ad02cdc 111 RMS0[count0%150]=If0;
MatthewMaat 13:ec4708dab45d 112 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
MatthewMaat 15:c4799ad02cdc 113 P0=sqrt(RMS_value0);
MatthewMaat 15:c4799ad02cdc 114 I1=emg1.read(); //read signal
MatthewMaat 17:d1acb6888b82 115 double notched1=Notch1.step(I1);
MatthewMaat 17:d1acb6888b82 116 HighPass_value1+=(notched1-HighPass1[count1%30])/30.0;
MatthewMaat 17:d1acb6888b82 117 HighPass1[count1%30]=notched1;
MatthewMaat 15:c4799ad02cdc 118 If1=pow(I1-HighPass_value1,2.0f); // Highpass-filtered value squared
MatthewMaat 15:c4799ad02cdc 119 RMS_value1+=(If1-RMS1[count1%150])/150.0;
MatthewMaat 15:c4799ad02cdc 120 RMS1[count1%150]=If1;
MatthewMaat 15:c4799ad02cdc 121 /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
MatthewMaat 15:c4799ad02cdc 122 P1=sqrt(RMS_value1);
MatthewMaat 13:ec4708dab45d 123 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
MatthewMaat 13:ec4708dab45d 124 * Ensure that enough channels are available (HIDScope scope( 2 ))
MatthewMaat 13:ec4708dab45d 125 * Finally, send all channels to the PC at once */
MatthewMaat 13:ec4708dab45d 126 /* To indicate that the function is working, the LED is toggled */
MatthewMaat 14:dc89250ebc52 127 ledred=1;
MatthewMaat 14:dc89250ebc52 128 ledgreen=0;
MatthewMaat 14:dc89250ebc52 129 ledblue=1;
MatthewMaat 13:ec4708dab45d 130 }
MatthewMaat 14:dc89250ebc52 131
MatthewMaat 18:8002c75b8e20 132 void get_angles(void)
MatthewMaat 18:8002c75b8e20 133 {
MatthewMaat 18:8002c75b8e20 134 float pulses1=motor1_pos.getPulses();
MatthewMaat 18:8002c75b8e20 135 float pulses2=motor2_pos.getPulses();
MatthewMaat 19:fb3d570a115e 136 theta1=angle1_offset+pulses1*17.0/16.0*2*pi/131.0/32.0;
MatthewMaat 19:fb3d570a115e 137 theta2=angle2_offset+pulses2*17.0/16.0*2*pi/131.0/32.0;
MatthewMaat 18:8002c75b8e20 138 }
MatthewMaat 18:8002c75b8e20 139
MatthewMaat 18:8002c75b8e20 140 void pos_cal(void)
MatthewMaat 18:8002c75b8e20 141 {
MatthewMaat 18:8002c75b8e20 142 float t=state_time.read();
MatthewMaat 18:8002c75b8e20 143 static int pos_time_counter=0;
MatthewMaat 18:8002c75b8e20 144 static int last_ticks=10000;
MatthewMaat 18:8002c75b8e20 145 float pulses;
MatthewMaat 18:8002c75b8e20 146 pos_time_counter+=1;
MatthewMaat 18:8002c75b8e20 147 if(!motor1_calibrated&&t>1.0f)
MatthewMaat 18:8002c75b8e20 148 {
MatthewMaat 18:8002c75b8e20 149 dir1=1; //???
MatthewMaat 22:335a30b0825d 150 motor1_pwm.write(0.75f);
MatthewMaat 18:8002c75b8e20 151 pulses=motor1_pos.getPulses();
MatthewMaat 18:8002c75b8e20 152 if(pos_time_counter%500==0&&fabs(pulses-last_ticks)<1)
MatthewMaat 18:8002c75b8e20 153 {
MatthewMaat 21:a316452da8cd 154 ledblue=!ledblue;
MatthewMaat 18:8002c75b8e20 155 motor1_pos.reset();
MatthewMaat 18:8002c75b8e20 156 last_ticks=10000;
MatthewMaat 18:8002c75b8e20 157 }
MatthewMaat 18:8002c75b8e20 158 else if(pos_time_counter%500==0)
MatthewMaat 18:8002c75b8e20 159 {
MatthewMaat 18:8002c75b8e20 160 last_ticks=motor1_pos.getPulses();
MatthewMaat 18:8002c75b8e20 161 }
MatthewMaat 18:8002c75b8e20 162
MatthewMaat 18:8002c75b8e20 163 }
MatthewMaat 22:335a30b0825d 164 else if(!motor1_calibrated)
MatthewMaat 22:335a30b0825d 165 {
MatthewMaat 22:335a30b0825d 166 motor2_pwm.write(0.75f);
MatthewMaat 22:335a30b0825d 167 dir2=1;
MatthewMaat 22:335a30b0825d 168 }
MatthewMaat 23:b0222fa7c131 169 else if(t>1.0f)
MatthewMaat 18:8002c75b8e20 170 {
MatthewMaat 18:8002c75b8e20 171 motor1_pwm.write(0.0f);
MatthewMaat 18:8002c75b8e20 172 dir2=1; //???
MatthewMaat 22:335a30b0825d 173 motor2_pwm.write(0.75f);
MatthewMaat 18:8002c75b8e20 174 pulses=motor2_pos.getPulses();
MatthewMaat 18:8002c75b8e20 175 if(pos_time_counter%500==0&&fabs(pulses-last_ticks)<1)
MatthewMaat 18:8002c75b8e20 176 {
MatthewMaat 21:a316452da8cd 177 ledblue=!ledblue;
MatthewMaat 18:8002c75b8e20 178 motor2_pos.reset();
MatthewMaat 18:8002c75b8e20 179 motor2_pwm.write(0.0f);
MatthewMaat 18:8002c75b8e20 180 }
MatthewMaat 18:8002c75b8e20 181 else if(pos_time_counter%500==0)
MatthewMaat 18:8002c75b8e20 182 {
MatthewMaat 18:8002c75b8e20 183 last_ticks=motor2_pos.getPulses();
MatthewMaat 18:8002c75b8e20 184 }
MatthewMaat 18:8002c75b8e20 185 }
MatthewMaat 18:8002c75b8e20 186
MatthewMaat 18:8002c75b8e20 187 }
MatthewMaat 18:8002c75b8e20 188
MatthewMaat 15:c4799ad02cdc 189 void record_min_max(void)
MatthewMaat 15:c4799ad02cdc 190 {
MatthewMaat 15:c4799ad02cdc 191 float t=state_time.read();
MatthewMaat 15:c4799ad02cdc 192 if(t>0.4)
MatthewMaat 15:c4799ad02cdc 193 {
MatthewMaat 15:c4799ad02cdc 194 if(P0<EMG_min0)
MatthewMaat 15:c4799ad02cdc 195 {
MatthewMaat 15:c4799ad02cdc 196 EMG_min0=P0;
MatthewMaat 15:c4799ad02cdc 197 }
MatthewMaat 15:c4799ad02cdc 198 else if(P0>EMG_max0)
MatthewMaat 15:c4799ad02cdc 199 {
MatthewMaat 15:c4799ad02cdc 200 EMG_max0=P0;
MatthewMaat 15:c4799ad02cdc 201 }
MatthewMaat 15:c4799ad02cdc 202 if(P1<EMG_min1)
MatthewMaat 15:c4799ad02cdc 203 {
MatthewMaat 15:c4799ad02cdc 204 EMG_min1=P1;
MatthewMaat 15:c4799ad02cdc 205 }
MatthewMaat 15:c4799ad02cdc 206 else if(P1>EMG_max1)
MatthewMaat 15:c4799ad02cdc 207 {
MatthewMaat 15:c4799ad02cdc 208 EMG_max1=P1;
MatthewMaat 15:c4799ad02cdc 209 }
MatthewMaat 15:c4799ad02cdc 210 }
MatthewMaat 15:c4799ad02cdc 211 }
MatthewMaat 15:c4799ad02cdc 212
MatthewMaat 23:b0222fa7c131 213 void unignore_peaksx(void)
MatthewMaat 15:c4799ad02cdc 214 {
MatthewMaat 23:b0222fa7c131 215 ignore_peaksx=false;
MatthewMaat 23:b0222fa7c131 216 }
MatthewMaat 23:b0222fa7c131 217 void start_ignore_turnx(void)
MatthewMaat 23:b0222fa7c131 218 {
MatthewMaat 23:b0222fa7c131 219 ignore_turnx=true;
MatthewMaat 15:c4799ad02cdc 220 }
MatthewMaat 23:b0222fa7c131 221 void unignore_peaksy(void)
MatthewMaat 16:2d115aa2773e 222 {
MatthewMaat 23:b0222fa7c131 223 ignore_peaksy=false;
MatthewMaat 23:b0222fa7c131 224 }
MatthewMaat 23:b0222fa7c131 225 void start_ignore_turny(void)
MatthewMaat 23:b0222fa7c131 226 {
MatthewMaat 23:b0222fa7c131 227 ignore_turny=true;
MatthewMaat 16:2d115aa2773e 228 }
MatthewMaat 15:c4799ad02cdc 229
MatthewMaat 23:b0222fa7c131 230 void set_v_refxy(void)
MatthewMaat 11:de4a85703169 231 {
MatthewMaat 23:b0222fa7c131 232 static bool motorx_on=false;
MatthewMaat 23:b0222fa7c131 233 static float signx=0.15;
MatthewMaat 15:c4799ad02cdc 234 float Q0;
MatthewMaat 15:c4799ad02cdc 235 Q0=2.0f*(P0-(EMG_min0+EMG_max0)/2.0f)/(EMG_max0-EMG_min0);
MatthewMaat 23:b0222fa7c131 236 static bool motory_on=false;
MatthewMaat 23:b0222fa7c131 237 static float signy=0.15;
MatthewMaat 23:b0222fa7c131 238 float Q1;
MatthewMaat 23:b0222fa7c131 239 Q1=2.0f*(P1-(EMG_min1+EMG_max1)/2.0f)/(EMG_max1-EMG_min1);
MatthewMaat 23:b0222fa7c131 240 if (Q0>-0.2f && !ignore_peaksx)
MatthewMaat 13:ec4708dab45d 241 {
MatthewMaat 23:b0222fa7c131 242 if (motorx_on)
MatthewMaat 15:c4799ad02cdc 243 {
MatthewMaat 23:b0222fa7c131 244 v_refx=0;
MatthewMaat 23:b0222fa7c131 245 EMG_peakx.attach(unignore_peaksx,0.8);
MatthewMaat 23:b0222fa7c131 246 turnx.attach(start_ignore_turnx,1);
MatthewMaat 23:b0222fa7c131 247 ignore_turnx=false;
MatthewMaat 23:b0222fa7c131 248 ignore_peaksx=true;
MatthewMaat 23:b0222fa7c131 249 motorx_on=false;
MatthewMaat 15:c4799ad02cdc 250 }
MatthewMaat 23:b0222fa7c131 251 else if(ignore_turnx)
MatthewMaat 15:c4799ad02cdc 252 {
MatthewMaat 23:b0222fa7c131 253 v_refx=1.0*signx;
MatthewMaat 23:b0222fa7c131 254 EMG_peakx.attach(unignore_peaksx,0.8);
MatthewMaat 23:b0222fa7c131 255 turnx.attach(start_ignore_turnx,1);
MatthewMaat 23:b0222fa7c131 256 ignore_turnx=false;
MatthewMaat 23:b0222fa7c131 257 ignore_peaksx=true;
MatthewMaat 23:b0222fa7c131 258 motorx_on=true;
MatthewMaat 15:c4799ad02cdc 259 }
MatthewMaat 16:2d115aa2773e 260 else
MatthewMaat 16:2d115aa2773e 261 {
MatthewMaat 23:b0222fa7c131 262 signx=-1.0*signx;
MatthewMaat 23:b0222fa7c131 263 v_refx=1.0*signx;
MatthewMaat 23:b0222fa7c131 264 EMG_peakx.attach(unignore_peaksx,1.5);
MatthewMaat 23:b0222fa7c131 265 ignore_peaksx=true;
MatthewMaat 23:b0222fa7c131 266 motorx_on=true;
MatthewMaat 16:2d115aa2773e 267 }
MatthewMaat 13:ec4708dab45d 268 }
MatthewMaat 23:b0222fa7c131 269 if (Q1>-0.2f && !ignore_peaksy)
MatthewMaat 23:b0222fa7c131 270 {
MatthewMaat 23:b0222fa7c131 271 if (motory_on)
MatthewMaat 23:b0222fa7c131 272 {
MatthewMaat 23:b0222fa7c131 273 v_refy=0;
MatthewMaat 23:b0222fa7c131 274 EMG_peaky.attach(unignore_peaksy,0.8);
MatthewMaat 23:b0222fa7c131 275 turny.attach(start_ignore_turny,1);
MatthewMaat 23:b0222fa7c131 276 ignore_turny=false;
MatthewMaat 23:b0222fa7c131 277 ignore_peaksy=true;
MatthewMaat 23:b0222fa7c131 278 motory_on=false;
MatthewMaat 23:b0222fa7c131 279 }
MatthewMaat 23:b0222fa7c131 280 else if(ignore_turny)
MatthewMaat 23:b0222fa7c131 281 {
MatthewMaat 23:b0222fa7c131 282 v_refy=1.0*signy;
MatthewMaat 23:b0222fa7c131 283 EMG_peaky.attach(unignore_peaksy,0.8);
MatthewMaat 23:b0222fa7c131 284 turny.attach(start_ignore_turny,1);
MatthewMaat 23:b0222fa7c131 285 ignore_turny=false;
MatthewMaat 23:b0222fa7c131 286 ignore_peaksy=true;
MatthewMaat 23:b0222fa7c131 287 motory_on=true;
MatthewMaat 23:b0222fa7c131 288 }
MatthewMaat 23:b0222fa7c131 289 else
MatthewMaat 23:b0222fa7c131 290 {
MatthewMaat 23:b0222fa7c131 291 signy=-1.0*signy;
MatthewMaat 23:b0222fa7c131 292 v_refy=1.0*signy;
MatthewMaat 23:b0222fa7c131 293 EMG_peaky.attach(unignore_peaksy,1.5);
MatthewMaat 23:b0222fa7c131 294 ignore_peaksy=true;
MatthewMaat 23:b0222fa7c131 295 motory_on=true;
MatthewMaat 23:b0222fa7c131 296 }
MatthewMaat 23:b0222fa7c131 297 }
MatthewMaat 11:de4a85703169 298 }
MatthewMaat 11:de4a85703169 299
jobjansen 20:0f6a88f29a71 300 void error(void){
jobjansen 20:0f6a88f29a71 301
MatthewMaat 23:b0222fa7c131 302 float dvd=L * (tan(theta2) * pow(tan(theta1),2) + tan(theta1) * pow(tan(theta2),2));//+ is aangepast
jobjansen 20:0f6a88f29a71 303
MatthewMaat 21:a316452da8cd 304 float a = ( -pow(cos(theta1),2) * pow((tan(theta1)+tan(theta2)),2) * pow(tan(theta1),2) ) / dvd; // inverse matrix components of differential x and differential y
MatthewMaat 21:a316452da8cd 305 float b = ( pow(cos(theta1),2) * pow((tan(theta1)+tan(theta2)),2) * tan(theta1) ) / dvd;
MatthewMaat 21:a316452da8cd 306 float c = ( pow(cos(theta2),2) * pow((tan(theta1)+tan(theta2)),2) * pow(tan(theta2),2) ) / dvd;
MatthewMaat 21:a316452da8cd 307 float d = ( pow(cos(theta2),2) * pow((tan(theta1)+tan(theta2)),2) * pow(tan(theta2),2) ) / dvd;
jobjansen 20:0f6a88f29a71 308
jobjansen 20:0f6a88f29a71 309 float theta1_dot = a*v_refx + b*v_refy;
jobjansen 20:0f6a88f29a71 310 float theta2_dot = c*v_refx + d*v_refy;
jobjansen 20:0f6a88f29a71 311
MatthewMaat 21:a316452da8cd 312 float theta1_ref;
MatthewMaat 21:a316452da8cd 313 float theta2_ref;
MatthewMaat 21:a316452da8cd 314 if(currentState==Operating)
MatthewMaat 21:a316452da8cd 315 {
MatthewMaat 21:a316452da8cd 316 theta1_ref = theta1_old+theta1_dot*T;
MatthewMaat 21:a316452da8cd 317 theta2_ref = theta2_old+theta2_dot*T;
MatthewMaat 21:a316452da8cd 318 }
MatthewMaat 21:a316452da8cd 319 else if(currentState==Homing)
MatthewMaat 21:a316452da8cd 320 {
MatthewMaat 21:a316452da8cd 321 theta1_ref=pi/4.0f;
MatthewMaat 21:a316452da8cd 322 theta2_ref=pi/4.0f;
MatthewMaat 21:a316452da8cd 323 }
MatthewMaat 23:b0222fa7c131 324 else if(currentState==Demo)
MatthewMaat 23:b0222fa7c131 325 {
MatthewMaat 23:b0222fa7c131 326 theta1_ref=atan(y_ref/x_ref);
MatthewMaat 23:b0222fa7c131 327 theta2_ref=atan(y_ref/(0.35f-x_ref));
MatthewMaat 23:b0222fa7c131 328 }
MatthewMaat 21:a316452da8cd 329 error_1 = theta1 - theta1_ref;
MatthewMaat 21:a316452da8cd 330 error_2 = theta2 - theta2_ref;
jobjansen 20:0f6a88f29a71 331
jobjansen 20:0f6a88f29a71 332 theta1_old = theta1_ref;
jobjansen 20:0f6a88f29a71 333 theta2_old = theta2_ref;
jobjansen 20:0f6a88f29a71 334
jobjansen 20:0f6a88f29a71 335 }
jobjansen 20:0f6a88f29a71 336
MatthewMaat 21:a316452da8cd 337 float PID1(float err){
jobjansen 20:0f6a88f29a71 338 //P action
MatthewMaat 21:a316452da8cd 339 u_p = Kp * err;
jobjansen 20:0f6a88f29a71 340
jobjansen 20:0f6a88f29a71 341 //I action
MatthewMaat 21:a316452da8cd 342 error_integral1 = error_integral1 + err * T;
MatthewMaat 21:a316452da8cd 343 u_i = Ki * error_integral1;
MatthewMaat 21:a316452da8cd 344
MatthewMaat 21:a316452da8cd 345 /*
MatthewMaat 21:a316452da8cd 346 //D action
MatthewMaat 21:a316452da8cd 347 if(q){
MatthewMaat 21:a316452da8cd 348 error_prev = err;
MatthewMaat 21:a316452da8cd 349 q=false;
MatthewMaat 21:a316452da8cd 350 }
MatthewMaat 21:a316452da8cd 351
MatthewMaat 21:a316452da8cd 352 float error_derivative = (err - error_prev)/T;
MatthewMaat 21:a316452da8cd 353 float filtered_error_derivative = LowPassFilter.step(error_derivative);
MatthewMaat 21:a316452da8cd 354 u_d = Kd * filtered_error_derivative;
MatthewMaat 21:a316452da8cd 355 error_prev = err;
MatthewMaat 21:a316452da8cd 356
MatthewMaat 21:a316452da8cd 357 */
MatthewMaat 21:a316452da8cd 358
MatthewMaat 21:a316452da8cd 359 u = u_p + u_i;
MatthewMaat 21:a316452da8cd 360
MatthewMaat 21:a316452da8cd 361 return u;
MatthewMaat 21:a316452da8cd 362 }
MatthewMaat 21:a316452da8cd 363
MatthewMaat 21:a316452da8cd 364 float PID2(float err){
MatthewMaat 21:a316452da8cd 365 //P action
MatthewMaat 21:a316452da8cd 366 u_p = Kp * err;
MatthewMaat 21:a316452da8cd 367
MatthewMaat 21:a316452da8cd 368 //I action
MatthewMaat 21:a316452da8cd 369 error_integral2 = error_integral2 + err * T;
MatthewMaat 21:a316452da8cd 370 u_i = Ki * error_integral2;
jobjansen 20:0f6a88f29a71 371
jobjansen 20:0f6a88f29a71 372 /*
jobjansen 20:0f6a88f29a71 373 //D action
jobjansen 20:0f6a88f29a71 374 if(q){
jobjansen 20:0f6a88f29a71 375 error_prev = err;
jobjansen 20:0f6a88f29a71 376 q=false;
jobjansen 20:0f6a88f29a71 377 }
jobjansen 20:0f6a88f29a71 378
jobjansen 20:0f6a88f29a71 379 float error_derivative = (err - error_prev)/T;
jobjansen 20:0f6a88f29a71 380 float filtered_error_derivative = LowPassFilter.step(error_derivative);
jobjansen 20:0f6a88f29a71 381 u_d = Kd * filtered_error_derivative;
jobjansen 20:0f6a88f29a71 382 error_prev = err;
jobjansen 20:0f6a88f29a71 383
jobjansen 20:0f6a88f29a71 384 */
jobjansen 20:0f6a88f29a71 385
jobjansen 20:0f6a88f29a71 386 u = u_p + u_i;
jobjansen 20:0f6a88f29a71 387
jobjansen 20:0f6a88f29a71 388 return u;
jobjansen 20:0f6a88f29a71 389 }
MatthewMaat 23:b0222fa7c131 390 void set_refxy(void)
MatthewMaat 23:b0222fa7c131 391 {
MatthewMaat 23:b0222fa7c131 392 float t=state_time.read();
MatthewMaat 23:b0222fa7c131 393 int tfloor=floor(t)/1;
MatthewMaat 23:b0222fa7c131 394 if(tfloor%8==0||tfloor%8==1)
MatthewMaat 23:b0222fa7c131 395 {
MatthewMaat 23:b0222fa7c131 396 x_ref=0.075+0.05*(t-tfloor+tfloor%8);
MatthewMaat 23:b0222fa7c131 397 y_ref=0.2-0.05*(t-tfloor+tfloor%8);
MatthewMaat 23:b0222fa7c131 398 }
MatthewMaat 23:b0222fa7c131 399 else if(tfloor%8==2||tfloor%8==3)
MatthewMaat 23:b0222fa7c131 400 {
MatthewMaat 23:b0222fa7c131 401 x_ref=0.175+0.05*(t-tfloor+tfloor%8-2);
MatthewMaat 23:b0222fa7c131 402 y_ref=0.1+0.05*(t-tfloor+tfloor%8-2);
MatthewMaat 23:b0222fa7c131 403 }
MatthewMaat 23:b0222fa7c131 404 else if(tfloor%8==4||tfloor%8==5)
MatthewMaat 23:b0222fa7c131 405 {
MatthewMaat 23:b0222fa7c131 406 x_ref=0.275-0.05*(t-tfloor+tfloor%8-4);
MatthewMaat 23:b0222fa7c131 407 y_ref=0.2+0.05*(t-tfloor+tfloor%8-4);
MatthewMaat 23:b0222fa7c131 408 }
MatthewMaat 23:b0222fa7c131 409 else
MatthewMaat 23:b0222fa7c131 410 {
MatthewMaat 23:b0222fa7c131 411 x_ref=0.175-0.05*(t-tfloor+tfloor%8-6);
MatthewMaat 23:b0222fa7c131 412 y_ref=0.3-0.05*(t-tfloor+tfloor%8-6);
MatthewMaat 23:b0222fa7c131 413 }
MatthewMaat 23:b0222fa7c131 414 }
MatthewMaat 23:b0222fa7c131 415
jobjansen 20:0f6a88f29a71 416 void setPWM_controller(void){
MatthewMaat 21:a316452da8cd 417 error();
MatthewMaat 21:a316452da8cd 418 u_1 = PID1(error_1);
MatthewMaat 21:a316452da8cd 419 u_2 = PID2(error_2);
jobjansen 20:0f6a88f29a71 420
jobjansen 20:0f6a88f29a71 421 if(u_1 < 0.0f){
MatthewMaat 23:b0222fa7c131 422 dir1 = 0;
jobjansen 20:0f6a88f29a71 423 }else{
MatthewMaat 23:b0222fa7c131 424 dir1 = 1;
jobjansen 20:0f6a88f29a71 425 }
MatthewMaat 23:b0222fa7c131 426 motor1_pwm.write(min(fabs(u_1),1.0f));
jobjansen 20:0f6a88f29a71 427
jobjansen 20:0f6a88f29a71 428 if(u_2 < 0.0f){
MatthewMaat 23:b0222fa7c131 429 dir2 = 0;
jobjansen 20:0f6a88f29a71 430 }else{
MatthewMaat 23:b0222fa7c131 431 dir2 = 1;
jobjansen 20:0f6a88f29a71 432 }
MatthewMaat 23:b0222fa7c131 433 motor2_pwm.write(min(fabs(u_2),1.0f));
jobjansen 20:0f6a88f29a71 434
jobjansen 20:0f6a88f29a71 435 }
jobjansen 20:0f6a88f29a71 436
MatthewMaat 14:dc89250ebc52 437 void sample()
MatthewMaat 14:dc89250ebc52 438 {
MatthewMaat 18:8002c75b8e20 439 get_angles();
MatthewMaat 18:8002c75b8e20 440 scope.set(0,P0);
MatthewMaat 18:8002c75b8e20 441 scope.set(1,P1);
MatthewMaat 18:8002c75b8e20 442 scope.set(2,theta1);
MatthewMaat 18:8002c75b8e20 443 scope.set(3,theta2);
MatthewMaat 18:8002c75b8e20 444 scope.send();
MatthewMaat 14:dc89250ebc52 445 switch(currentState)
MatthewMaat 14:dc89250ebc52 446 {
MatthewMaat 15:c4799ad02cdc 447 case Waiting:
MatthewMaat 15:c4799ad02cdc 448 ledred=0;
MatthewMaat 15:c4799ad02cdc 449 ledgreen=0;
MatthewMaat 15:c4799ad02cdc 450 ledblue=1;
MatthewMaat 15:c4799ad02cdc 451 break;
MatthewMaat 15:c4799ad02cdc 452 case Position_calibration:
MatthewMaat 15:c4799ad02cdc 453 ledred=1;
MatthewMaat 15:c4799ad02cdc 454 ledgreen=1;
MatthewMaat 15:c4799ad02cdc 455 ledblue=0;
MatthewMaat 18:8002c75b8e20 456 pos_cal();
MatthewMaat 15:c4799ad02cdc 457 break;
MatthewMaat 15:c4799ad02cdc 458 case EMG_calibration:
MatthewMaat 15:c4799ad02cdc 459 ledred=1;
MatthewMaat 15:c4799ad02cdc 460 ledgreen=0;
MatthewMaat 15:c4799ad02cdc 461 ledblue=0;
MatthewMaat 15:c4799ad02cdc 462 read_emg();
MatthewMaat 15:c4799ad02cdc 463 record_min_max();
MatthewMaat 15:c4799ad02cdc 464 break;
MatthewMaat 15:c4799ad02cdc 465 case Homing:
MatthewMaat 21:a316452da8cd 466 setPWM_controller();
MatthewMaat 15:c4799ad02cdc 467 ledred=0;
MatthewMaat 15:c4799ad02cdc 468 ledgreen=1;
MatthewMaat 15:c4799ad02cdc 469 ledblue=0;
MatthewMaat 15:c4799ad02cdc 470 break;
MatthewMaat 14:dc89250ebc52 471 case Operating:
MatthewMaat 14:dc89250ebc52 472 read_emg();
MatthewMaat 23:b0222fa7c131 473 set_v_refxy();
MatthewMaat 23:b0222fa7c131 474 setPWM_controller();
MatthewMaat 15:c4799ad02cdc 475 ledred=1;
MatthewMaat 15:c4799ad02cdc 476 ledgreen=0;
MatthewMaat 15:c4799ad02cdc 477 ledblue=1;
MatthewMaat 15:c4799ad02cdc 478 break;
MatthewMaat 15:c4799ad02cdc 479 case Demo:
MatthewMaat 23:b0222fa7c131 480 set_refxy();
MatthewMaat 23:b0222fa7c131 481 setPWM_controller();
MatthewMaat 15:c4799ad02cdc 482 ledred=0;
MatthewMaat 15:c4799ad02cdc 483 ledgreen=0;
MatthewMaat 15:c4799ad02cdc 484 ledblue=0;
MatthewMaat 14:dc89250ebc52 485 break;
MatthewMaat 14:dc89250ebc52 486 case Failure:
MatthewMaat 14:dc89250ebc52 487 ledred=0;
MatthewMaat 14:dc89250ebc52 488 ledgreen=1;
MatthewMaat 14:dc89250ebc52 489 ledblue=1;
MatthewMaat 15:c4799ad02cdc 490 motor1_pwm.write(0.0);
MatthewMaat 15:c4799ad02cdc 491 motor2_pwm.write(0.0);
MatthewMaat 14:dc89250ebc52 492 break;
MatthewMaat 14:dc89250ebc52 493 }
MatthewMaat 21:a316452da8cd 494 //Preventing the machine from breaking
MatthewMaat 21:a316452da8cd 495 if(theta1>=1.6f)
MatthewMaat 21:a316452da8cd 496 {
MatthewMaat 21:a316452da8cd 497 dir1=1;
MatthewMaat 21:a316452da8cd 498 }
MatthewMaat 21:a316452da8cd 499 if(theta2>=1.6f)
MatthewMaat 21:a316452da8cd 500 {
MatthewMaat 21:a316452da8cd 501 dir2=2;
MatthewMaat 21:a316452da8cd 502 }
MatthewMaat 14:dc89250ebc52 503 }
MatthewMaat 14:dc89250ebc52 504
MatthewMaat 14:dc89250ebc52 505 void error_occur()
MatthewMaat 14:dc89250ebc52 506 {
MatthewMaat 14:dc89250ebc52 507 currentState=Failure;
MatthewMaat 14:dc89250ebc52 508 }
MatthewMaat 14:dc89250ebc52 509
MatthewMaat 18:8002c75b8e20 510 void button_press(void)
MatthewMaat 15:c4799ad02cdc 511 //Press button to change state
MatthewMaat 15:c4799ad02cdc 512 {
MatthewMaat 18:8002c75b8e20 513 state_time.reset();
MatthewMaat 15:c4799ad02cdc 514 switch(currentState)
MatthewMaat 15:c4799ad02cdc 515 {
MatthewMaat 15:c4799ad02cdc 516 case Waiting:
MatthewMaat 15:c4799ad02cdc 517 currentState=Position_calibration;
MatthewMaat 15:c4799ad02cdc 518 wait(1);
MatthewMaat 15:c4799ad02cdc 519 break;
MatthewMaat 15:c4799ad02cdc 520 case Position_calibration:
MatthewMaat 21:a316452da8cd 521 if(!motor1_calibrated)
MatthewMaat 21:a316452da8cd 522 {
MatthewMaat 21:a316452da8cd 523 motor1_calibrated=true;
MatthewMaat 22:335a30b0825d 524 state_time.reset();
MatthewMaat 22:335a30b0825d 525 dir1=!dir1;
MatthewMaat 23:b0222fa7c131 526 motor1_pwm.write(0.0f);
MatthewMaat 21:a316452da8cd 527 }
MatthewMaat 21:a316452da8cd 528 else
MatthewMaat 21:a316452da8cd 529 {
MatthewMaat 21:a316452da8cd 530 currentState=EMG_calibration;
MatthewMaat 23:b0222fa7c131 531 motor2_pwm.write(0.0f);
MatthewMaat 23:b0222fa7c131 532 motor1_pwm.write(0.0f);
MatthewMaat 21:a316452da8cd 533 }
MatthewMaat 15:c4799ad02cdc 534 wait(1);
MatthewMaat 15:c4799ad02cdc 535 break;
MatthewMaat 15:c4799ad02cdc 536 case EMG_calibration:
MatthewMaat 15:c4799ad02cdc 537 currentState=Homing;
MatthewMaat 15:c4799ad02cdc 538 wait(1);
MatthewMaat 15:c4799ad02cdc 539 break;
MatthewMaat 15:c4799ad02cdc 540 case Homing:
MatthewMaat 15:c4799ad02cdc 541 currentState=Operating;
MatthewMaat 15:c4799ad02cdc 542 wait(1);
MatthewMaat 15:c4799ad02cdc 543 break;
MatthewMaat 15:c4799ad02cdc 544 case Operating:
MatthewMaat 15:c4799ad02cdc 545 currentState=Demo;
MatthewMaat 15:c4799ad02cdc 546 wait(1);
MatthewMaat 15:c4799ad02cdc 547 break;
MatthewMaat 15:c4799ad02cdc 548 case Demo:
MatthewMaat 15:c4799ad02cdc 549 currentState=Operating;
MatthewMaat 15:c4799ad02cdc 550 wait(1);
MatthewMaat 15:c4799ad02cdc 551 break;
MatthewMaat 15:c4799ad02cdc 552 }
MatthewMaat 15:c4799ad02cdc 553 }
MatthewMaat 15:c4799ad02cdc 554
MatthewMaat 8:ec3c634390c7 555 int main()
MatthewMaat 4:f988679bf9a1 556 {
MatthewMaat 14:dc89250ebc52 557 pc.baud(115200);
MatthewMaat 14:dc89250ebc52 558 pc.printf("Starting...");
MatthewMaat 14:dc89250ebc52 559 ledred=0;
MatthewMaat 13:ec4708dab45d 560 sample_timer.attach(&sample, 0.002);
MatthewMaat 14:dc89250ebc52 561 err.fall(error_occur);
MatthewMaat 15:c4799ad02cdc 562 button.fall(button_press);
MatthewMaat 12:7f280a661e71 563 int frequency_pwm=10000;
MatthewMaat 12:7f280a661e71 564 motor1_pwm.period(1.0/frequency_pwm);
MatthewMaat 13:ec4708dab45d 565 motor2_pwm.period(1.0/frequency_pwm);
MatthewMaat 18:8002c75b8e20 566 state_time.start();
MatthewMaat 8:ec3c634390c7 567 while (true) {
MatthewMaat 12:7f280a661e71 568 wait(10);
MatthewMaat 4:f988679bf9a1 569 }
jobjansen 20:0f6a88f29a71 570
jobjansen 20:0f6a88f29a71 571 }