Matthew Maat / Mbed 2 deprecated template_biorobotics

fancy lampje

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FXOS8700Q FastPWM

main.cpp@26:88639564e3af, 2019-10-30 (annotated)

Committer:
MatthewMaat
Date:
Wed Oct 30 11:34:26 2019 +0000
Revision:
26:88639564e3af
Parent:
25:2cc29fa67345
Child:
27:c15170a5cd3d
Coole demo modus

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 25:2cc29fa67345 28 HIDScope scope( 6);
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 25:2cc29fa67345 34 InterruptIn left_button(D3);
MatthewMaat 25:2cc29fa67345 35 InterruptIn right_button(D2);
MatthewMaat 15:c4799ad02cdc 36
MatthewMaat 15:c4799ad02cdc 37 volatile float P0;
MatthewMaat 15:c4799ad02cdc 38 volatile float P1;
MatthewMaat 15:c4799ad02cdc 39 volatile float EMG_min0=1;
MatthewMaat 15:c4799ad02cdc 40 volatile float EMG_max0=0;
MatthewMaat 15:c4799ad02cdc 41 volatile float EMG_min1=1;
MatthewMaat 15:c4799ad02cdc 42 volatile float EMG_max1=0;
MatthewMaat 23:b0222fa7c131 43 volatile bool ignore_peaksx=false;
MatthewMaat 23:b0222fa7c131 44 volatile bool ignore_turnx=true;
MatthewMaat 23:b0222fa7c131 45 volatile bool ignore_peaksy=false;
MatthewMaat 23:b0222fa7c131 46 volatile bool ignore_turny=true;
MatthewMaat 14:dc89250ebc52 47 enum states{Waiting,Position_calibration,EMG_calibration,Homing,Operating,Demo,Failure};
MatthewMaat 15:c4799ad02cdc 48 states currentState=Waiting;
MatthewMaat 21:a316452da8cd 49 volatile bool motor1_calibrated=false;
MatthewMaat 10:e1eb73e19540 50
MatthewMaat 26:88639564e3af 51 volatile bool motorx_on=false;
MatthewMaat 26:88639564e3af 52 volatile float signx=0.075;
MatthewMaat 26:88639564e3af 53 volatile bool motory_on=false;
MatthewMaat 26:88639564e3af 54 volatile float signy=0.075;
MatthewMaat 26:88639564e3af 55
MatthewMaat 26:88639564e3af 56 volatile bool demo_done=false;
MatthewMaat 23:b0222fa7c131 57 volatile float x_ref=0.175;
MatthewMaat 23:b0222fa7c131 58 volatile float y_ref=0.175;
jobjansen 20:0f6a88f29a71 59 static float v_refx; //reference speeds
jobjansen 20:0f6a88f29a71 60 static float v_refy;
jobjansen 20:0f6a88f29a71 61 static float T = 0.002; //measuring period
jobjansen 20:0f6a88f29a71 62 static float L = 0.35; //distance between the motor axles, has to be checked
jobjansen 20:0f6a88f29a71 63 volatile float theta1_old = 0.0; //feedback variables
jobjansen 20:0f6a88f29a71 64 volatile float theta2_old = 0.0;
jobjansen 20:0f6a88f29a71 65
jobjansen 20:0f6a88f29a71 66
MatthewMaat 21:a316452da8cd 67 float error_1;
MatthewMaat 21:a316452da8cd 68 float error_2;
MatthewMaat 25:2cc29fa67345 69 volatile float filtered_error_derivativex;
MatthewMaat 25:2cc29fa67345 70 volatile float filtered_error_derivativey;
jobjansen 20:0f6a88f29a71 71
jobjansen 20:0f6a88f29a71 72 //PID controller variables
jobjansen 20:0f6a88f29a71 73 bool q = true;
jobjansen 20:0f6a88f29a71 74 float error_prev;
MatthewMaat 26:88639564e3af 75 volatile float Kp = 24.6;
MatthewMaat 26:88639564e3af 76 volatile float Ki = 28.3;
MatthewMaat 26:88639564e3af 77 volatile float Kd = 0.1;
jobjansen 20:0f6a88f29a71 78 float u;
jobjansen 20:0f6a88f29a71 79 float u_p;
jobjansen 20:0f6a88f29a71 80 float u_i;
jobjansen 20:0f6a88f29a71 81 float u_d;
MatthewMaat 21:a316452da8cd 82 float error_integral1 = 0.0;
MatthewMaat 21:a316452da8cd 83 float error_integral2 = 0.0;
jobjansen 20:0f6a88f29a71 84
jobjansen 20:0f6a88f29a71 85 float u_1;
jobjansen 20:0f6a88f29a71 86 float u_2;
MatthewMaat 18:8002c75b8e20 87 const float angle2_offset=asin(0.2);
MatthewMaat 22:335a30b0825d 88 const float angle1_offset=atan(2.2/32.8);
MatthewMaat 18:8002c75b8e20 89 const double pi=3.1415926535897932384626;
MatthewMaat 18:8002c75b8e20 90 volatile float theta1;
MatthewMaat 18:8002c75b8e20 91 volatile float theta2;
MatthewMaat 25:2cc29fa67345 92 volatile float theta1_ref;
MatthewMaat 25:2cc29fa67345 93 volatile float theta2_ref;
MatthewMaat 14:dc89250ebc52 94
MatthewMaat 14:dc89250ebc52 95 void read_emg()
MatthewMaat 13:ec4708dab45d 96 {
MatthewMaat 15:c4799ad02cdc 97 //EMG signal 0
MatthewMaat 15:c4799ad02cdc 98 static int count0=0;
MatthewMaat 15:c4799ad02cdc 99 static float RMS_value0=0;
MatthewMaat 15:c4799ad02cdc 100 static float HighPass_value0=0;
MatthewMaat 15:c4799ad02cdc 101 count0+=1;
MatthewMaat 15:c4799ad02cdc 102 static float RMS0[150];
MatthewMaat 15:c4799ad02cdc 103 static float HighPass0[30];
MatthewMaat 17:d1acb6888b82 104 static BiQuad Notch0(0.9695f,-1.5695f,0.9695f,-1.5695f,0.9391f);
MatthewMaat 17:d1acb6888b82 105 static BiQuad Notch1(0.9695f,-1.5695f,0.9695f,-1.5695f,0.9391f);
MatthewMaat 15:c4799ad02cdc 106 float I0;
MatthewMaat 15:c4799ad02cdc 107 float If0;
MatthewMaat 15:c4799ad02cdc 108 //signal 1
MatthewMaat 15:c4799ad02cdc 109 static int count1=0;
MatthewMaat 15:c4799ad02cdc 110 static float RMS_value1=0;
MatthewMaat 15:c4799ad02cdc 111 static float HighPass_value1=0;
MatthewMaat 15:c4799ad02cdc 112 count1+=1;
MatthewMaat 15:c4799ad02cdc 113 static float RMS1[150];
MatthewMaat 15:c4799ad02cdc 114 static float HighPass1[30];
MatthewMaat 13:ec4708dab45d 115 float I1;
MatthewMaat 15:c4799ad02cdc 116 float If1;
MatthewMaat 15:c4799ad02cdc 117 I0=emg0.read(); //read signal
MatthewMaat 17:d1acb6888b82 118 double notched0=Notch0.step(I0);
MatthewMaat 17:d1acb6888b82 119 HighPass_value0+=(notched0-HighPass0[count0%30])/30.0;
MatthewMaat 17:d1acb6888b82 120 HighPass0[count0%30]=notched0;
MatthewMaat 15:c4799ad02cdc 121 If0=pow(I0-HighPass_value0,2.0f); // Highpass-filtered value squared
MatthewMaat 15:c4799ad02cdc 122 RMS_value0+=(If0-RMS0[count0%150])/150.0;
MatthewMaat 15:c4799ad02cdc 123 RMS0[count0%150]=If0;
MatthewMaat 13:ec4708dab45d 124 /* 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 125 P0=sqrt(RMS_value0);
MatthewMaat 15:c4799ad02cdc 126 I1=emg1.read(); //read signal
MatthewMaat 17:d1acb6888b82 127 double notched1=Notch1.step(I1);
MatthewMaat 17:d1acb6888b82 128 HighPass_value1+=(notched1-HighPass1[count1%30])/30.0;
MatthewMaat 17:d1acb6888b82 129 HighPass1[count1%30]=notched1;
MatthewMaat 15:c4799ad02cdc 130 If1=pow(I1-HighPass_value1,2.0f); // Highpass-filtered value squared
MatthewMaat 15:c4799ad02cdc 131 RMS_value1+=(If1-RMS1[count1%150])/150.0;
MatthewMaat 15:c4799ad02cdc 132 RMS1[count1%150]=If1;
MatthewMaat 15:c4799ad02cdc 133 /* 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 134 P1=sqrt(RMS_value1);
MatthewMaat 13:ec4708dab45d 135 /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels)
MatthewMaat 13:ec4708dab45d 136 * Ensure that enough channels are available (HIDScope scope( 2 ))
MatthewMaat 13:ec4708dab45d 137 * Finally, send all channels to the PC at once */
MatthewMaat 13:ec4708dab45d 138 /* To indicate that the function is working, the LED is toggled */
MatthewMaat 14:dc89250ebc52 139 ledred=1;
MatthewMaat 14:dc89250ebc52 140 ledgreen=0;
MatthewMaat 14:dc89250ebc52 141 ledblue=1;
MatthewMaat 13:ec4708dab45d 142 }
MatthewMaat 14:dc89250ebc52 143
MatthewMaat 18:8002c75b8e20 144 void get_angles(void)
MatthewMaat 18:8002c75b8e20 145 {
MatthewMaat 18:8002c75b8e20 146 float pulses1=motor1_pos.getPulses();
MatthewMaat 18:8002c75b8e20 147 float pulses2=motor2_pos.getPulses();
MatthewMaat 19:fb3d570a115e 148 theta1=angle1_offset+pulses1*17.0/16.0*2*pi/131.0/32.0;
MatthewMaat 19:fb3d570a115e 149 theta2=angle2_offset+pulses2*17.0/16.0*2*pi/131.0/32.0;
MatthewMaat 18:8002c75b8e20 150 }
MatthewMaat 18:8002c75b8e20 151
MatthewMaat 18:8002c75b8e20 152 void pos_cal(void)
MatthewMaat 18:8002c75b8e20 153 {
MatthewMaat 18:8002c75b8e20 154 float t=state_time.read();
MatthewMaat 18:8002c75b8e20 155 static int pos_time_counter=0;
MatthewMaat 18:8002c75b8e20 156 static int last_ticks=10000;
MatthewMaat 18:8002c75b8e20 157 float pulses;
MatthewMaat 18:8002c75b8e20 158 pos_time_counter+=1;
MatthewMaat 18:8002c75b8e20 159 if(!motor1_calibrated&&t>1.0f)
MatthewMaat 18:8002c75b8e20 160 {
MatthewMaat 18:8002c75b8e20 161 dir1=1; //???
MatthewMaat 22:335a30b0825d 162 motor1_pwm.write(0.75f);
MatthewMaat 18:8002c75b8e20 163 pulses=motor1_pos.getPulses();
MatthewMaat 18:8002c75b8e20 164 if(pos_time_counter%500==0&&fabs(pulses-last_ticks)<1)
MatthewMaat 18:8002c75b8e20 165 {
MatthewMaat 21:a316452da8cd 166 ledblue=!ledblue;
MatthewMaat 18:8002c75b8e20 167 motor1_pos.reset();
MatthewMaat 18:8002c75b8e20 168 last_ticks=10000;
MatthewMaat 18:8002c75b8e20 169 }
MatthewMaat 18:8002c75b8e20 170 else if(pos_time_counter%500==0)
MatthewMaat 18:8002c75b8e20 171 {
MatthewMaat 18:8002c75b8e20 172 last_ticks=motor1_pos.getPulses();
MatthewMaat 18:8002c75b8e20 173 }
MatthewMaat 18:8002c75b8e20 174
MatthewMaat 18:8002c75b8e20 175 }
MatthewMaat 22:335a30b0825d 176 else if(!motor1_calibrated)
MatthewMaat 22:335a30b0825d 177 {
MatthewMaat 22:335a30b0825d 178 motor2_pwm.write(0.75f);
MatthewMaat 22:335a30b0825d 179 dir2=1;
MatthewMaat 22:335a30b0825d 180 }
MatthewMaat 23:b0222fa7c131 181 else if(t>1.0f)
MatthewMaat 18:8002c75b8e20 182 {
MatthewMaat 18:8002c75b8e20 183 motor1_pwm.write(0.0f);
MatthewMaat 18:8002c75b8e20 184 dir2=1; //???
MatthewMaat 22:335a30b0825d 185 motor2_pwm.write(0.75f);
MatthewMaat 18:8002c75b8e20 186 pulses=motor2_pos.getPulses();
MatthewMaat 18:8002c75b8e20 187 if(pos_time_counter%500==0&&fabs(pulses-last_ticks)<1)
MatthewMaat 18:8002c75b8e20 188 {
MatthewMaat 21:a316452da8cd 189 ledblue=!ledblue;
MatthewMaat 18:8002c75b8e20 190 motor2_pos.reset();
MatthewMaat 18:8002c75b8e20 191 motor2_pwm.write(0.0f);
MatthewMaat 18:8002c75b8e20 192 }
MatthewMaat 18:8002c75b8e20 193 else if(pos_time_counter%500==0)
MatthewMaat 18:8002c75b8e20 194 {
MatthewMaat 18:8002c75b8e20 195 last_ticks=motor2_pos.getPulses();
MatthewMaat 18:8002c75b8e20 196 }
MatthewMaat 18:8002c75b8e20 197 }
MatthewMaat 18:8002c75b8e20 198
MatthewMaat 18:8002c75b8e20 199 }
MatthewMaat 18:8002c75b8e20 200
MatthewMaat 15:c4799ad02cdc 201 void record_min_max(void)
MatthewMaat 15:c4799ad02cdc 202 {
MatthewMaat 15:c4799ad02cdc 203 float t=state_time.read();
MatthewMaat 15:c4799ad02cdc 204 if(t>0.4)
MatthewMaat 15:c4799ad02cdc 205 {
MatthewMaat 15:c4799ad02cdc 206 if(P0<EMG_min0)
MatthewMaat 15:c4799ad02cdc 207 {
MatthewMaat 15:c4799ad02cdc 208 EMG_min0=P0;
MatthewMaat 15:c4799ad02cdc 209 }
MatthewMaat 15:c4799ad02cdc 210 else if(P0>EMG_max0)
MatthewMaat 15:c4799ad02cdc 211 {
MatthewMaat 15:c4799ad02cdc 212 EMG_max0=P0;
MatthewMaat 15:c4799ad02cdc 213 }
MatthewMaat 15:c4799ad02cdc 214 if(P1<EMG_min1)
MatthewMaat 15:c4799ad02cdc 215 {
MatthewMaat 15:c4799ad02cdc 216 EMG_min1=P1;
MatthewMaat 15:c4799ad02cdc 217 }
MatthewMaat 15:c4799ad02cdc 218 else if(P1>EMG_max1)
MatthewMaat 15:c4799ad02cdc 219 {
MatthewMaat 15:c4799ad02cdc 220 EMG_max1=P1;
MatthewMaat 15:c4799ad02cdc 221 }
MatthewMaat 15:c4799ad02cdc 222 }
MatthewMaat 15:c4799ad02cdc 223 }
MatthewMaat 15:c4799ad02cdc 224
MatthewMaat 23:b0222fa7c131 225 void unignore_peaksx(void)
MatthewMaat 15:c4799ad02cdc 226 {
MatthewMaat 23:b0222fa7c131 227 ignore_peaksx=false;
MatthewMaat 23:b0222fa7c131 228 }
MatthewMaat 23:b0222fa7c131 229 void start_ignore_turnx(void)
MatthewMaat 23:b0222fa7c131 230 {
MatthewMaat 23:b0222fa7c131 231 ignore_turnx=true;
MatthewMaat 15:c4799ad02cdc 232 }
MatthewMaat 23:b0222fa7c131 233 void unignore_peaksy(void)
MatthewMaat 16:2d115aa2773e 234 {
MatthewMaat 23:b0222fa7c131 235 ignore_peaksy=false;
MatthewMaat 23:b0222fa7c131 236 }
MatthewMaat 23:b0222fa7c131 237 void start_ignore_turny(void)
MatthewMaat 23:b0222fa7c131 238 {
MatthewMaat 23:b0222fa7c131 239 ignore_turny=true;
MatthewMaat 16:2d115aa2773e 240 }
MatthewMaat 15:c4799ad02cdc 241
MatthewMaat 23:b0222fa7c131 242 void set_v_refxy(void)
MatthewMaat 11:de4a85703169 243 {
MatthewMaat 15:c4799ad02cdc 244 float Q0;
MatthewMaat 15:c4799ad02cdc 245 Q0=2.0f*(P0-(EMG_min0+EMG_max0)/2.0f)/(EMG_max0-EMG_min0);
MatthewMaat 23:b0222fa7c131 246 float Q1;
MatthewMaat 23:b0222fa7c131 247 Q1=2.0f*(P1-(EMG_min1+EMG_max1)/2.0f)/(EMG_max1-EMG_min1);
MatthewMaat 23:b0222fa7c131 248 if (Q0>-0.2f && !ignore_peaksx)
MatthewMaat 13:ec4708dab45d 249 {
MatthewMaat 23:b0222fa7c131 250 if (motorx_on)
MatthewMaat 15:c4799ad02cdc 251 {
MatthewMaat 23:b0222fa7c131 252 v_refx=0;
MatthewMaat 23:b0222fa7c131 253 EMG_peakx.attach(unignore_peaksx,0.8);
MatthewMaat 23:b0222fa7c131 254 turnx.attach(start_ignore_turnx,1);
MatthewMaat 23:b0222fa7c131 255 ignore_turnx=false;
MatthewMaat 23:b0222fa7c131 256 ignore_peaksx=true;
MatthewMaat 23:b0222fa7c131 257 motorx_on=false;
MatthewMaat 15:c4799ad02cdc 258 }
MatthewMaat 23:b0222fa7c131 259 else if(ignore_turnx)
MatthewMaat 15:c4799ad02cdc 260 {
MatthewMaat 23:b0222fa7c131 261 v_refx=1.0*signx;
MatthewMaat 23:b0222fa7c131 262 EMG_peakx.attach(unignore_peaksx,0.8);
MatthewMaat 23:b0222fa7c131 263 turnx.attach(start_ignore_turnx,1);
MatthewMaat 23:b0222fa7c131 264 ignore_turnx=false;
MatthewMaat 23:b0222fa7c131 265 ignore_peaksx=true;
MatthewMaat 23:b0222fa7c131 266 motorx_on=true;
MatthewMaat 15:c4799ad02cdc 267 }
MatthewMaat 16:2d115aa2773e 268 else
MatthewMaat 16:2d115aa2773e 269 {
MatthewMaat 23:b0222fa7c131 270 signx=-1.0*signx;
MatthewMaat 23:b0222fa7c131 271 v_refx=1.0*signx;
MatthewMaat 25:2cc29fa67345 272 EMG_peakx.attach(unignore_peaksx,0.8);
MatthewMaat 23:b0222fa7c131 273 ignore_peaksx=true;
MatthewMaat 23:b0222fa7c131 274 motorx_on=true;
MatthewMaat 16:2d115aa2773e 275 }
MatthewMaat 13:ec4708dab45d 276 }
MatthewMaat 23:b0222fa7c131 277 if (Q1>-0.2f && !ignore_peaksy)
MatthewMaat 23:b0222fa7c131 278 {
MatthewMaat 23:b0222fa7c131 279 if (motory_on)
MatthewMaat 23:b0222fa7c131 280 {
MatthewMaat 23:b0222fa7c131 281 v_refy=0;
MatthewMaat 23:b0222fa7c131 282 EMG_peaky.attach(unignore_peaksy,0.8);
MatthewMaat 23:b0222fa7c131 283 turny.attach(start_ignore_turny,1);
MatthewMaat 23:b0222fa7c131 284 ignore_turny=false;
MatthewMaat 23:b0222fa7c131 285 ignore_peaksy=true;
MatthewMaat 23:b0222fa7c131 286 motory_on=false;
MatthewMaat 23:b0222fa7c131 287 }
MatthewMaat 23:b0222fa7c131 288 else if(ignore_turny)
MatthewMaat 23:b0222fa7c131 289 {
MatthewMaat 23:b0222fa7c131 290 v_refy=1.0*signy;
MatthewMaat 23:b0222fa7c131 291 EMG_peaky.attach(unignore_peaksy,0.8);
MatthewMaat 23:b0222fa7c131 292 turny.attach(start_ignore_turny,1);
MatthewMaat 23:b0222fa7c131 293 ignore_turny=false;
MatthewMaat 23:b0222fa7c131 294 ignore_peaksy=true;
MatthewMaat 23:b0222fa7c131 295 motory_on=true;
MatthewMaat 23:b0222fa7c131 296 }
MatthewMaat 23:b0222fa7c131 297 else
MatthewMaat 23:b0222fa7c131 298 {
MatthewMaat 23:b0222fa7c131 299 signy=-1.0*signy;
MatthewMaat 23:b0222fa7c131 300 v_refy=1.0*signy;
MatthewMaat 25:2cc29fa67345 301 EMG_peaky.attach(unignore_peaksy,0.8);
MatthewMaat 23:b0222fa7c131 302 ignore_peaksy=true;
MatthewMaat 23:b0222fa7c131 303 motory_on=true;
MatthewMaat 23:b0222fa7c131 304 }
MatthewMaat 23:b0222fa7c131 305 }
MatthewMaat 24:3c9ac44890f0 306 float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
MatthewMaat 24:3c9ac44890f0 307 float y_pos=tan(theta1)*x_pos;
MatthewMaat 24:3c9ac44890f0 308 if(x_pos>=0.35&&v_refx>0)
MatthewMaat 24:3c9ac44890f0 309 {
MatthewMaat 24:3c9ac44890f0 310 v_refx=0;
MatthewMaat 24:3c9ac44890f0 311 }
MatthewMaat 24:3c9ac44890f0 312 else if(x_pos<=0.0&&v_refx<0)
MatthewMaat 24:3c9ac44890f0 313 {
MatthewMaat 24:3c9ac44890f0 314 v_refx=0;
MatthewMaat 24:3c9ac44890f0 315 }
MatthewMaat 24:3c9ac44890f0 316 if(y_pos>=0.45&&v_refy>0)
MatthewMaat 24:3c9ac44890f0 317 {
MatthewMaat 24:3c9ac44890f0 318 v_refy=0;
MatthewMaat 24:3c9ac44890f0 319 }
MatthewMaat 24:3c9ac44890f0 320 else if(y_pos<=0.07&&v_refy<0)
MatthewMaat 24:3c9ac44890f0 321 {
MatthewMaat 24:3c9ac44890f0 322 v_refy=0;
MatthewMaat 24:3c9ac44890f0 323 }
MatthewMaat 11:de4a85703169 324 }
MatthewMaat 11:de4a85703169 325
MatthewMaat 25:2cc29fa67345 326 void left_fake_emg(void)
MatthewMaat 25:2cc29fa67345 327 {
MatthewMaat 25:2cc29fa67345 328 if (!ignore_peaksx)
MatthewMaat 25:2cc29fa67345 329 {
MatthewMaat 25:2cc29fa67345 330 if (motorx_on)
MatthewMaat 25:2cc29fa67345 331 {
MatthewMaat 25:2cc29fa67345 332 v_refx=0;
MatthewMaat 25:2cc29fa67345 333 EMG_peakx.attach(unignore_peaksx,0.8);
MatthewMaat 25:2cc29fa67345 334 turnx.attach(start_ignore_turnx,1);
MatthewMaat 25:2cc29fa67345 335 ignore_turnx=false;
MatthewMaat 25:2cc29fa67345 336 ignore_peaksx=true;
MatthewMaat 25:2cc29fa67345 337 motorx_on=false;
MatthewMaat 25:2cc29fa67345 338 }
MatthewMaat 25:2cc29fa67345 339 else if(ignore_turnx)
MatthewMaat 25:2cc29fa67345 340 {
MatthewMaat 25:2cc29fa67345 341 v_refx=1.0*signx;
MatthewMaat 25:2cc29fa67345 342 EMG_peakx.attach(unignore_peaksx,0.8);
MatthewMaat 25:2cc29fa67345 343 turnx.attach(start_ignore_turnx,1);
MatthewMaat 25:2cc29fa67345 344 ignore_turnx=false;
MatthewMaat 25:2cc29fa67345 345 ignore_peaksx=true;
MatthewMaat 25:2cc29fa67345 346 motorx_on=true;
MatthewMaat 25:2cc29fa67345 347 }
MatthewMaat 25:2cc29fa67345 348 else
MatthewMaat 25:2cc29fa67345 349 {
MatthewMaat 25:2cc29fa67345 350 signx=-1.0*signx;
MatthewMaat 25:2cc29fa67345 351 v_refx=1.0*signx;
MatthewMaat 25:2cc29fa67345 352 EMG_peakx.attach(unignore_peaksx,0.8);
MatthewMaat 25:2cc29fa67345 353 ignore_peaksx=true;
MatthewMaat 25:2cc29fa67345 354 motorx_on=true;
MatthewMaat 25:2cc29fa67345 355 }
MatthewMaat 25:2cc29fa67345 356 }
MatthewMaat 25:2cc29fa67345 357 }
MatthewMaat 25:2cc29fa67345 358
MatthewMaat 25:2cc29fa67345 359 void right_fake_emg(void)
MatthewMaat 25:2cc29fa67345 360 {
MatthewMaat 25:2cc29fa67345 361 if (!ignore_peaksy)
MatthewMaat 25:2cc29fa67345 362 {
MatthewMaat 25:2cc29fa67345 363 if (motory_on)
MatthewMaat 25:2cc29fa67345 364 {
MatthewMaat 25:2cc29fa67345 365 v_refy=0;
MatthewMaat 25:2cc29fa67345 366 EMG_peaky.attach(unignore_peaksy,0.8);
MatthewMaat 25:2cc29fa67345 367 turny.attach(start_ignore_turny,1);
MatthewMaat 25:2cc29fa67345 368 ignore_turny=false;
MatthewMaat 25:2cc29fa67345 369 ignore_peaksy=true;
MatthewMaat 25:2cc29fa67345 370 motory_on=false;
MatthewMaat 25:2cc29fa67345 371 }
MatthewMaat 25:2cc29fa67345 372 else if(ignore_turny)
MatthewMaat 25:2cc29fa67345 373 {
MatthewMaat 25:2cc29fa67345 374 v_refy=1.0*signy;
MatthewMaat 25:2cc29fa67345 375 EMG_peaky.attach(unignore_peaksy,0.8);
MatthewMaat 25:2cc29fa67345 376 turny.attach(start_ignore_turny,1);
MatthewMaat 25:2cc29fa67345 377 ignore_turny=false;
MatthewMaat 25:2cc29fa67345 378 ignore_peaksy=true;
MatthewMaat 25:2cc29fa67345 379 motory_on=true;
MatthewMaat 25:2cc29fa67345 380 }
MatthewMaat 25:2cc29fa67345 381 else
MatthewMaat 25:2cc29fa67345 382 {
MatthewMaat 25:2cc29fa67345 383 signy=-1.0*signy;
MatthewMaat 25:2cc29fa67345 384 v_refy=1.0*signy;
MatthewMaat 25:2cc29fa67345 385 EMG_peaky.attach(unignore_peaksy,0.8);
MatthewMaat 25:2cc29fa67345 386 ignore_peaksy=true;
MatthewMaat 25:2cc29fa67345 387 motory_on=true;
MatthewMaat 25:2cc29fa67345 388 }
MatthewMaat 25:2cc29fa67345 389 }
MatthewMaat 25:2cc29fa67345 390 }
MatthewMaat 25:2cc29fa67345 391
MatthewMaat 25:2cc29fa67345 392 void set_ref_fake_emg(void)
MatthewMaat 25:2cc29fa67345 393 {
MatthewMaat 25:2cc29fa67345 394 float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
MatthewMaat 25:2cc29fa67345 395 float y_pos=tan(theta1)*x_pos;
MatthewMaat 25:2cc29fa67345 396 if(x_pos>=0.35&&v_refx>0)
MatthewMaat 25:2cc29fa67345 397 {
MatthewMaat 25:2cc29fa67345 398 v_refx=0;
MatthewMaat 25:2cc29fa67345 399 }
MatthewMaat 25:2cc29fa67345 400 else if(x_pos<=0.0&&v_refx<0)
MatthewMaat 25:2cc29fa67345 401 {
MatthewMaat 25:2cc29fa67345 402 v_refx=0;
MatthewMaat 25:2cc29fa67345 403 }
MatthewMaat 25:2cc29fa67345 404 if(y_pos>=0.45&&v_refy>0)
MatthewMaat 25:2cc29fa67345 405 {
MatthewMaat 25:2cc29fa67345 406 v_refy=0;
MatthewMaat 25:2cc29fa67345 407 }
MatthewMaat 25:2cc29fa67345 408 else if(y_pos<=0.07&&v_refy<0)
MatthewMaat 25:2cc29fa67345 409 {
MatthewMaat 25:2cc29fa67345 410 v_refy=0;
MatthewMaat 25:2cc29fa67345 411 }
MatthewMaat 25:2cc29fa67345 412 }
MatthewMaat 25:2cc29fa67345 413
jobjansen 20:0f6a88f29a71 414 void error(void){
MatthewMaat 25:2cc29fa67345 415 float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
MatthewMaat 25:2cc29fa67345 416 float y_pos=tan(theta1)*x_pos;
MatthewMaat 25:2cc29fa67345 417 float r1=sqrt(pow(x_pos,2)+pow(y_pos,2));
MatthewMaat 25:2cc29fa67345 418 float r2=sqrt(pow(0.35-x_pos,2)+pow(y_pos,2));
MatthewMaat 25:2cc29fa67345 419 float J11=-r1*cos(theta2)/sin(theta1+theta2);
MatthewMaat 25:2cc29fa67345 420 float J21=r1*sin(theta2)/sin(theta1+theta2);
MatthewMaat 25:2cc29fa67345 421 float J12=r2*cos(theta1)/sin(theta1+theta2);
MatthewMaat 25:2cc29fa67345 422 float J22=r2*sin(theta1)/sin(theta1+theta2);
MatthewMaat 25:2cc29fa67345 423 float a=J22/(J11*J22-J12*J21);
MatthewMaat 25:2cc29fa67345 424 float b=-J12/(J11*J22-J12*J21);
MatthewMaat 25:2cc29fa67345 425 float c=-J21/(J11*J22-J12*J21);
MatthewMaat 25:2cc29fa67345 426 float d=J11/(J11*J22-J12*J21);
MatthewMaat 25:2cc29fa67345 427 /*
MatthewMaat 23:b0222fa7c131 428 float dvd=L * (tan(theta2) * pow(tan(theta1),2) + tan(theta1) * pow(tan(theta2),2));//+ is aangepast
jobjansen 20:0f6a88f29a71 429
MatthewMaat 21:a316452da8cd 430 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 431 float b = ( pow(cos(theta1),2) * pow((tan(theta1)+tan(theta2)),2) * tan(theta1) ) / dvd;
MatthewMaat 21:a316452da8cd 432 float c = ( pow(cos(theta2),2) * pow((tan(theta1)+tan(theta2)),2) * pow(tan(theta2),2) ) / dvd;
MatthewMaat 21:a316452da8cd 433 float d = ( pow(cos(theta2),2) * pow((tan(theta1)+tan(theta2)),2) * pow(tan(theta2),2) ) / dvd;
MatthewMaat 25:2cc29fa67345 434 */
jobjansen 20:0f6a88f29a71 435 float theta1_dot = a*v_refx + b*v_refy;
jobjansen 20:0f6a88f29a71 436 float theta2_dot = c*v_refx + d*v_refy;
jobjansen 20:0f6a88f29a71 437
MatthewMaat 21:a316452da8cd 438 if(currentState==Operating)
MatthewMaat 21:a316452da8cd 439 {
MatthewMaat 21:a316452da8cd 440 theta1_ref = theta1_old+theta1_dot*T;
MatthewMaat 21:a316452da8cd 441 theta2_ref = theta2_old+theta2_dot*T;
MatthewMaat 25:2cc29fa67345 442 if(theta1_ref<theta1-0.2)
MatthewMaat 25:2cc29fa67345 443 {
MatthewMaat 25:2cc29fa67345 444 theta1_ref=theta1-0.2;
MatthewMaat 25:2cc29fa67345 445 }
MatthewMaat 25:2cc29fa67345 446 else if(theta1_ref>theta1+0.2)
MatthewMaat 25:2cc29fa67345 447 {
MatthewMaat 25:2cc29fa67345 448 theta1_ref=theta1+0.2;
MatthewMaat 25:2cc29fa67345 449 }
MatthewMaat 25:2cc29fa67345 450 if(theta2_ref<theta2-0.2)
MatthewMaat 25:2cc29fa67345 451 {
MatthewMaat 25:2cc29fa67345 452 theta2_ref=theta2-0.2;
MatthewMaat 25:2cc29fa67345 453 }
MatthewMaat 25:2cc29fa67345 454 else if(theta2_ref>theta2+0.2)
MatthewMaat 25:2cc29fa67345 455 {
MatthewMaat 25:2cc29fa67345 456 theta2_ref=theta2+0.2;
MatthewMaat 25:2cc29fa67345 457 }
MatthewMaat 24:3c9ac44890f0 458 if(theta1_ref<0.1f)
MatthewMaat 24:3c9ac44890f0 459 {
MatthewMaat 24:3c9ac44890f0 460 theta1_ref=0.1f;
MatthewMaat 24:3c9ac44890f0 461 }
MatthewMaat 25:2cc29fa67345 462 else if(theta1_ref>1.8f)
MatthewMaat 24:3c9ac44890f0 463 {
MatthewMaat 25:2cc29fa67345 464 theta1_ref=1.8f;
MatthewMaat 24:3c9ac44890f0 465 }
MatthewMaat 24:3c9ac44890f0 466 if(theta2_ref<0.1f)
MatthewMaat 24:3c9ac44890f0 467 {
MatthewMaat 24:3c9ac44890f0 468 theta2_ref=0.1f;
MatthewMaat 24:3c9ac44890f0 469 }
MatthewMaat 25:2cc29fa67345 470 else if(theta2_ref>1.8f)
MatthewMaat 24:3c9ac44890f0 471 {
MatthewMaat 25:2cc29fa67345 472 theta2_ref=1.8f;
MatthewMaat 24:3c9ac44890f0 473 }
MatthewMaat 21:a316452da8cd 474 }
MatthewMaat 21:a316452da8cd 475 else if(currentState==Homing)
MatthewMaat 21:a316452da8cd 476 {
MatthewMaat 21:a316452da8cd 477 theta1_ref=pi/4.0f;
MatthewMaat 21:a316452da8cd 478 theta2_ref=pi/4.0f;
MatthewMaat 21:a316452da8cd 479 }
MatthewMaat 23:b0222fa7c131 480 else if(currentState==Demo)
MatthewMaat 23:b0222fa7c131 481 {
MatthewMaat 23:b0222fa7c131 482 theta1_ref=atan(y_ref/x_ref);
MatthewMaat 23:b0222fa7c131 483 theta2_ref=atan(y_ref/(0.35f-x_ref));
MatthewMaat 23:b0222fa7c131 484 }
MatthewMaat 21:a316452da8cd 485 error_1 = theta1 - theta1_ref;
MatthewMaat 21:a316452da8cd 486 error_2 = theta2 - theta2_ref;
jobjansen 20:0f6a88f29a71 487
jobjansen 20:0f6a88f29a71 488 theta1_old = theta1_ref;
jobjansen 20:0f6a88f29a71 489 theta2_old = theta2_ref;
jobjansen 20:0f6a88f29a71 490
jobjansen 20:0f6a88f29a71 491 }
jobjansen 20:0f6a88f29a71 492
MatthewMaat 21:a316452da8cd 493 float PID1(float err){
jobjansen 20:0f6a88f29a71 494 //P action
MatthewMaat 21:a316452da8cd 495 u_p = Kp * err;
MatthewMaat 26:88639564e3af 496 float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
MatthewMaat 26:88639564e3af 497 float y_pos=tan(theta1)*x_pos;
MatthewMaat 26:88639564e3af 498 float r1=0.1+sqrt(pow(x_pos,2)+pow(y_pos,2));
jobjansen 20:0f6a88f29a71 499 //I action
MatthewMaat 21:a316452da8cd 500 error_integral1 = error_integral1 + err * T;
MatthewMaat 21:a316452da8cd 501 u_i = Ki * error_integral1;
MatthewMaat 21:a316452da8cd 502
MatthewMaat 21:a316452da8cd 503 //D action
MatthewMaat 25:2cc29fa67345 504 static float error_prevx=0;
MatthewMaat 25:2cc29fa67345 505 static float error_derivativex;
MatthewMaat 25:2cc29fa67345 506 static BiQuad LowPassFilterx(0.1311f,0.2622f,0.1311f,-0.7478,0.2722f);
MatthewMaat 25:2cc29fa67345 507
MatthewMaat 21:a316452da8cd 508
MatthewMaat 25:2cc29fa67345 509 error_derivativex = (err - error_prevx)/T;
MatthewMaat 25:2cc29fa67345 510 filtered_error_derivativex = LowPassFilterx.step(error_derivativex);
MatthewMaat 25:2cc29fa67345 511 u_d = Kd * filtered_error_derivativex;
MatthewMaat 25:2cc29fa67345 512 error_prevx = err;
MatthewMaat 21:a316452da8cd 513
MatthewMaat 26:88639564e3af 514 u = r1/0.3f*(u_p + u_i+u_d);
MatthewMaat 25:2cc29fa67345 515 if(filtered_error_derivativex>0.3)
MatthewMaat 25:2cc29fa67345 516 {
MatthewMaat 25:2cc29fa67345 517 u=0;
MatthewMaat 25:2cc29fa67345 518 }
MatthewMaat 21:a316452da8cd 519 return u;
MatthewMaat 21:a316452da8cd 520 }
MatthewMaat 21:a316452da8cd 521
MatthewMaat 21:a316452da8cd 522 float PID2(float err){
MatthewMaat 21:a316452da8cd 523 //P action
MatthewMaat 21:a316452da8cd 524 u_p = Kp * err;
MatthewMaat 26:88639564e3af 525 float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
MatthewMaat 26:88639564e3af 526 float y_pos=tan(theta1)*x_pos;
MatthewMaat 26:88639564e3af 527 float r2=0.1+sqrt(pow(0.35f-x_pos,2)+pow(y_pos,2));
MatthewMaat 21:a316452da8cd 528 //I action
MatthewMaat 21:a316452da8cd 529 error_integral2 = error_integral2 + err * T;
MatthewMaat 21:a316452da8cd 530 u_i = Ki * error_integral2;
jobjansen 20:0f6a88f29a71 531
MatthewMaat 25:2cc29fa67345 532
jobjansen 20:0f6a88f29a71 533 //D action
MatthewMaat 25:2cc29fa67345 534 static float error_prevy=0;
MatthewMaat 25:2cc29fa67345 535 static float error_derivativey;
MatthewMaat 25:2cc29fa67345 536 static BiQuad LowPassFiltery(0.1311f,0.2622f,0.1311f,-0.7478,0.2722f);
MatthewMaat 25:2cc29fa67345 537
jobjansen 20:0f6a88f29a71 538
MatthewMaat 25:2cc29fa67345 539 error_derivativey = (err - error_prevy)/T;
MatthewMaat 25:2cc29fa67345 540 filtered_error_derivativey = LowPassFiltery.step(error_derivativey);
MatthewMaat 25:2cc29fa67345 541 u_d = Kd * filtered_error_derivativey;
MatthewMaat 25:2cc29fa67345 542 error_prevy = err;
jobjansen 20:0f6a88f29a71 543
jobjansen 20:0f6a88f29a71 544
MatthewMaat 26:88639564e3af 545 u = r2/0.3f*(u_p + u_i+u_d);
jobjansen 20:0f6a88f29a71 546 return u;
jobjansen 20:0f6a88f29a71 547 }
MatthewMaat 23:b0222fa7c131 548 void set_refxy(void)
MatthewMaat 23:b0222fa7c131 549 {
MatthewMaat 23:b0222fa7c131 550 float t=state_time.read();
MatthewMaat 23:b0222fa7c131 551 int tfloor=floor(t)/1;
MatthewMaat 26:88639564e3af 552 float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
MatthewMaat 26:88639564e3af 553 float y_pos=tan(theta1)*x_pos;
MatthewMaat 26:88639564e3af 554 static float x_start=x_pos;
MatthewMaat 26:88639564e3af 555 static float y_start=y_pos;
MatthewMaat 26:88639564e3af 556 static int demo_subpart=1;
MatthewMaat 26:88639564e3af 557 float v_demo=0.015;
MatthewMaat 25:2cc29fa67345 558 /* Rectangle
MatthewMaat 25:2cc29fa67345 559 if(tfloor%12==0||tfloor%12==1||tfloor%12==2)
MatthewMaat 23:b0222fa7c131 560 {
MatthewMaat 25:2cc29fa67345 561 x_ref=0.025+0.1*(t-tfloor+tfloor%12);
MatthewMaat 25:2cc29fa67345 562 y_ref=0.2;
MatthewMaat 23:b0222fa7c131 563 }
MatthewMaat 25:2cc29fa67345 564 else if(tfloor%12==3||tfloor%12==4||tfloor%12==5)
MatthewMaat 23:b0222fa7c131 565 {
MatthewMaat 25:2cc29fa67345 566 x_ref=0.325;
MatthewMaat 25:2cc29fa67345 567 y_ref=0.2+0.05*(t-tfloor+tfloor%12-3);
MatthewMaat 23:b0222fa7c131 568 }
MatthewMaat 25:2cc29fa67345 569 else if(tfloor%12==6||tfloor%12==7||tfloor%12==8)
MatthewMaat 23:b0222fa7c131 570 {
MatthewMaat 25:2cc29fa67345 571 x_ref=0.325-0.1*(t-tfloor+tfloor%12-6);
MatthewMaat 25:2cc29fa67345 572 y_ref=0.35;
MatthewMaat 23:b0222fa7c131 573 }
MatthewMaat 23:b0222fa7c131 574 else
MatthewMaat 23:b0222fa7c131 575 {
MatthewMaat 25:2cc29fa67345 576 x_ref=0.025;
MatthewMaat 25:2cc29fa67345 577 y_ref=0.35-0.05*(t-tfloor+tfloor%12-9);
MatthewMaat 25:2cc29fa67345 578 }
MatthewMaat 25:2cc29fa67345 579 */
MatthewMaat 25:2cc29fa67345 580 /* Circle
MatthewMaat 25:2cc29fa67345 581 x_ref=0.175f+0.15*cos(t/2.5f);
MatthewMaat 25:2cc29fa67345 582 y_ref=0.275f+0.15*sin(t/2.5f);
MatthewMaat 25:2cc29fa67345 583 */
MatthewMaat 25:2cc29fa67345 584 if(tfloor<3)
MatthewMaat 25:2cc29fa67345 585 {
MatthewMaat 26:88639564e3af 586 x_ref=0.025+(3.0f-t)/3.0f*(x_start-0.025);
MatthewMaat 26:88639564e3af 587 y_ref=0.1+(3.0f-t)/3.0f*(y_start-0.1);
MatthewMaat 25:2cc29fa67345 588 }
MatthewMaat 26:88639564e3af 589 else if(demo_subpart==1&&y_pos<0.4)
MatthewMaat 25:2cc29fa67345 590 {
MatthewMaat 25:2cc29fa67345 591 x_ref=0.025;
MatthewMaat 26:88639564e3af 592 y_ref=min(0.41f,y_pos+v_demo);
MatthewMaat 25:2cc29fa67345 593 }
MatthewMaat 26:88639564e3af 594 else if(demo_subpart==2&&x_pos<0.075)
MatthewMaat 25:2cc29fa67345 595 {
MatthewMaat 26:88639564e3af 596 x_ref=0.085;
MatthewMaat 26:88639564e3af 597 y_ref=y_pos;
MatthewMaat 25:2cc29fa67345 598 }
MatthewMaat 26:88639564e3af 599 else if(demo_subpart==3&&y_pos>0.1)
MatthewMaat 25:2cc29fa67345 600 {
MatthewMaat 25:2cc29fa67345 601 x_ref=0.075;
MatthewMaat 26:88639564e3af 602 y_ref=max(0.09f,y_pos-v_demo);
MatthewMaat 25:2cc29fa67345 603 }
MatthewMaat 26:88639564e3af 604 else if(demo_subpart==4&&x_pos<0.125)
MatthewMaat 25:2cc29fa67345 605 {
MatthewMaat 26:88639564e3af 606 x_ref=0.135;
MatthewMaat 26:88639564e3af 607 y_ref=y_pos;
MatthewMaat 25:2cc29fa67345 608 }
MatthewMaat 26:88639564e3af 609 else if(demo_subpart==5&&y_pos<0.4)
MatthewMaat 25:2cc29fa67345 610 {
MatthewMaat 25:2cc29fa67345 611 x_ref=0.125;
MatthewMaat 26:88639564e3af 612 y_ref=min(0.41f,y_pos+v_demo);
MatthewMaat 23:b0222fa7c131 613 }
MatthewMaat 26:88639564e3af 614 else if(demo_subpart==6&&x_pos<0.175)
MatthewMaat 25:2cc29fa67345 615 {
MatthewMaat 26:88639564e3af 616 x_ref=0.185;
MatthewMaat 26:88639564e3af 617 y_ref=y_pos;
MatthewMaat 25:2cc29fa67345 618 }
MatthewMaat 26:88639564e3af 619 else if(demo_subpart==7&&y_pos>0.1)
MatthewMaat 25:2cc29fa67345 620 {
MatthewMaat 25:2cc29fa67345 621 x_ref=0.175;
MatthewMaat 26:88639564e3af 622 y_ref=max(0.09f,y_pos-v_demo);
MatthewMaat 25:2cc29fa67345 623 }
MatthewMaat 26:88639564e3af 624 else if(demo_subpart==8&&x_pos<0.225)
MatthewMaat 25:2cc29fa67345 625 {
MatthewMaat 26:88639564e3af 626 x_ref=0.235;
MatthewMaat 26:88639564e3af 627 y_ref=y_pos;
MatthewMaat 25:2cc29fa67345 628 }
MatthewMaat 26:88639564e3af 629 else if(demo_subpart==9&&y_pos<0.4)
MatthewMaat 25:2cc29fa67345 630 {
MatthewMaat 25:2cc29fa67345 631 x_ref=0.225;
MatthewMaat 26:88639564e3af 632 y_ref=min(0.41f,y_pos+v_demo);
MatthewMaat 25:2cc29fa67345 633 }
MatthewMaat 26:88639564e3af 634 else if(demo_subpart==10&&x_pos<0.275)
MatthewMaat 25:2cc29fa67345 635 {
MatthewMaat 26:88639564e3af 636 x_ref=0.285;
MatthewMaat 26:88639564e3af 637 y_ref=y_pos;
MatthewMaat 25:2cc29fa67345 638 }
MatthewMaat 26:88639564e3af 639 else if(demo_subpart==11&&y_pos>0.1)
MatthewMaat 25:2cc29fa67345 640 {
MatthewMaat 25:2cc29fa67345 641 x_ref=0.275;
MatthewMaat 26:88639564e3af 642 y_ref=max(0.09f,y_pos-v_demo);
MatthewMaat 25:2cc29fa67345 643 }
MatthewMaat 26:88639564e3af 644 else if(demo_subpart==12&&x_pos<0.325)
MatthewMaat 25:2cc29fa67345 645 {
MatthewMaat 26:88639564e3af 646 x_ref=0.335;
MatthewMaat 26:88639564e3af 647 y_ref=y_pos;
MatthewMaat 25:2cc29fa67345 648 }
MatthewMaat 26:88639564e3af 649 else if(demo_subpart==13&&y_pos<0.4)
MatthewMaat 25:2cc29fa67345 650 {
MatthewMaat 25:2cc29fa67345 651 x_ref=0.325;
MatthewMaat 26:88639564e3af 652 y_ref=min(0.41f,y_pos+v_demo);
MatthewMaat 26:88639564e3af 653 }
MatthewMaat 26:88639564e3af 654 else if(demo_subpart==14)
MatthewMaat 26:88639564e3af 655 {
MatthewMaat 26:88639564e3af 656
MatthewMaat 26:88639564e3af 657 wait(10);
MatthewMaat 26:88639564e3af 658 }
MatthewMaat 26:88639564e3af 659 else
MatthewMaat 26:88639564e3af 660 {
MatthewMaat 26:88639564e3af 661 demo_subpart+=1;
MatthewMaat 25:2cc29fa67345 662 }
MatthewMaat 25:2cc29fa67345 663 //float asdfgh=x_ref;
MatthewMaat 25:2cc29fa67345 664 //x_ref=y_ref-0.075;
MatthewMaat 25:2cc29fa67345 665 //y_ref=asdfgh+0.125;
MatthewMaat 23:b0222fa7c131 666 }
MatthewMaat 23:b0222fa7c131 667
jobjansen 20:0f6a88f29a71 668 void setPWM_controller(void){
MatthewMaat 21:a316452da8cd 669 error();
MatthewMaat 21:a316452da8cd 670 u_1 = PID1(error_1);
MatthewMaat 21:a316452da8cd 671 u_2 = PID2(error_2);
jobjansen 20:0f6a88f29a71 672
jobjansen 20:0f6a88f29a71 673 if(u_1 < 0.0f){
MatthewMaat 23:b0222fa7c131 674 dir1 = 0;
jobjansen 20:0f6a88f29a71 675 }else{
MatthewMaat 23:b0222fa7c131 676 dir1 = 1;
jobjansen 20:0f6a88f29a71 677 }
MatthewMaat 25:2cc29fa67345 678 motor1_pwm.write(min(0.5f+0.5f*fabs(u_1),1.0f));
jobjansen 20:0f6a88f29a71 679
jobjansen 20:0f6a88f29a71 680 if(u_2 < 0.0f){
MatthewMaat 23:b0222fa7c131 681 dir2 = 0;
jobjansen 20:0f6a88f29a71 682 }else{
MatthewMaat 23:b0222fa7c131 683 dir2 = 1;
jobjansen 20:0f6a88f29a71 684 }
MatthewMaat 25:2cc29fa67345 685 motor2_pwm.write(min(0.5f+0.5f*fabs(u_2),1.0f));
jobjansen 20:0f6a88f29a71 686
jobjansen 20:0f6a88f29a71 687 }
jobjansen 20:0f6a88f29a71 688
MatthewMaat 14:dc89250ebc52 689 void sample()
MatthewMaat 14:dc89250ebc52 690 {
MatthewMaat 18:8002c75b8e20 691 get_angles();
MatthewMaat 25:2cc29fa67345 692 scope.set(0,theta1);
MatthewMaat 25:2cc29fa67345 693 scope.set(1,theta2);
MatthewMaat 24:3c9ac44890f0 694 scope.set(2,v_refx);
MatthewMaat 24:3c9ac44890f0 695 scope.set(3,v_refy);
MatthewMaat 25:2cc29fa67345 696 scope.set(4,theta1_ref);
MatthewMaat 25:2cc29fa67345 697 scope.set(5,theta2_ref);
MatthewMaat 18:8002c75b8e20 698 scope.send();
MatthewMaat 14:dc89250ebc52 699 switch(currentState)
MatthewMaat 14:dc89250ebc52 700 {
MatthewMaat 15:c4799ad02cdc 701 case Waiting:
MatthewMaat 15:c4799ad02cdc 702 ledred=0;
MatthewMaat 15:c4799ad02cdc 703 ledgreen=0;
MatthewMaat 15:c4799ad02cdc 704 ledblue=1;
MatthewMaat 15:c4799ad02cdc 705 break;
MatthewMaat 15:c4799ad02cdc 706 case Position_calibration:
MatthewMaat 15:c4799ad02cdc 707 ledred=1;
MatthewMaat 15:c4799ad02cdc 708 ledgreen=1;
MatthewMaat 15:c4799ad02cdc 709 ledblue=0;
MatthewMaat 18:8002c75b8e20 710 pos_cal();
MatthewMaat 15:c4799ad02cdc 711 break;
MatthewMaat 15:c4799ad02cdc 712 case EMG_calibration:
MatthewMaat 15:c4799ad02cdc 713 ledred=1;
MatthewMaat 15:c4799ad02cdc 714 ledgreen=0;
MatthewMaat 15:c4799ad02cdc 715 ledblue=0;
MatthewMaat 15:c4799ad02cdc 716 read_emg();
MatthewMaat 15:c4799ad02cdc 717 record_min_max();
MatthewMaat 15:c4799ad02cdc 718 break;
MatthewMaat 15:c4799ad02cdc 719 case Homing:
MatthewMaat 21:a316452da8cd 720 setPWM_controller();
MatthewMaat 15:c4799ad02cdc 721 ledred=0;
MatthewMaat 15:c4799ad02cdc 722 ledgreen=1;
MatthewMaat 15:c4799ad02cdc 723 ledblue=0;
MatthewMaat 15:c4799ad02cdc 724 break;
MatthewMaat 14:dc89250ebc52 725 case Operating:
MatthewMaat 14:dc89250ebc52 726 read_emg();
MatthewMaat 26:88639564e3af 727 set_v_refxy();
MatthewMaat 25:2cc29fa67345 728 set_ref_fake_emg();
MatthewMaat 23:b0222fa7c131 729 setPWM_controller();
MatthewMaat 15:c4799ad02cdc 730 ledred=1;
MatthewMaat 15:c4799ad02cdc 731 ledgreen=0;
MatthewMaat 15:c4799ad02cdc 732 ledblue=1;
MatthewMaat 15:c4799ad02cdc 733 break;
MatthewMaat 15:c4799ad02cdc 734 case Demo:
MatthewMaat 26:88639564e3af 735 if(!demo_done)
MatthewMaat 26:88639564e3af 736 {
MatthewMaat 26:88639564e3af 737 set_refxy();
MatthewMaat 26:88639564e3af 738 setPWM_controller();
MatthewMaat 26:88639564e3af 739 }
MatthewMaat 26:88639564e3af 740 else
MatthewMaat 26:88639564e3af 741 {
MatthewMaat 26:88639564e3af 742 motor1_pwm.write(0.0f);
MatthewMaat 26:88639564e3af 743 motor2_pwm.write(0.0f);
MatthewMaat 26:88639564e3af 744 }
MatthewMaat 15:c4799ad02cdc 745 ledred=0;
MatthewMaat 15:c4799ad02cdc 746 ledgreen=0;
MatthewMaat 15:c4799ad02cdc 747 ledblue=0;
MatthewMaat 14:dc89250ebc52 748 break;
MatthewMaat 14:dc89250ebc52 749 case Failure:
MatthewMaat 14:dc89250ebc52 750 ledred=0;
MatthewMaat 14:dc89250ebc52 751 ledgreen=1;
MatthewMaat 14:dc89250ebc52 752 ledblue=1;
MatthewMaat 15:c4799ad02cdc 753 motor1_pwm.write(0.0);
MatthewMaat 15:c4799ad02cdc 754 motor2_pwm.write(0.0);
MatthewMaat 14:dc89250ebc52 755 break;
MatthewMaat 14:dc89250ebc52 756 }
MatthewMaat 21:a316452da8cd 757 //Preventing the machine from breaking
MatthewMaat 14:dc89250ebc52 758 }
MatthewMaat 14:dc89250ebc52 759
MatthewMaat 14:dc89250ebc52 760 void error_occur()
MatthewMaat 14:dc89250ebc52 761 {
MatthewMaat 14:dc89250ebc52 762 currentState=Failure;
MatthewMaat 14:dc89250ebc52 763 }
MatthewMaat 14:dc89250ebc52 764
MatthewMaat 26:88639564e3af 765 void return_home()
MatthewMaat 26:88639564e3af 766 {
MatthewMaat 26:88639564e3af 767 theta1_ref=pi/4.0f;
MatthewMaat 26:88639564e3af 768 theta2_ref=pi/4.0f;
MatthewMaat 26:88639564e3af 769 theta1_old=pi/4.0f;
MatthewMaat 26:88639564e3af 770 theta2_old=pi/4.0f;
MatthewMaat 26:88639564e3af 771 v_refx=0;
MatthewMaat 26:88639564e3af 772 EMG_peakx.attach(unignore_peaksx,0.8);
MatthewMaat 26:88639564e3af 773 ignore_peaksx=true;
MatthewMaat 26:88639564e3af 774 motorx_on=false;
MatthewMaat 26:88639564e3af 775 v_refy=0;
MatthewMaat 26:88639564e3af 776 EMG_peaky.attach(unignore_peaksy,0.8);
MatthewMaat 26:88639564e3af 777 ignore_peaksy=true;
MatthewMaat 26:88639564e3af 778 motory_on=false;
MatthewMaat 26:88639564e3af 779 }
MatthewMaat 26:88639564e3af 780
MatthewMaat 18:8002c75b8e20 781 void button_press(void)
MatthewMaat 15:c4799ad02cdc 782 //Press button to change state
MatthewMaat 15:c4799ad02cdc 783 {
MatthewMaat 18:8002c75b8e20 784 state_time.reset();
MatthewMaat 15:c4799ad02cdc 785 switch(currentState)
MatthewMaat 15:c4799ad02cdc 786 {
MatthewMaat 15:c4799ad02cdc 787 case Waiting:
MatthewMaat 15:c4799ad02cdc 788 currentState=Position_calibration;
MatthewMaat 15:c4799ad02cdc 789 wait(1);
MatthewMaat 15:c4799ad02cdc 790 break;
MatthewMaat 15:c4799ad02cdc 791 case Position_calibration:
MatthewMaat 21:a316452da8cd 792 if(!motor1_calibrated)
MatthewMaat 21:a316452da8cd 793 {
MatthewMaat 21:a316452da8cd 794 motor1_calibrated=true;
MatthewMaat 22:335a30b0825d 795 state_time.reset();
MatthewMaat 22:335a30b0825d 796 dir1=!dir1;
MatthewMaat 23:b0222fa7c131 797 motor1_pwm.write(0.0f);
MatthewMaat 21:a316452da8cd 798 }
MatthewMaat 21:a316452da8cd 799 else
MatthewMaat 21:a316452da8cd 800 {
MatthewMaat 21:a316452da8cd 801 currentState=EMG_calibration;
MatthewMaat 23:b0222fa7c131 802 motor2_pwm.write(0.0f);
MatthewMaat 23:b0222fa7c131 803 motor1_pwm.write(0.0f);
MatthewMaat 21:a316452da8cd 804 }
MatthewMaat 15:c4799ad02cdc 805 wait(1);
MatthewMaat 15:c4799ad02cdc 806 break;
MatthewMaat 15:c4799ad02cdc 807 case EMG_calibration:
MatthewMaat 15:c4799ad02cdc 808 currentState=Homing;
MatthewMaat 15:c4799ad02cdc 809 wait(1);
MatthewMaat 15:c4799ad02cdc 810 break;
MatthewMaat 15:c4799ad02cdc 811 case Homing:
MatthewMaat 15:c4799ad02cdc 812 currentState=Operating;
MatthewMaat 15:c4799ad02cdc 813 wait(1);
MatthewMaat 15:c4799ad02cdc 814 break;
MatthewMaat 15:c4799ad02cdc 815 case Operating:
MatthewMaat 15:c4799ad02cdc 816 currentState=Demo;
MatthewMaat 25:2cc29fa67345 817 Kp=24.6;
MatthewMaat 25:2cc29fa67345 818 Ki=28.3;
MatthewMaat 26:88639564e3af 819 Kd=0.1;
MatthewMaat 15:c4799ad02cdc 820 wait(1);
MatthewMaat 15:c4799ad02cdc 821 break;
MatthewMaat 15:c4799ad02cdc 822 case Demo:
MatthewMaat 15:c4799ad02cdc 823 currentState=Operating;
MatthewMaat 25:2cc29fa67345 824 Kp=15;
MatthewMaat 25:2cc29fa67345 825 Ki=38.3;
MatthewMaat 26:88639564e3af 826 Kd=10;
MatthewMaat 15:c4799ad02cdc 827 wait(1);
MatthewMaat 15:c4799ad02cdc 828 break;
MatthewMaat 15:c4799ad02cdc 829 }
MatthewMaat 15:c4799ad02cdc 830 }
MatthewMaat 15:c4799ad02cdc 831
MatthewMaat 8:ec3c634390c7 832 int main()
MatthewMaat 4:f988679bf9a1 833 {
MatthewMaat 14:dc89250ebc52 834 pc.baud(115200);
MatthewMaat 14:dc89250ebc52 835 pc.printf("Starting...");
MatthewMaat 14:dc89250ebc52 836 ledred=0;
MatthewMaat 13:ec4708dab45d 837 sample_timer.attach(&sample, 0.002);
MatthewMaat 14:dc89250ebc52 838 err.fall(error_occur);
MatthewMaat 15:c4799ad02cdc 839 button.fall(button_press);
MatthewMaat 25:2cc29fa67345 840 left_button.fall(left_fake_emg);
MatthewMaat 25:2cc29fa67345 841 left_button.rise(left_fake_emg);
MatthewMaat 26:88639564e3af 842 right_button.fall(return_home);
MatthewMaat 26:88639564e3af 843 right_button.rise(return_home);
MatthewMaat 26:88639564e3af 844 int frequency_pwm=21000;
MatthewMaat 12:7f280a661e71 845 motor1_pwm.period(1.0/frequency_pwm);
MatthewMaat 13:ec4708dab45d 846 motor2_pwm.period(1.0/frequency_pwm);
MatthewMaat 18:8002c75b8e20 847 state_time.start();
MatthewMaat 8:ec3c634390c7 848 while (true) {
MatthewMaat 12:7f280a661e71 849 wait(10);
MatthewMaat 4:f988679bf9a1 850 }
jobjansen 20:0f6a88f29a71 851
jobjansen 20:0f6a88f29a71 852 }