Matthew Maat / Mbed 2 deprecated template_biorobotics

fancy lampje

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FXOS8700Q FastPWM

main.cpp@24:3c9ac44890f0, 2019-10-25 (annotated)

Committer:
MatthewMaat
Date:
Fri Oct 25 12:25:48 2019 +0000
Revision:
24:3c9ac44890f0
Parent:
23:b0222fa7c131
Child:
25:2cc29fa67345
Versie met originele Jacobian

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 24:3c9ac44890f0 298 float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
MatthewMaat 24:3c9ac44890f0 299 float y_pos=tan(theta1)*x_pos;
MatthewMaat 24:3c9ac44890f0 300 if(x_pos>=0.35&&v_refx>0)
MatthewMaat 24:3c9ac44890f0 301 {
MatthewMaat 24:3c9ac44890f0 302 v_refx=0;
MatthewMaat 24:3c9ac44890f0 303 }
MatthewMaat 24:3c9ac44890f0 304 else if(x_pos<=0.0&&v_refx<0)
MatthewMaat 24:3c9ac44890f0 305 {
MatthewMaat 24:3c9ac44890f0 306 v_refx=0;
MatthewMaat 24:3c9ac44890f0 307 }
MatthewMaat 24:3c9ac44890f0 308 if(y_pos>=0.45&&v_refy>0)
MatthewMaat 24:3c9ac44890f0 309 {
MatthewMaat 24:3c9ac44890f0 310 v_refy=0;
MatthewMaat 24:3c9ac44890f0 311 }
MatthewMaat 24:3c9ac44890f0 312 else if(y_pos<=0.07&&v_refy<0)
MatthewMaat 24:3c9ac44890f0 313 {
MatthewMaat 24:3c9ac44890f0 314 v_refy=0;
MatthewMaat 24:3c9ac44890f0 315 }
MatthewMaat 11:de4a85703169 316 }
MatthewMaat 11:de4a85703169 317
jobjansen 20:0f6a88f29a71 318 void error(void){
jobjansen 20:0f6a88f29a71 319
MatthewMaat 23:b0222fa7c131 320 float dvd=L * (tan(theta2) * pow(tan(theta1),2) + tan(theta1) * pow(tan(theta2),2));//+ is aangepast
jobjansen 20:0f6a88f29a71 321
MatthewMaat 21:a316452da8cd 322 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 323 float b = ( pow(cos(theta1),2) * pow((tan(theta1)+tan(theta2)),2) * tan(theta1) ) / dvd;
MatthewMaat 21:a316452da8cd 324 float c = ( pow(cos(theta2),2) * pow((tan(theta1)+tan(theta2)),2) * pow(tan(theta2),2) ) / dvd;
MatthewMaat 21:a316452da8cd 325 float d = ( pow(cos(theta2),2) * pow((tan(theta1)+tan(theta2)),2) * pow(tan(theta2),2) ) / dvd;
jobjansen 20:0f6a88f29a71 326
jobjansen 20:0f6a88f29a71 327 float theta1_dot = a*v_refx + b*v_refy;
jobjansen 20:0f6a88f29a71 328 float theta2_dot = c*v_refx + d*v_refy;
jobjansen 20:0f6a88f29a71 329
MatthewMaat 21:a316452da8cd 330 float theta1_ref;
MatthewMaat 21:a316452da8cd 331 float theta2_ref;
MatthewMaat 21:a316452da8cd 332 if(currentState==Operating)
MatthewMaat 21:a316452da8cd 333 {
MatthewMaat 21:a316452da8cd 334 theta1_ref = theta1_old+theta1_dot*T;
MatthewMaat 21:a316452da8cd 335 theta2_ref = theta2_old+theta2_dot*T;
MatthewMaat 24:3c9ac44890f0 336 if(theta1_ref<0.1f)
MatthewMaat 24:3c9ac44890f0 337 {
MatthewMaat 24:3c9ac44890f0 338 theta1_ref=0.1f;
MatthewMaat 24:3c9ac44890f0 339 }
MatthewMaat 24:3c9ac44890f0 340 else if(theta1_ref<1.6f)
MatthewMaat 24:3c9ac44890f0 341 {
MatthewMaat 24:3c9ac44890f0 342 theta1_ref=1.6f;
MatthewMaat 24:3c9ac44890f0 343 }
MatthewMaat 24:3c9ac44890f0 344 if(theta2_ref<0.1f)
MatthewMaat 24:3c9ac44890f0 345 {
MatthewMaat 24:3c9ac44890f0 346 theta2_ref=0.1f;
MatthewMaat 24:3c9ac44890f0 347 }
MatthewMaat 24:3c9ac44890f0 348 else if(theta2_ref<1.6f)
MatthewMaat 24:3c9ac44890f0 349 {
MatthewMaat 24:3c9ac44890f0 350 theta2_ref=1.6f;
MatthewMaat 24:3c9ac44890f0 351 }
MatthewMaat 21:a316452da8cd 352 }
MatthewMaat 21:a316452da8cd 353 else if(currentState==Homing)
MatthewMaat 21:a316452da8cd 354 {
MatthewMaat 21:a316452da8cd 355 theta1_ref=pi/4.0f;
MatthewMaat 21:a316452da8cd 356 theta2_ref=pi/4.0f;
MatthewMaat 21:a316452da8cd 357 }
MatthewMaat 23:b0222fa7c131 358 else if(currentState==Demo)
MatthewMaat 23:b0222fa7c131 359 {
MatthewMaat 23:b0222fa7c131 360 theta1_ref=atan(y_ref/x_ref);
MatthewMaat 23:b0222fa7c131 361 theta2_ref=atan(y_ref/(0.35f-x_ref));
MatthewMaat 23:b0222fa7c131 362 }
MatthewMaat 21:a316452da8cd 363 error_1 = theta1 - theta1_ref;
MatthewMaat 21:a316452da8cd 364 error_2 = theta2 - theta2_ref;
jobjansen 20:0f6a88f29a71 365
jobjansen 20:0f6a88f29a71 366 theta1_old = theta1_ref;
jobjansen 20:0f6a88f29a71 367 theta2_old = theta2_ref;
jobjansen 20:0f6a88f29a71 368
jobjansen 20:0f6a88f29a71 369 }
jobjansen 20:0f6a88f29a71 370
MatthewMaat 21:a316452da8cd 371 float PID1(float err){
jobjansen 20:0f6a88f29a71 372 //P action
MatthewMaat 21:a316452da8cd 373 u_p = Kp * err;
jobjansen 20:0f6a88f29a71 374
jobjansen 20:0f6a88f29a71 375 //I action
MatthewMaat 21:a316452da8cd 376 error_integral1 = error_integral1 + err * T;
MatthewMaat 21:a316452da8cd 377 u_i = Ki * error_integral1;
MatthewMaat 21:a316452da8cd 378
MatthewMaat 21:a316452da8cd 379 /*
MatthewMaat 21:a316452da8cd 380 //D action
MatthewMaat 21:a316452da8cd 381 if(q){
MatthewMaat 21:a316452da8cd 382 error_prev = err;
MatthewMaat 21:a316452da8cd 383 q=false;
MatthewMaat 21:a316452da8cd 384 }
MatthewMaat 21:a316452da8cd 385
MatthewMaat 21:a316452da8cd 386 float error_derivative = (err - error_prev)/T;
MatthewMaat 21:a316452da8cd 387 float filtered_error_derivative = LowPassFilter.step(error_derivative);
MatthewMaat 21:a316452da8cd 388 u_d = Kd * filtered_error_derivative;
MatthewMaat 21:a316452da8cd 389 error_prev = err;
MatthewMaat 21:a316452da8cd 390
MatthewMaat 21:a316452da8cd 391 */
MatthewMaat 21:a316452da8cd 392
MatthewMaat 21:a316452da8cd 393 u = u_p + u_i;
MatthewMaat 21:a316452da8cd 394
MatthewMaat 21:a316452da8cd 395 return u;
MatthewMaat 21:a316452da8cd 396 }
MatthewMaat 21:a316452da8cd 397
MatthewMaat 21:a316452da8cd 398 float PID2(float err){
MatthewMaat 21:a316452da8cd 399 //P action
MatthewMaat 21:a316452da8cd 400 u_p = Kp * err;
MatthewMaat 21:a316452da8cd 401
MatthewMaat 21:a316452da8cd 402 //I action
MatthewMaat 21:a316452da8cd 403 error_integral2 = error_integral2 + err * T;
MatthewMaat 21:a316452da8cd 404 u_i = Ki * error_integral2;
jobjansen 20:0f6a88f29a71 405
jobjansen 20:0f6a88f29a71 406 /*
jobjansen 20:0f6a88f29a71 407 //D action
jobjansen 20:0f6a88f29a71 408 if(q){
jobjansen 20:0f6a88f29a71 409 error_prev = err;
jobjansen 20:0f6a88f29a71 410 q=false;
jobjansen 20:0f6a88f29a71 411 }
jobjansen 20:0f6a88f29a71 412
jobjansen 20:0f6a88f29a71 413 float error_derivative = (err - error_prev)/T;
jobjansen 20:0f6a88f29a71 414 float filtered_error_derivative = LowPassFilter.step(error_derivative);
jobjansen 20:0f6a88f29a71 415 u_d = Kd * filtered_error_derivative;
jobjansen 20:0f6a88f29a71 416 error_prev = err;
jobjansen 20:0f6a88f29a71 417
jobjansen 20:0f6a88f29a71 418 */
jobjansen 20:0f6a88f29a71 419
jobjansen 20:0f6a88f29a71 420 u = u_p + u_i;
jobjansen 20:0f6a88f29a71 421
jobjansen 20:0f6a88f29a71 422 return u;
jobjansen 20:0f6a88f29a71 423 }
MatthewMaat 23:b0222fa7c131 424 void set_refxy(void)
MatthewMaat 23:b0222fa7c131 425 {
MatthewMaat 23:b0222fa7c131 426 float t=state_time.read();
MatthewMaat 23:b0222fa7c131 427 int tfloor=floor(t)/1;
MatthewMaat 23:b0222fa7c131 428 if(tfloor%8==0||tfloor%8==1)
MatthewMaat 23:b0222fa7c131 429 {
MatthewMaat 23:b0222fa7c131 430 x_ref=0.075+0.05*(t-tfloor+tfloor%8);
MatthewMaat 23:b0222fa7c131 431 y_ref=0.2-0.05*(t-tfloor+tfloor%8);
MatthewMaat 23:b0222fa7c131 432 }
MatthewMaat 23:b0222fa7c131 433 else if(tfloor%8==2||tfloor%8==3)
MatthewMaat 23:b0222fa7c131 434 {
MatthewMaat 23:b0222fa7c131 435 x_ref=0.175+0.05*(t-tfloor+tfloor%8-2);
MatthewMaat 23:b0222fa7c131 436 y_ref=0.1+0.05*(t-tfloor+tfloor%8-2);
MatthewMaat 23:b0222fa7c131 437 }
MatthewMaat 23:b0222fa7c131 438 else if(tfloor%8==4||tfloor%8==5)
MatthewMaat 23:b0222fa7c131 439 {
MatthewMaat 23:b0222fa7c131 440 x_ref=0.275-0.05*(t-tfloor+tfloor%8-4);
MatthewMaat 23:b0222fa7c131 441 y_ref=0.2+0.05*(t-tfloor+tfloor%8-4);
MatthewMaat 23:b0222fa7c131 442 }
MatthewMaat 23:b0222fa7c131 443 else
MatthewMaat 23:b0222fa7c131 444 {
MatthewMaat 23:b0222fa7c131 445 x_ref=0.175-0.05*(t-tfloor+tfloor%8-6);
MatthewMaat 23:b0222fa7c131 446 y_ref=0.3-0.05*(t-tfloor+tfloor%8-6);
MatthewMaat 23:b0222fa7c131 447 }
MatthewMaat 23:b0222fa7c131 448 }
MatthewMaat 23:b0222fa7c131 449
jobjansen 20:0f6a88f29a71 450 void setPWM_controller(void){
MatthewMaat 21:a316452da8cd 451 error();
MatthewMaat 21:a316452da8cd 452 u_1 = PID1(error_1);
MatthewMaat 21:a316452da8cd 453 u_2 = PID2(error_2);
jobjansen 20:0f6a88f29a71 454
jobjansen 20:0f6a88f29a71 455 if(u_1 < 0.0f){
MatthewMaat 23:b0222fa7c131 456 dir1 = 0;
jobjansen 20:0f6a88f29a71 457 }else{
MatthewMaat 23:b0222fa7c131 458 dir1 = 1;
jobjansen 20:0f6a88f29a71 459 }
MatthewMaat 23:b0222fa7c131 460 motor1_pwm.write(min(fabs(u_1),1.0f));
jobjansen 20:0f6a88f29a71 461
jobjansen 20:0f6a88f29a71 462 if(u_2 < 0.0f){
MatthewMaat 23:b0222fa7c131 463 dir2 = 0;
jobjansen 20:0f6a88f29a71 464 }else{
MatthewMaat 23:b0222fa7c131 465 dir2 = 1;
jobjansen 20:0f6a88f29a71 466 }
MatthewMaat 23:b0222fa7c131 467 motor2_pwm.write(min(fabs(u_2),1.0f));
jobjansen 20:0f6a88f29a71 468
jobjansen 20:0f6a88f29a71 469 }
jobjansen 20:0f6a88f29a71 470
MatthewMaat 14:dc89250ebc52 471 void sample()
MatthewMaat 14:dc89250ebc52 472 {
MatthewMaat 18:8002c75b8e20 473 get_angles();
MatthewMaat 18:8002c75b8e20 474 scope.set(0,P0);
MatthewMaat 18:8002c75b8e20 475 scope.set(1,P1);
MatthewMaat 24:3c9ac44890f0 476 scope.set(2,v_refx);
MatthewMaat 24:3c9ac44890f0 477 scope.set(3,v_refy);
MatthewMaat 18:8002c75b8e20 478 scope.send();
MatthewMaat 14:dc89250ebc52 479 switch(currentState)
MatthewMaat 14:dc89250ebc52 480 {
MatthewMaat 15:c4799ad02cdc 481 case Waiting:
MatthewMaat 15:c4799ad02cdc 482 ledred=0;
MatthewMaat 15:c4799ad02cdc 483 ledgreen=0;
MatthewMaat 15:c4799ad02cdc 484 ledblue=1;
MatthewMaat 15:c4799ad02cdc 485 break;
MatthewMaat 15:c4799ad02cdc 486 case Position_calibration:
MatthewMaat 15:c4799ad02cdc 487 ledred=1;
MatthewMaat 15:c4799ad02cdc 488 ledgreen=1;
MatthewMaat 15:c4799ad02cdc 489 ledblue=0;
MatthewMaat 18:8002c75b8e20 490 pos_cal();
MatthewMaat 15:c4799ad02cdc 491 break;
MatthewMaat 15:c4799ad02cdc 492 case EMG_calibration:
MatthewMaat 15:c4799ad02cdc 493 ledred=1;
MatthewMaat 15:c4799ad02cdc 494 ledgreen=0;
MatthewMaat 15:c4799ad02cdc 495 ledblue=0;
MatthewMaat 15:c4799ad02cdc 496 read_emg();
MatthewMaat 15:c4799ad02cdc 497 record_min_max();
MatthewMaat 15:c4799ad02cdc 498 break;
MatthewMaat 15:c4799ad02cdc 499 case Homing:
MatthewMaat 21:a316452da8cd 500 setPWM_controller();
MatthewMaat 15:c4799ad02cdc 501 ledred=0;
MatthewMaat 15:c4799ad02cdc 502 ledgreen=1;
MatthewMaat 15:c4799ad02cdc 503 ledblue=0;
MatthewMaat 15:c4799ad02cdc 504 break;
MatthewMaat 14:dc89250ebc52 505 case Operating:
MatthewMaat 14:dc89250ebc52 506 read_emg();
MatthewMaat 23:b0222fa7c131 507 set_v_refxy();
MatthewMaat 23:b0222fa7c131 508 setPWM_controller();
MatthewMaat 15:c4799ad02cdc 509 ledred=1;
MatthewMaat 15:c4799ad02cdc 510 ledgreen=0;
MatthewMaat 15:c4799ad02cdc 511 ledblue=1;
MatthewMaat 15:c4799ad02cdc 512 break;
MatthewMaat 15:c4799ad02cdc 513 case Demo:
MatthewMaat 23:b0222fa7c131 514 set_refxy();
MatthewMaat 23:b0222fa7c131 515 setPWM_controller();
MatthewMaat 15:c4799ad02cdc 516 ledred=0;
MatthewMaat 15:c4799ad02cdc 517 ledgreen=0;
MatthewMaat 15:c4799ad02cdc 518 ledblue=0;
MatthewMaat 14:dc89250ebc52 519 break;
MatthewMaat 14:dc89250ebc52 520 case Failure:
MatthewMaat 14:dc89250ebc52 521 ledred=0;
MatthewMaat 14:dc89250ebc52 522 ledgreen=1;
MatthewMaat 14:dc89250ebc52 523 ledblue=1;
MatthewMaat 15:c4799ad02cdc 524 motor1_pwm.write(0.0);
MatthewMaat 15:c4799ad02cdc 525 motor2_pwm.write(0.0);
MatthewMaat 14:dc89250ebc52 526 break;
MatthewMaat 14:dc89250ebc52 527 }
MatthewMaat 21:a316452da8cd 528 //Preventing the machine from breaking
MatthewMaat 14:dc89250ebc52 529 }
MatthewMaat 14:dc89250ebc52 530
MatthewMaat 14:dc89250ebc52 531 void error_occur()
MatthewMaat 14:dc89250ebc52 532 {
MatthewMaat 14:dc89250ebc52 533 currentState=Failure;
MatthewMaat 14:dc89250ebc52 534 }
MatthewMaat 14:dc89250ebc52 535
MatthewMaat 18:8002c75b8e20 536 void button_press(void)
MatthewMaat 15:c4799ad02cdc 537 //Press button to change state
MatthewMaat 15:c4799ad02cdc 538 {
MatthewMaat 18:8002c75b8e20 539 state_time.reset();
MatthewMaat 15:c4799ad02cdc 540 switch(currentState)
MatthewMaat 15:c4799ad02cdc 541 {
MatthewMaat 15:c4799ad02cdc 542 case Waiting:
MatthewMaat 15:c4799ad02cdc 543 currentState=Position_calibration;
MatthewMaat 15:c4799ad02cdc 544 wait(1);
MatthewMaat 15:c4799ad02cdc 545 break;
MatthewMaat 15:c4799ad02cdc 546 case Position_calibration:
MatthewMaat 21:a316452da8cd 547 if(!motor1_calibrated)
MatthewMaat 21:a316452da8cd 548 {
MatthewMaat 21:a316452da8cd 549 motor1_calibrated=true;
MatthewMaat 22:335a30b0825d 550 state_time.reset();
MatthewMaat 22:335a30b0825d 551 dir1=!dir1;
MatthewMaat 23:b0222fa7c131 552 motor1_pwm.write(0.0f);
MatthewMaat 21:a316452da8cd 553 }
MatthewMaat 21:a316452da8cd 554 else
MatthewMaat 21:a316452da8cd 555 {
MatthewMaat 21:a316452da8cd 556 currentState=EMG_calibration;
MatthewMaat 23:b0222fa7c131 557 motor2_pwm.write(0.0f);
MatthewMaat 23:b0222fa7c131 558 motor1_pwm.write(0.0f);
MatthewMaat 21:a316452da8cd 559 }
MatthewMaat 15:c4799ad02cdc 560 wait(1);
MatthewMaat 15:c4799ad02cdc 561 break;
MatthewMaat 15:c4799ad02cdc 562 case EMG_calibration:
MatthewMaat 15:c4799ad02cdc 563 currentState=Homing;
MatthewMaat 15:c4799ad02cdc 564 wait(1);
MatthewMaat 15:c4799ad02cdc 565 break;
MatthewMaat 15:c4799ad02cdc 566 case Homing:
MatthewMaat 15:c4799ad02cdc 567 currentState=Operating;
MatthewMaat 15:c4799ad02cdc 568 wait(1);
MatthewMaat 15:c4799ad02cdc 569 break;
MatthewMaat 15:c4799ad02cdc 570 case Operating:
MatthewMaat 15:c4799ad02cdc 571 currentState=Demo;
MatthewMaat 15:c4799ad02cdc 572 wait(1);
MatthewMaat 15:c4799ad02cdc 573 break;
MatthewMaat 15:c4799ad02cdc 574 case Demo:
MatthewMaat 15:c4799ad02cdc 575 currentState=Operating;
MatthewMaat 15:c4799ad02cdc 576 wait(1);
MatthewMaat 15:c4799ad02cdc 577 break;
MatthewMaat 15:c4799ad02cdc 578 }
MatthewMaat 15:c4799ad02cdc 579 }
MatthewMaat 15:c4799ad02cdc 580
MatthewMaat 8:ec3c634390c7 581 int main()
MatthewMaat 4:f988679bf9a1 582 {
MatthewMaat 14:dc89250ebc52 583 pc.baud(115200);
MatthewMaat 14:dc89250ebc52 584 pc.printf("Starting...");
MatthewMaat 14:dc89250ebc52 585 ledred=0;
MatthewMaat 13:ec4708dab45d 586 sample_timer.attach(&sample, 0.002);
MatthewMaat 14:dc89250ebc52 587 err.fall(error_occur);
MatthewMaat 15:c4799ad02cdc 588 button.fall(button_press);
MatthewMaat 12:7f280a661e71 589 int frequency_pwm=10000;
MatthewMaat 12:7f280a661e71 590 motor1_pwm.period(1.0/frequency_pwm);
MatthewMaat 13:ec4708dab45d 591 motor2_pwm.period(1.0/frequency_pwm);
MatthewMaat 18:8002c75b8e20 592 state_time.start();
MatthewMaat 8:ec3c634390c7 593 while (true) {
MatthewMaat 12:7f280a661e71 594 wait(10);
MatthewMaat 4:f988679bf9a1 595 }
jobjansen 20:0f6a88f29a71 596
jobjansen 20:0f6a88f29a71 597 }