Matthew Maat / Mbed 2 deprecated template_biorobotics

fancy lampje

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FXOS8700Q FastPWM

main.cpp@21:a316452da8cd, 2019-10-21 (annotated)

Committer:
MatthewMaat
Date:
Mon Oct 21 13:23:11 2019 +0000
Revision:
21:a316452da8cd
Parent:
20:0f6a88f29a71
Parent:
19:fb3d570a115e
Child:
22:335a30b0825d
Homing is added-with some safety measures

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