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@3:48438eea184e, 2015-10-13 (annotated)

Committer:
Gerth
Date:
Tue Oct 13 13:42:16 2015 +0000
Revision:
3:48438eea184e
Parent:
2:c7707856d137
Child:
4:bf7765b0f612
now with threshold working. when threshold is crossed, led lights up;

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 1:917c07a4f3ec 14 QEI encoder1(D12,D13,NC,32);
Gerth 1:917c07a4f3ec 15 QEI encoder2(D10,D11,NC,32);
Gerth 1:917c07a4f3ec 16
Gerth 1:917c07a4f3ec 17 //////////////////////////////////CONTROLLER
Gerth 1:917c07a4f3ec 18 controlandadjust mycontroller; // make a controller
Gerth 1:917c07a4f3ec 19 //controller constants
Gerth 1:917c07a4f3ec 20 float Kp=0.5;
Gerth 1:917c07a4f3ec 21 float Ki=0.01;
Gerth 1:917c07a4f3ec 22 float Kd=0.001;
Gerth 1:917c07a4f3ec 23 Ticker control_ticker;
Gerth 3:48438eea184e 24 const double control_frequency=25;
Gerth 1:917c07a4f3ec 25
Gerth 1:917c07a4f3ec 26 double error1_int=0;// storage variables for the errors
Gerth 1:917c07a4f3ec 27 double error2_int=0;
Gerth 1:917c07a4f3ec 28 double error1_prev=0;
Gerth 1:917c07a4f3ec 29 double error2_prev=0;
Gerth 1:917c07a4f3ec 30
Gerth 1:917c07a4f3ec 31 InterruptIn valuechangebutton(PTC6);//button to change controller constants
Gerth 1:917c07a4f3ec 32
Gerth 1:917c07a4f3ec 33 //safetyandthreshold
Gerth 3:48438eea184e 34 AnalogIn safety_pot(A3);//pot 2, used for the safety cutoff value for the pwm
Gerth 3:48438eea184e 35 AnalogIn threshold_pot(A2);//pot1, used to adjust threshold if signal differs per person
Gerth 1:917c07a4f3ec 36
Gerth 1:917c07a4f3ec 37 Ticker safetyandthreshold_ticker; // ticker to read potmeters
Gerth 3:48438eea184e 38 const double safetyandthreshold_frequency=1; // frequency for the ticker
Gerth 3:48438eea184e 39
Gerth 3:48438eea184e 40 float threshold_value=1;
Gerth 1:917c07a4f3ec 41
Gerth 1:917c07a4f3ec 42 ////////////////////////////////FILTER
Gerth 1:917c07a4f3ec 43 #include "filtervalues.h"
Gerth 1:917c07a4f3ec 44 Ticker filter_ticker;
Gerth 3:48438eea184e 45 const double filter_frequency=500;
Gerth 1:917c07a4f3ec 46 Biquad myfilter1;
Gerth 1:917c07a4f3ec 47 Biquad myfilter2;
Gerth 1:917c07a4f3ec 48
Gerth 1:917c07a4f3ec 49 AnalogIn emg1_input(A0);
Gerth 1:917c07a4f3ec 50 AnalogIn emg2_input(A1);
Gerth 1:917c07a4f3ec 51
Gerth 1:917c07a4f3ec 52 double filteredsignal1=0;
Gerth 1:917c07a4f3ec 53 double filteredsignal2=0;
Gerth 2:c7707856d137 54 double filter_extragain=1;
Gerth 1:917c07a4f3ec 55
Gerth 3:48438eea184e 56 /////////////////READSIGNAL
Gerth 3:48438eea184e 57 Ticker readsignal_ticker;
Gerth 3:48438eea184e 58 const double readsignal_frequency=25;
Gerth 3:48438eea184e 59
Gerth 3:48438eea184e 60 DigitalOut led(D4);
Gerth 3:48438eea184e 61
Gerth 1:917c07a4f3ec 62 //////////////////////GO FLAGS AND ACTIVATION FUNCTIONS
Gerth 1:917c07a4f3ec 63 volatile bool scopedata_go=false,
Gerth 1:917c07a4f3ec 64 control_go=false,
Gerth 1:917c07a4f3ec 65 filter_go=false,
Gerth 3:48438eea184e 66 safetyandthreshold_go=false,
Gerth 3:48438eea184e 67 readsignal_go=false;
Gerth 1:917c07a4f3ec 68
Gerth 1:917c07a4f3ec 69 void scopedata_activate()
Gerth 1:917c07a4f3ec 70 {
Gerth 1:917c07a4f3ec 71 scopedata_go=true;
Gerth 1:917c07a4f3ec 72 }
Gerth 1:917c07a4f3ec 73 void control_activate()
Gerth 1:917c07a4f3ec 74 {
Gerth 1:917c07a4f3ec 75 control_go=true;
Gerth 1:917c07a4f3ec 76 }
Gerth 1:917c07a4f3ec 77 void filter_activate()
Gerth 1:917c07a4f3ec 78 {
Gerth 1:917c07a4f3ec 79 filter_go=true;
Gerth 1:917c07a4f3ec 80 }
Gerth 1:917c07a4f3ec 81 void safetyandthreshold_activate()
Gerth 1:917c07a4f3ec 82 {
Gerth 1:917c07a4f3ec 83 safetyandthreshold_go=true;
Gerth 1:917c07a4f3ec 84 }
Gerth 3:48438eea184e 85 void readsignal_activate()
Gerth 3:48438eea184e 86 {
Gerth 3:48438eea184e 87 readsignal_go=true;
Gerth 3:48438eea184e 88 }
Gerth 0:dd66fff537d7 89
Gerth 1:917c07a4f3ec 90
Gerth 1:917c07a4f3ec 91 ////////////////////////FUNCTIONS
Gerth 1:917c07a4f3ec 92 //gather data and send to scope
Gerth 1:917c07a4f3ec 93 void scopedata()
Gerth 1:917c07a4f3ec 94 {
Gerth 3:48438eea184e 95 scope.set(0,emg1_input.read());
Gerth 3:48438eea184e 96 scope.set(1,filteredsignal1);
Gerth 3:48438eea184e 97 scope.send();
Gerth 3:48438eea184e 98 }
Gerth 1:917c07a4f3ec 99 //read potmeters and adjust the safetyfactor and threshold
Gerth 1:917c07a4f3ec 100 void safetyandthreshold()
Gerth 1:917c07a4f3ec 101 {
Gerth 3:48438eea184e 102 mycontroller.cutoff((ceil (10*safety_pot.read()) )/10); // adjust the safetyfactor value between 0 and 1 rounded to 1 decimal
Gerth 3:48438eea184e 103 threshold_value=((ceil (10*threshold_pot.read()) )/10); // adjust the threshold value between 0 and 1 rounded to 1 decimal
Gerth 1:917c07a4f3ec 104 }
Gerth 1:917c07a4f3ec 105 /////filter
Gerth 1:917c07a4f3ec 106 void filtereverything()
Gerth 1:917c07a4f3ec 107 {
Gerth 1:917c07a4f3ec 108 //filter_timer.reset();
Gerth 1:917c07a4f3ec 109 // filter_timer.start();
Gerth 1:917c07a4f3ec 110 //pass1 so f1
Gerth 2:c7707856d137 111 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 112 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 113
Gerth 1:917c07a4f3ec 114 //pass2 so f2
Gerth 2:c7707856d137 115 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 116 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 117
Gerth 1:917c07a4f3ec 118 //pass3 so f3
Gerth 2:c7707856d137 119 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 120 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 121
Gerth 1:917c07a4f3ec 122 //pass4 so f4
Gerth 2:c7707856d137 123 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 124 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 125
Gerth 1:917c07a4f3ec 126 //pass5 so f5
Gerth 2:c7707856d137 127 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 128 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 129
Gerth 1:917c07a4f3ec 130 ///// take absolute value
Gerth 2:c7707856d137 131 double pass5_emg1_abs=(fabs(pass5_emg1));
Gerth 2:c7707856d137 132 double pass5_emg2_abs=(fabs(pass5_emg2));
Gerth 1:917c07a4f3ec 133
Gerth 1:917c07a4f3ec 134 //pass6 so f6
Gerth 2:c7707856d137 135 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 136 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 137
Gerth 1:917c07a4f3ec 138
Gerth 1:917c07a4f3ec 139 //pass7 so f7
Gerth 2:c7707856d137 140 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 141 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 142
Gerth 2:c7707856d137 143 filteredsignal1=(pass7_emg1*9e11*filter_extragain);
Gerth 2:c7707856d137 144 filteredsignal2=(pass7_emg2*9e11*filter_extragain);
Gerth 1:917c07a4f3ec 145
Gerth 1:917c07a4f3ec 146 //filter_timer.stop();
Gerth 1:917c07a4f3ec 147 }
Gerth 1:917c07a4f3ec 148
Gerth 3:48438eea184e 149 void control()
Gerth 3:48438eea184e 150 {
Gerth 1:917c07a4f3ec 151 ///call desired controller here
Gerth 3:48438eea184e 152 }
Gerth 1:917c07a4f3ec 153
Gerth 1:917c07a4f3ec 154 //adjust controller values when sw2 is pressed
Gerth 1:917c07a4f3ec 155 void valuechange()
Gerth 1:917c07a4f3ec 156 {
Gerth 3:48438eea184e 157 mycontroller.STOP();
Gerth 3:48438eea184e 158 /*pc.printf("KP is now %f, enter new value\n",Kp);
Gerth 1:917c07a4f3ec 159 pc.scanf("%f", &Kp);
Gerth 1:917c07a4f3ec 160
Gerth 1:917c07a4f3ec 161 pc.printf("KI is now %f, enter new value\n",Ki);
Gerth 1:917c07a4f3ec 162 pc.scanf("%f", &Ki);
Gerth 1:917c07a4f3ec 163
Gerth 1:917c07a4f3ec 164 pc.printf("KD is now %f, enter new value\n",Kd);
Gerth 2:c7707856d137 165 pc.scanf("%f", &Kd);*/
Gerth 3:48438eea184e 166
Gerth 3:48438eea184e 167 pc.printf("Extra gain is now %f, enter new value\n",filter_extragain);
Gerth 3:48438eea184e 168 pc.scanf("%f",&filter_extragain);
Gerth 1:917c07a4f3ec 169 }
Gerth 3:48438eea184e 170 void readsignal()
Gerth 3:48438eea184e 171 {
Gerth 3:48438eea184e 172 if (filteredsignal1>=threshold_value) {
Gerth 3:48438eea184e 173 led=1;
Gerth 3:48438eea184e 174 } else {
Gerth 3:48438eea184e 175 led=0;
Gerth 3:48438eea184e 176 }
Gerth 3:48438eea184e 177 }
Gerth 3:48438eea184e 178
Gerth 0:dd66fff537d7 179
Gerth 0:dd66fff537d7 180 int main()
Gerth 0:dd66fff537d7 181 {
Gerth 1:917c07a4f3ec 182 //tickers
Gerth 1:917c07a4f3ec 183 safetyandthreshold_ticker.attach(&safetyandthreshold_activate,1.0/safetyandthreshold_frequency);
Gerth 1:917c07a4f3ec 184 filter_ticker.attach(&filter_activate,1.0/filter_frequency);
Gerth 1:917c07a4f3ec 185 control_ticker.attach(&control_activate,1.0/control_frequency);
Gerth 1:917c07a4f3ec 186 scope_ticker.attach(&scopedata_activate,1.0/scope_frequency);
Gerth 3:48438eea184e 187 readsignal_ticker.attach(&readsignal_activate, 1.0/readsignal_frequency);
Gerth 1:917c07a4f3ec 188
Gerth 1:917c07a4f3ec 189 while(1) {
Gerth 1:917c07a4f3ec 190 if (scopedata_go==true) {
Gerth 1:917c07a4f3ec 191 scopedata();
Gerth 1:917c07a4f3ec 192 scopedata_go=false;
Gerth 1:917c07a4f3ec 193 }
Gerth 1:917c07a4f3ec 194 if (filter_go==true) {
Gerth 1:917c07a4f3ec 195 filtereverything();
Gerth 1:917c07a4f3ec 196 filter_go=false;
Gerth 1:917c07a4f3ec 197 }
Gerth 1:917c07a4f3ec 198 if (safetyandthreshold_go==true) {
Gerth 1:917c07a4f3ec 199 safetyandthreshold();
Gerth 1:917c07a4f3ec 200 safetyandthreshold_go=false;
Gerth 1:917c07a4f3ec 201 }
Gerth 1:917c07a4f3ec 202 if (control_go==true) {
Gerth 1:917c07a4f3ec 203 control();
Gerth 1:917c07a4f3ec 204 control_go=false;
Gerth 1:917c07a4f3ec 205 }
Gerth 3:48438eea184e 206 if (readsignal_go==true) {
Gerth 3:48438eea184e 207 readsignal();
Gerth 3:48438eea184e 208 readsignal_go=false;
Gerth 3:48438eea184e 209 }
Gerth 3:48438eea184e 210 valuechangebutton.fall(&valuechange);
Gerth 0:dd66fff537d7 211 }
Gerth 0:dd66fff537d7 212 }