Matthew Maat / Mbed 2 deprecated template_biorobotics

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FXOS8700Q FastPWM

main.cpp@27:c15170a5cd3d, 2019-10-31 (annotated)

Committer:
MatthewMaat
Date:
Thu Oct 31 11:23:49 2019 +0000
Revision:
27:c15170a5cd3d
Parent:
26:88639564e3af
Child:
28:5aece9593681
Theoretisch kloppende waarde voor inertia

Who changed what in which revision?

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