Matthew Maat / Mbed 2 deprecated template_biorobotics

fancy lampje

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FXOS8700Q FastPWM

main.cpp@29:78419e287e62, 2019-11-04 (annotated)

Committer:
MatthewMaat
Date:
Mon Nov 04 21:20:11 2019 +0000
Revision:
29:78419e287e62
Parent:
28:5aece9593681 
met comments

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