Jasper Gerth / Mbed 2 deprecated includeair

The final program for the #include AIR robot

Dependencies:   Biquad HIDScope QEI angleandposition controlandadjust mbed

Fork of includeair by BioRobotics

main.cpp@7:7fbb2c028778, 2015-10-14 (annotated)

Committer:
Gerth
Date:
Wed Oct 14 13:42:28 2015 +0000
Revision:
7:7fbb2c028778
Parent:
6:37c94a5e205f
Child:
8:54a7da09ccad
added control for calibration, now read out buttons;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gerth 0:dd66fff537d7 1 #include "mbed.h"
Gerth 1:917c07a4f3ec 2 #include "QEI.h"
Gerth 1:917c07a4f3ec 3 #include "HIDScope.h"
Gerth 1:917c07a4f3ec 4 #include "Biquad.h"
Gerth 1:917c07a4f3ec 5 #include "controlandadjust.h"
Gerth 1:917c07a4f3ec 6
Gerth 1:917c07a4f3ec 7 //info out
Gerth 1:917c07a4f3ec 8 HIDScope scope(4);
Gerth 1:917c07a4f3ec 9 Ticker scope_ticker;
Gerth 3:48438eea184e 10 const double scope_frequency=500;
Gerth 1:917c07a4f3ec 11 Serial pc(USBTX,USBRX);
Gerth 1:917c07a4f3ec 12
Gerth 1:917c07a4f3ec 13 ////////////////ENCODERS
Gerth 7:7fbb2c028778 14 const float cpr=32;
Gerth 7:7fbb2c028778 15 QEI encoder1(D12,D13,NC,cpr);/// maybe use Encoder in stead of QEI, because Encoder had setposition
Gerth 7:7fbb2c028778 16 QEI encoder2(D10,D11,NC,cpr);
Gerth 7:7fbb2c028778 17 const float PIE=3.14159265359;
Gerth 7:7fbb2c028778 18 const float counttorad=(cpr/(2*PIE));
Gerth 1:917c07a4f3ec 19
Gerth 6:37c94a5e205f 20 /////////////////////////////////CALIBRATION (MODE)
Gerth 6:37c94a5e205f 21 int modecounter=0;
Gerth 6:37c94a5e205f 22 DigitalIn changemodebutton(PTA4);
Gerth 6:37c94a5e205f 23
Gerth 7:7fbb2c028778 24 Ticker readbuttoncalibrate_ticker;
Gerth 7:7fbb2c028778 25 const double readbuttoncalibrate_frequency=10;
Gerth 7:7fbb2c028778 26
Gerth 1:917c07a4f3ec 27 //////////////////////////////////CONTROLLER
Gerth 1:917c07a4f3ec 28 controlandadjust mycontroller; // make a controller
Gerth 1:917c07a4f3ec 29 //controller constants
Gerth 1:917c07a4f3ec 30 float Kp=0.5;
Gerth 1:917c07a4f3ec 31 float Ki=0.01;
Gerth 1:917c07a4f3ec 32 float Kd=0.001;
Gerth 1:917c07a4f3ec 33 Ticker control_ticker;
Gerth 3:48438eea184e 34 const double control_frequency=25;
Gerth 7:7fbb2c028778 35 const double Ts_control=1.0/control_frequency;
Gerth 1:917c07a4f3ec 36
Gerth 7:7fbb2c028778 37 float error1_int=0;// storage variables for the errors
Gerth 7:7fbb2c028778 38 float error2_int=0;
Gerth 7:7fbb2c028778 39 float error1_prev=0;
Gerth 7:7fbb2c028778 40 float error2_prev=0;
Gerth 1:917c07a4f3ec 41
Gerth 1:917c07a4f3ec 42 InterruptIn valuechangebutton(PTC6);//button to change controller constants
Gerth 1:917c07a4f3ec 43
Gerth 1:917c07a4f3ec 44 //safetyandthreshold
Gerth 3:48438eea184e 45 AnalogIn safety_pot(A3);//pot 2, used for the safety cutoff value for the pwm
Gerth 3:48438eea184e 46 AnalogIn threshold_pot(A2);//pot1, used to adjust threshold if signal differs per person
Gerth 1:917c07a4f3ec 47
Gerth 1:917c07a4f3ec 48 Ticker safetyandthreshold_ticker; // ticker to read potmeters
Gerth 3:48438eea184e 49 const double safetyandthreshold_frequency=1; // frequency for the ticker
Gerth 3:48438eea184e 50
Gerth 4:bf7765b0f612 51 float threshold_value=1;//initial threshold value
Gerth 1:917c07a4f3ec 52
Gerth 1:917c07a4f3ec 53 ////////////////////////////////FILTER
Gerth 1:917c07a4f3ec 54 #include "filtervalues.h"
Gerth 1:917c07a4f3ec 55 Ticker filter_ticker;
Gerth 3:48438eea184e 56 const double filter_frequency=500;
Gerth 1:917c07a4f3ec 57 Biquad myfilter1;
Gerth 1:917c07a4f3ec 58 Biquad myfilter2;
Gerth 1:917c07a4f3ec 59
Gerth 1:917c07a4f3ec 60 AnalogIn emg1_input(A0);
Gerth 1:917c07a4f3ec 61 AnalogIn emg2_input(A1);
Gerth 1:917c07a4f3ec 62
Gerth 1:917c07a4f3ec 63 double filteredsignal1=0;
Gerth 1:917c07a4f3ec 64 double filteredsignal2=0;
Gerth 5:8ac5d0651e4d 65 float filter_extragain=1;
Gerth 1:917c07a4f3ec 66
Gerth 3:48438eea184e 67 /////////////////READSIGNAL
Gerth 3:48438eea184e 68 Ticker readsignal_ticker;
Gerth 3:48438eea184e 69 const double readsignal_frequency=25;
Gerth 3:48438eea184e 70
Gerth 4:bf7765b0f612 71 DigitalOut led1(D8);
Gerth 4:bf7765b0f612 72 DigitalOut led2(D9);
Gerth 4:bf7765b0f612 73
Gerth 4:bf7765b0f612 74 //////////////////////////////// POSITION AND ANGLE SHIZZLE
Gerth 4:bf7765b0f612 75 float desired_position=0;
Gerth 4:bf7765b0f612 76 float desired_angle[]= {0,0};
Gerth 3:48438eea184e 77
Gerth 1:917c07a4f3ec 78 //////////////////////GO FLAGS AND ACTIVATION FUNCTIONS
Gerth 1:917c07a4f3ec 79 volatile bool scopedata_go=false,
Gerth 1:917c07a4f3ec 80 control_go=false,
Gerth 1:917c07a4f3ec 81 filter_go=false,
Gerth 3:48438eea184e 82 safetyandthreshold_go=false,
Gerth 6:37c94a5e205f 83 readsignal_go=false,
Gerth 7:7fbb2c028778 84 switchedmode=false,
Gerth 7:7fbb2c028778 85 readbuttoncalibrate_go=false;
Gerth 1:917c07a4f3ec 86
Gerth 1:917c07a4f3ec 87 void scopedata_activate()
Gerth 1:917c07a4f3ec 88 {
Gerth 1:917c07a4f3ec 89 scopedata_go=true;
Gerth 1:917c07a4f3ec 90 }
Gerth 1:917c07a4f3ec 91 void control_activate()
Gerth 1:917c07a4f3ec 92 {
Gerth 1:917c07a4f3ec 93 control_go=true;
Gerth 1:917c07a4f3ec 94 }
Gerth 1:917c07a4f3ec 95 void filter_activate()
Gerth 1:917c07a4f3ec 96 {
Gerth 1:917c07a4f3ec 97 filter_go=true;
Gerth 1:917c07a4f3ec 98 }
Gerth 1:917c07a4f3ec 99 void safetyandthreshold_activate()
Gerth 1:917c07a4f3ec 100 {
Gerth 1:917c07a4f3ec 101 safetyandthreshold_go=true;
Gerth 1:917c07a4f3ec 102 }
Gerth 3:48438eea184e 103 void readsignal_activate()
Gerth 3:48438eea184e 104 {
Gerth 3:48438eea184e 105 readsignal_go=true;
Gerth 3:48438eea184e 106 }
Gerth 7:7fbb2c028778 107 void readbuttoncalibrate_activate()
Gerth 7:7fbb2c028778 108 {
Gerth 7:7fbb2c028778 109 readbuttoncalibrate_go=true;
Gerth 7:7fbb2c028778 110 }
Gerth 1:917c07a4f3ec 111
Gerth 1:917c07a4f3ec 112 ////////////////////////FUNCTIONS
Gerth 1:917c07a4f3ec 113 //gather data and send to scope
Gerth 1:917c07a4f3ec 114 void scopedata()
Gerth 1:917c07a4f3ec 115 {
Gerth 3:48438eea184e 116 scope.set(0,emg1_input.read());
Gerth 3:48438eea184e 117 scope.set(1,filteredsignal1);
Gerth 3:48438eea184e 118 scope.send();
Gerth 4:bf7765b0f612 119 }
Gerth 1:917c07a4f3ec 120 //read potmeters and adjust the safetyfactor and threshold
Gerth 1:917c07a4f3ec 121 void safetyandthreshold()
Gerth 1:917c07a4f3ec 122 {
Gerth 3:48438eea184e 123 mycontroller.cutoff((ceil (10*safety_pot.read()) )/10); // adjust the safetyfactor value between 0 and 1 rounded to 1 decimal
Gerth 3:48438eea184e 124 threshold_value=((ceil (10*threshold_pot.read()) )/10); // adjust the threshold value between 0 and 1 rounded to 1 decimal
Gerth 1:917c07a4f3ec 125 }
Gerth 1:917c07a4f3ec 126 /////filter
Gerth 1:917c07a4f3ec 127 void filtereverything()
Gerth 1:917c07a4f3ec 128 {
Gerth 1:917c07a4f3ec 129 //filter_timer.reset();
Gerth 1:917c07a4f3ec 130 // filter_timer.start();
Gerth 1:917c07a4f3ec 131 //pass1 so f1
Gerth 2:c7707856d137 132 double pass1_emg1 = myfilter1.filter(emg1_input.read(), v1_f1_emg1 , v2_f1_emg1 , a1_f1 , a2_f1 , b0_f1 , b1_f1 , b2_f1);
Gerth 2:c7707856d137 133 double pass1_emg2 = myfilter2.filter(emg2_input.read(), v1_f1_emg2 , v2_f1_emg2 , a1_f1 , a2_f1 , b0_f1 , b1_f1 , b2_f1);
Gerth 1:917c07a4f3ec 134
Gerth 1:917c07a4f3ec 135 //pass2 so f2
Gerth 2:c7707856d137 136 double pass2_emg1 = myfilter1.filter(pass1_emg1, v1_f2_emg1 , v2_f2_emg1 , a1_f2 , a2_f2 , b0_f2 , b1_f2 , b2_f2);
Gerth 2:c7707856d137 137 double pass2_emg2 = myfilter2.filter(pass1_emg2, v1_f2_emg2 , v2_f2_emg2 , a1_f2 , a2_f2 , b0_f2 , b1_f2 , b2_f2);
Gerth 1:917c07a4f3ec 138
Gerth 1:917c07a4f3ec 139 //pass3 so f3
Gerth 2:c7707856d137 140 double pass3_emg1 = myfilter1.filter(pass2_emg1, v1_f3_emg1 , v2_f3_emg1 , a1_f3 , a2_f3 , b0_f3 , b1_f3 , b2_f3);
Gerth 2:c7707856d137 141 double pass3_emg2 = myfilter2.filter(pass2_emg2, v1_f3_emg2 , v2_f3_emg2 , a1_f3 , a2_f3 , b0_f3 , b1_f3 , b2_f3);
Gerth 1:917c07a4f3ec 142
Gerth 1:917c07a4f3ec 143 //pass4 so f4
Gerth 2:c7707856d137 144 double pass4_emg1 = myfilter1.filter(pass3_emg1, v1_f4_emg1 , v2_f4_emg1 , a1_f4 , a2_f4 , b0_f4 , b1_f4 , b2_f4);
Gerth 2:c7707856d137 145 double pass4_emg2 = myfilter2.filter(pass3_emg2, v1_f4_emg2 , v2_f4_emg2 , a1_f4 , a2_f4 , b0_f4 , b1_f4 , b2_f4);
Gerth 1:917c07a4f3ec 146
Gerth 1:917c07a4f3ec 147 //pass5 so f5
Gerth 2:c7707856d137 148 double pass5_emg1 = myfilter1.filter(pass4_emg1, v1_f5_emg1 , v2_f5_emg1 , a1_f5 , a2_f5 , b0_f5 , b1_f5 , b2_f5);
Gerth 2:c7707856d137 149 double pass5_emg2 = myfilter2.filter(pass4_emg2, v1_f5_emg2 , v2_f5_emg2 , a1_f5 , a2_f5 , b0_f5 , b1_f5 , b2_f5);
Gerth 1:917c07a4f3ec 150
Gerth 1:917c07a4f3ec 151 ///// take absolute value
Gerth 2:c7707856d137 152 double pass5_emg1_abs=(fabs(pass5_emg1));
Gerth 2:c7707856d137 153 double pass5_emg2_abs=(fabs(pass5_emg2));
Gerth 1:917c07a4f3ec 154
Gerth 1:917c07a4f3ec 155 //pass6 so f6
Gerth 2:c7707856d137 156 double pass6_emg1 = myfilter1.filter(pass5_emg1_abs, v1_f6_emg1 , v2_f6_emg1 , a1_f6 , a2_f6 , b0_f6 , b1_f6 , b2_f6);
Gerth 2:c7707856d137 157 double pass6_emg2 = myfilter2.filter(pass5_emg2_abs, v1_f6_emg2 , v2_f6_emg2 , a1_f6 , a2_f6 , b0_f6 , b1_f6 , b2_f6);
Gerth 1:917c07a4f3ec 158
Gerth 1:917c07a4f3ec 159
Gerth 1:917c07a4f3ec 160 //pass7 so f7
Gerth 2:c7707856d137 161 double pass7_emg1 = myfilter1.filter(pass6_emg1, v1_f7_emg1 , v2_f7_emg1 , a1_f7 , a2_f7 , b0_f7 , b1_f7 , b2_f7);
Gerth 2:c7707856d137 162 double pass7_emg2 = myfilter2.filter(pass6_emg2, v1_f7_emg2 , v2_f7_emg2 , a1_f7 , a2_f7 , b0_f7 , b1_f7 , b2_f7);
Gerth 1:917c07a4f3ec 163
Gerth 2:c7707856d137 164 filteredsignal1=(pass7_emg1*9e11*filter_extragain);
Gerth 2:c7707856d137 165 filteredsignal2=(pass7_emg2*9e11*filter_extragain);
Gerth 1:917c07a4f3ec 166
Gerth 1:917c07a4f3ec 167 //filter_timer.stop();
Gerth 1:917c07a4f3ec 168 }
Gerth 1:917c07a4f3ec 169
Gerth 7:7fbb2c028778 170
Gerth 1:917c07a4f3ec 171
Gerth 1:917c07a4f3ec 172 //adjust controller values when sw2 is pressed
Gerth 1:917c07a4f3ec 173 void valuechange()
Gerth 1:917c07a4f3ec 174 {
Gerth 3:48438eea184e 175 mycontroller.STOP();
Gerth 5:8ac5d0651e4d 176 pc.printf("KP is now %f, enter new value\n",Kp);
Gerth 1:917c07a4f3ec 177 pc.scanf("%f", &Kp);
Gerth 1:917c07a4f3ec 178
Gerth 1:917c07a4f3ec 179 pc.printf("KI is now %f, enter new value\n",Ki);
Gerth 1:917c07a4f3ec 180 pc.scanf("%f", &Ki);
Gerth 1:917c07a4f3ec 181
Gerth 1:917c07a4f3ec 182 pc.printf("KD is now %f, enter new value\n",Kd);
Gerth 5:8ac5d0651e4d 183 pc.scanf("%f", &Kd);
Gerth 3:48438eea184e 184
Gerth 3:48438eea184e 185 pc.printf("Extra gain is now %f, enter new value\n",filter_extragain);
Gerth 5:8ac5d0651e4d 186 pc.scanf("%f", &filter_extragain);
Gerth 1:917c07a4f3ec 187 }
Gerth 3:48438eea184e 188 void readsignal()
Gerth 3:48438eea184e 189 {
Gerth 4:bf7765b0f612 190 if (filteredsignal1>=threshold_value && filteredsignal2>=threshold_value) {
Gerth 4:bf7765b0f612 191 led1=led2=1;
Gerth 4:bf7765b0f612 192 } else if (filteredsignal1>=threshold_value && filteredsignal2<=threshold_value) {
Gerth 4:bf7765b0f612 193 led1=1;
Gerth 4:bf7765b0f612 194 led2=0;
Gerth 4:bf7765b0f612 195 } else if (filteredsignal1<=threshold_value && filteredsignal2>=threshold_value) {
Gerth 4:bf7765b0f612 196 led1=0;
Gerth 4:bf7765b0f612 197 led2=1;
Gerth 3:48438eea184e 198 } else {
Gerth 4:bf7765b0f612 199 led1=led2=0;
Gerth 3:48438eea184e 200 }
Gerth 4:bf7765b0f612 201
Gerth 3:48438eea184e 202 }
Gerth 3:48438eea184e 203
Gerth 6:37c94a5e205f 204 void changemode()
Gerth 6:37c94a5e205f 205 {
Gerth 6:37c94a5e205f 206 switchedmode=true;
Gerth 6:37c94a5e205f 207 modecounter++;
Gerth 6:37c94a5e205f 208 if (modecounter==3) {
Gerth 6:37c94a5e205f 209 modecounter=0;
Gerth 6:37c94a5e205f 210 } else {
Gerth 6:37c94a5e205f 211 modecounter=modecounter;
Gerth 6:37c94a5e205f 212 }
Gerth 6:37c94a5e205f 213 wait(1);
Gerth 6:37c94a5e205f 214 }
Gerth 0:dd66fff537d7 215
Gerth 7:7fbb2c028778 216
Gerth 7:7fbb2c028778 217
Gerth 0:dd66fff537d7 218 int main()
Gerth 0:dd66fff537d7 219 {
Gerth 1:917c07a4f3ec 220 //tickers
Gerth 1:917c07a4f3ec 221 safetyandthreshold_ticker.attach(&safetyandthreshold_activate,1.0/safetyandthreshold_frequency);
Gerth 1:917c07a4f3ec 222 filter_ticker.attach(&filter_activate,1.0/filter_frequency);
Gerth 1:917c07a4f3ec 223 control_ticker.attach(&control_activate,1.0/control_frequency);
Gerth 1:917c07a4f3ec 224 scope_ticker.attach(&scopedata_activate,1.0/scope_frequency);
Gerth 3:48438eea184e 225 readsignal_ticker.attach(&readsignal_activate, 1.0/readsignal_frequency);
Gerth 7:7fbb2c028778 226 readbuttoncalibrate_ticker.attach(&readbuttoncalibrate_activate, 1.0/readbuttoncalibrate_frequency);
Gerth 1:917c07a4f3ec 227
Gerth 1:917c07a4f3ec 228 while(1) {
Gerth 6:37c94a5e205f 229 if (changemodebutton==0) {
Gerth 6:37c94a5e205f 230 changemode();
Gerth 1:917c07a4f3ec 231 }
Gerth 7:7fbb2c028778 232 ///////////////////////////////////////////NORMAL RUNNING MODE
Gerth 7:7fbb2c028778 233 if(modecounter==0) {
Gerth 6:37c94a5e205f 234 if (switchedmode==true) {
Gerth 6:37c94a5e205f 235 encoder1.reset();
Gerth 6:37c94a5e205f 236 encoder2.reset();
Gerth 6:37c94a5e205f 237 pc.printf("Program running\n");//
Gerth 6:37c94a5e205f 238 switchedmode=false;
Gerth 6:37c94a5e205f 239 }
Gerth 6:37c94a5e205f 240 if (scopedata_go==true) {
Gerth 6:37c94a5e205f 241 scopedata();
Gerth 6:37c94a5e205f 242 scopedata_go=false;
Gerth 6:37c94a5e205f 243 }
Gerth 6:37c94a5e205f 244 if (filter_go==true) {
Gerth 6:37c94a5e205f 245 filtereverything();
Gerth 6:37c94a5e205f 246 filter_go=false;
Gerth 6:37c94a5e205f 247 }
Gerth 6:37c94a5e205f 248 if (safetyandthreshold_go==true) {
Gerth 6:37c94a5e205f 249 safetyandthreshold();
Gerth 6:37c94a5e205f 250 safetyandthreshold_go=false;
Gerth 6:37c94a5e205f 251 }
Gerth 6:37c94a5e205f 252 if (control_go==true) {
Gerth 7:7fbb2c028778 253 float error1=0;
Gerth 7:7fbb2c028778 254 float error2=0;
Gerth 7:7fbb2c028778 255 mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
Gerth 6:37c94a5e205f 256 control_go=false;
Gerth 6:37c94a5e205f 257 }
Gerth 6:37c94a5e205f 258 if (readsignal_go==true) {
Gerth 6:37c94a5e205f 259 readsignal();
Gerth 6:37c94a5e205f 260 readsignal_go=false;
Gerth 6:37c94a5e205f 261 }
Gerth 6:37c94a5e205f 262 valuechangebutton.fall(&valuechange);
Gerth 1:917c07a4f3ec 263 }
Gerth 7:7fbb2c028778 264 ////////////////////////////////////////////////////CALIBRATE RIGHT ARM
Gerth 6:37c94a5e205f 265 if (modecounter==1) {
Gerth 6:37c94a5e205f 266 if(switchedmode==true) {
Gerth 6:37c94a5e205f 267 pc.printf("Calibration mode! Use buttons to move rigth arm to 0 degrees\n");
Gerth 6:37c94a5e205f 268 switchedmode=false;
Gerth 6:37c94a5e205f 269 }
Gerth 7:7fbb2c028778 270 if (control_go==true) {
Gerth 7:7fbb2c028778 271 float error1=(desired_angle[0]-counttorad*encoder1.getPulses()); // this is the error you want to use
Gerth 7:7fbb2c028778 272 float error2=0;
Gerth 7:7fbb2c028778 273 mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
Gerth 7:7fbb2c028778 274 control_go=false;
Gerth 7:7fbb2c028778 275 }
Gerth 7:7fbb2c028778 276 ////////////////////////////////////////////CALIBRATE LEFT ARM
Gerth 7:7fbb2c028778 277 if (modecounter==2) {
Gerth 7:7fbb2c028778 278 if(switchedmode==true) {
Gerth 7:7fbb2c028778 279 pc.printf("Calibration mode! Use buttons to move left arm to 0 degrees\n");
Gerth 7:7fbb2c028778 280 switchedmode=false;
Gerth 7:7fbb2c028778 281 }
Gerth 7:7fbb2c028778 282 if (control_go==true) {
Gerth 7:7fbb2c028778 283 float error1=0;
Gerth 7:7fbb2c028778 284 float error2=(desired_angle[1]-counttorad*encoder2.getPulses());// this is the error you want to use
Gerth 7:7fbb2c028778 285 mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
Gerth 7:7fbb2c028778 286 control_go=false;
Gerth 6:37c94a5e205f 287
Gerth 7:7fbb2c028778 288 }
Gerth 6:37c94a5e205f 289 }
Gerth 3:48438eea184e 290 }
Gerth 0:dd66fff537d7 291 }
Gerth 0:dd66fff537d7 292 }