Matthew Maat / Mbed 2 deprecated template_biorobotics

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FXOS8700Q FastPWM

main.cpp@22:335a30b0825d, 2019-10-21 (annotated)

Committer:
MatthewMaat
Date:
Mon Oct 21 14:13:40 2019 +0000
Revision:
22:335a30b0825d
Parent:
21:a316452da8cd
Child:
23:b0222fa7c131
positie met wat bugs

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