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@14:4c4f45a1dd23, 2015-10-19 (annotated)

Committer:
Gerth
Date:
Mon Oct 19 09:13:01 2015 +0000
Revision:
14:4c4f45a1dd23
Parent:
13:47b065aadae9
Child:
15:17de575b7385
added led info green led is normal mode, blinking blue led is shooting, red led and corresponding blinking led on top is calibration mode

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 10:9e96d14d7034 8 HIDScope scope(6);
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 14:4c4f45a1dd23 13 DigitalOut ledred(LED1);
Gerth 14:4c4f45a1dd23 14 DigitalOut ledgreen(LED2);
Gerth 14:4c4f45a1dd23 15 DigitalOut ledblue(LED3);
Gerth 14:4c4f45a1dd23 16
Gerth 1:917c07a4f3ec 17 ////////////////ENCODERS
Gerth 9:4ee354663560 18 const float cpr_sensor=32;
Gerth 9:4ee354663560 19 const float cpr_shaft=cpr_sensor*131;
Gerth 9:4ee354663560 20 QEI encoder1(D12,D13,NC,cpr_sensor);/// maybe use Encoder in stead of QEI, because Encoder had setposition
Gerth 9:4ee354663560 21 QEI encoder2(D10,D11,NC,cpr_sensor);
Gerth 9:4ee354663560 22 const double PIE=3.14159265359;
Gerth 9:4ee354663560 23 const float counttorad=((2*PIE)/cpr_shaft);
Gerth 1:917c07a4f3ec 24
Gerth 6:37c94a5e205f 25 /////////////////////////////////CALIBRATION (MODE)
Gerth 8:54a7da09ccad 26 int modecounter=1;
Gerth 6:37c94a5e205f 27 DigitalIn changemodebutton(PTA4);
Gerth 6:37c94a5e205f 28
Gerth 7:7fbb2c028778 29 Ticker readbuttoncalibrate_ticker;
Gerth 7:7fbb2c028778 30 const double readbuttoncalibrate_frequency=10;
Gerth 7:7fbb2c028778 31
Gerth 14:4c4f45a1dd23 32 Ticker ledblink_ticker;
Gerth 14:4c4f45a1dd23 33 const double ledblink_frequency=4;
Gerth 14:4c4f45a1dd23 34
Gerth 9:4ee354663560 35 const double radpersec_calibrate=0.1*PIE;
Gerth 8:54a7da09ccad 36
Gerth 8:54a7da09ccad 37 DigitalIn buttonR(D2);
Gerth 8:54a7da09ccad 38 DigitalIn buttonL(D3);
Gerth 8:54a7da09ccad 39
Gerth 14:4c4f45a1dd23 40
Gerth 14:4c4f45a1dd23 41
Gerth 1:917c07a4f3ec 42 //////////////////////////////////CONTROLLER
Gerth 1:917c07a4f3ec 43 controlandadjust mycontroller; // make a controller
Gerth 1:917c07a4f3ec 44 //controller constants
Gerth 1:917c07a4f3ec 45 float Kp=0.5;
Gerth 9:4ee354663560 46 float Ki=0.01;
Gerth 1:917c07a4f3ec 47 float Kd=0.001;
Gerth 1:917c07a4f3ec 48 Ticker control_ticker;
Gerth 9:4ee354663560 49 const double control_frequency=25;
Gerth 7:7fbb2c028778 50 const double Ts_control=1.0/control_frequency;
Gerth 1:917c07a4f3ec 51
Gerth 7:7fbb2c028778 52 float error1_int=0;// storage variables for the errors
Gerth 7:7fbb2c028778 53 float error2_int=0;
Gerth 7:7fbb2c028778 54 float error1_prev=0;
Gerth 7:7fbb2c028778 55 float error2_prev=0;
Gerth 1:917c07a4f3ec 56
Gerth 1:917c07a4f3ec 57 InterruptIn valuechangebutton(PTC6);//button to change controller constants
Gerth 1:917c07a4f3ec 58
Gerth 1:917c07a4f3ec 59 //safetyandthreshold
Gerth 3:48438eea184e 60 AnalogIn safety_pot(A3);//pot 2, used for the safety cutoff value for the pwm
Gerth 3:48438eea184e 61 AnalogIn threshold_pot(A2);//pot1, used to adjust threshold if signal differs per person
Gerth 1:917c07a4f3ec 62
Gerth 1:917c07a4f3ec 63 Ticker safetyandthreshold_ticker; // ticker to read potmeters
Gerth 3:48438eea184e 64 const double safetyandthreshold_frequency=1; // frequency for the ticker
Gerth 3:48438eea184e 65
Gerth 4:bf7765b0f612 66 float threshold_value=1;//initial threshold value
Gerth 1:917c07a4f3ec 67
Gerth 1:917c07a4f3ec 68 ////////////////////////////////FILTER
Gerth 1:917c07a4f3ec 69 #include "filtervalues.h"
Gerth 1:917c07a4f3ec 70 Ticker filter_ticker;
Gerth 3:48438eea184e 71 const double filter_frequency=500;
Gerth 1:917c07a4f3ec 72 Biquad myfilter1;
Gerth 1:917c07a4f3ec 73 Biquad myfilter2;
Gerth 1:917c07a4f3ec 74
Gerth 1:917c07a4f3ec 75 AnalogIn emg1_input(A0);
Gerth 1:917c07a4f3ec 76 AnalogIn emg2_input(A1);
Gerth 1:917c07a4f3ec 77
Gerth 1:917c07a4f3ec 78 double filteredsignal1=0;
Gerth 1:917c07a4f3ec 79 double filteredsignal2=0;
Gerth 5:8ac5d0651e4d 80 float filter_extragain=1;
Gerth 1:917c07a4f3ec 81
Gerth 3:48438eea184e 82 /////////////////READSIGNAL
Gerth 3:48438eea184e 83 Ticker readsignal_ticker;
Gerth 3:48438eea184e 84 const double readsignal_frequency=25;
Gerth 3:48438eea184e 85
Gerth 11:c10651055871 86 DigitalOut led1(PTC12);
Gerth 4:bf7765b0f612 87 DigitalOut led2(D9);
Gerth 4:bf7765b0f612 88
Gerth 4:bf7765b0f612 89 //////////////////////////////// POSITION AND ANGLE SHIZZLE
Gerth 4:bf7765b0f612 90 float desired_position=0;
Gerth 4:bf7765b0f612 91 float desired_angle[]= {0,0};
Gerth 11:c10651055871 92 float mm_per_sec_emg=50;// move the pod 50 mm per sec if muscle is flexed
Gerth 11:c10651055871 93 float fieldwidth=473;
Gerth 11:c10651055871 94 float safetymarginfield=75; //adjustable, tweak for maximum but safe range
Gerth 11:c10651055871 95 float maxdisplacement=((fieldwidth/2)-safetymarginfield); //so the pod doesn't hit the edges of the playfield
Gerth 11:c10651055871 96 float rad_per_sec_emg=0.25*PIE;// THIS ONE IS NOT NESSECARY FOR ACTUAL PROGRAM
Gerth 3:48438eea184e 97
Gerth 1:917c07a4f3ec 98 //////////////////////GO FLAGS AND ACTIVATION FUNCTIONS
Gerth 1:917c07a4f3ec 99 volatile bool scopedata_go=false,
Gerth 1:917c07a4f3ec 100 control_go=false,
Gerth 1:917c07a4f3ec 101 filter_go=false,
Gerth 3:48438eea184e 102 safetyandthreshold_go=false,
Gerth 6:37c94a5e205f 103 readsignal_go=false,
Gerth 8:54a7da09ccad 104 switchedmode=true,
Gerth 14:4c4f45a1dd23 105 readbuttoncalibrate_go=false,
Gerth 14:4c4f45a1dd23 106 ledblink_go=false;
Gerth 1:917c07a4f3ec 107
Gerth 1:917c07a4f3ec 108 void scopedata_activate()
Gerth 1:917c07a4f3ec 109 {
Gerth 1:917c07a4f3ec 110 scopedata_go=true;
Gerth 1:917c07a4f3ec 111 }
Gerth 1:917c07a4f3ec 112 void control_activate()
Gerth 1:917c07a4f3ec 113 {
Gerth 1:917c07a4f3ec 114 control_go=true;
Gerth 1:917c07a4f3ec 115 }
Gerth 1:917c07a4f3ec 116 void filter_activate()
Gerth 1:917c07a4f3ec 117 {
Gerth 1:917c07a4f3ec 118 filter_go=true;
Gerth 1:917c07a4f3ec 119 }
Gerth 1:917c07a4f3ec 120 void safetyandthreshold_activate()
Gerth 1:917c07a4f3ec 121 {
Gerth 1:917c07a4f3ec 122 safetyandthreshold_go=true;
Gerth 1:917c07a4f3ec 123 }
Gerth 3:48438eea184e 124 void readsignal_activate()
Gerth 3:48438eea184e 125 {
Gerth 3:48438eea184e 126 readsignal_go=true;
Gerth 3:48438eea184e 127 }
Gerth 7:7fbb2c028778 128 void readbuttoncalibrate_activate()
Gerth 7:7fbb2c028778 129 {
Gerth 7:7fbb2c028778 130 readbuttoncalibrate_go=true;
Gerth 7:7fbb2c028778 131 }
Gerth 14:4c4f45a1dd23 132 void ledblink_activate()
Gerth 14:4c4f45a1dd23 133 {
Gerth 14:4c4f45a1dd23 134 ledblink_go=true;
Gerth 14:4c4f45a1dd23 135 }
Gerth 1:917c07a4f3ec 136
Gerth 1:917c07a4f3ec 137 ////////////////////////FUNCTIONS
Gerth 1:917c07a4f3ec 138 //gather data and send to scope
Gerth 1:917c07a4f3ec 139 void scopedata()
Gerth 1:917c07a4f3ec 140 {
Gerth 10:9e96d14d7034 141 scope.set(0,desired_angle[0]);
Gerth 10:9e96d14d7034 142 scope.set(1,counttorad*encoder1.getPulses());
Gerth 10:9e96d14d7034 143 scope.set(2,mycontroller.motor1pwm());
Gerth 10:9e96d14d7034 144 scope.set(3,desired_angle[1]);
Gerth 10:9e96d14d7034 145 scope.set(4,counttorad*encoder2.getPulses());
Gerth 12:b9f0b92bd659 146 scope.set(5,filteredsignal1);
Gerth 3:48438eea184e 147 scope.send();
Gerth 4:bf7765b0f612 148 }
Gerth 1:917c07a4f3ec 149 //read potmeters and adjust the safetyfactor and threshold
Gerth 1:917c07a4f3ec 150 void safetyandthreshold()
Gerth 1:917c07a4f3ec 151 {
Gerth 3:48438eea184e 152 mycontroller.cutoff((ceil (10*safety_pot.read()) )/10); // adjust the safetyfactor value between 0 and 1 rounded to 1 decimal
Gerth 3:48438eea184e 153 threshold_value=((ceil (10*threshold_pot.read()) )/10); // adjust the threshold value between 0 and 1 rounded to 1 decimal
Gerth 1:917c07a4f3ec 154 }
Gerth 1:917c07a4f3ec 155 /////filter
Gerth 1:917c07a4f3ec 156 void filtereverything()
Gerth 1:917c07a4f3ec 157 {
Gerth 1:917c07a4f3ec 158 //filter_timer.reset();
Gerth 1:917c07a4f3ec 159 // filter_timer.start();
Gerth 1:917c07a4f3ec 160 //pass1 so f1
Gerth 2:c7707856d137 161 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 162 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 163
Gerth 1:917c07a4f3ec 164 //pass2 so f2
Gerth 2:c7707856d137 165 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 166 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 167
Gerth 1:917c07a4f3ec 168 //pass3 so f3
Gerth 2:c7707856d137 169 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 170 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 171
Gerth 1:917c07a4f3ec 172 //pass4 so f4
Gerth 2:c7707856d137 173 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 174 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 175
Gerth 1:917c07a4f3ec 176 //pass5 so f5
Gerth 2:c7707856d137 177 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 178 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 179
Gerth 1:917c07a4f3ec 180 ///// take absolute value
Gerth 2:c7707856d137 181 double pass5_emg1_abs=(fabs(pass5_emg1));
Gerth 2:c7707856d137 182 double pass5_emg2_abs=(fabs(pass5_emg2));
Gerth 1:917c07a4f3ec 183
Gerth 1:917c07a4f3ec 184 //pass6 so f6
Gerth 2:c7707856d137 185 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 186 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 187
Gerth 1:917c07a4f3ec 188
Gerth 1:917c07a4f3ec 189 //pass7 so f7
Gerth 2:c7707856d137 190 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 191 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 192
Gerth 2:c7707856d137 193 filteredsignal1=(pass7_emg1*9e11*filter_extragain);
Gerth 2:c7707856d137 194 filteredsignal2=(pass7_emg2*9e11*filter_extragain);
Gerth 1:917c07a4f3ec 195
Gerth 1:917c07a4f3ec 196 //filter_timer.stop();
Gerth 1:917c07a4f3ec 197 }
Gerth 1:917c07a4f3ec 198
Gerth 7:7fbb2c028778 199
Gerth 1:917c07a4f3ec 200
Gerth 1:917c07a4f3ec 201 //adjust controller values when sw2 is pressed
Gerth 1:917c07a4f3ec 202 void valuechange()
Gerth 1:917c07a4f3ec 203 {
Gerth 3:48438eea184e 204 mycontroller.STOP();
Gerth 5:8ac5d0651e4d 205 pc.printf("KP is now %f, enter new value\n",Kp);
Gerth 1:917c07a4f3ec 206 pc.scanf("%f", &Kp);
Gerth 1:917c07a4f3ec 207
Gerth 1:917c07a4f3ec 208 pc.printf("KI is now %f, enter new value\n",Ki);
Gerth 1:917c07a4f3ec 209 pc.scanf("%f", &Ki);
Gerth 1:917c07a4f3ec 210
Gerth 1:917c07a4f3ec 211 pc.printf("KD is now %f, enter new value\n",Kd);
Gerth 5:8ac5d0651e4d 212 pc.scanf("%f", &Kd);
Gerth 3:48438eea184e 213
Gerth 3:48438eea184e 214 pc.printf("Extra gain is now %f, enter new value\n",filter_extragain);
Gerth 5:8ac5d0651e4d 215 pc.scanf("%f", &filter_extragain);
Gerth 1:917c07a4f3ec 216 }
Gerth 11:c10651055871 217
Gerth 11:c10651055871 218
Gerth 11:c10651055871 219
Gerth 11:c10651055871 220 const float schiethoek=0.35*PIE;
Gerth 11:c10651055871 221 const float schiettijd=0.5;
Gerth 11:c10651055871 222 void shoot() // THIS NEEDS ADJUSTMEND
Gerth 14:4c4f45a1dd23 223 {
Gerth 11:c10651055871 224 pc.printf("SHOOT\n");
Gerth 11:c10651055871 225 //hoeken groter maken
Gerth 11:c10651055871 226 desired_angle[0]-=schiethoek;
Gerth 11:c10651055871 227 desired_angle[1]+=schiethoek;
Gerth 11:c10651055871 228
Gerth 11:c10651055871 229 Timer schiettimer;
Gerth 11:c10651055871 230 schiettimer.reset();
Gerth 11:c10651055871 231 schiettimer.start();
Gerth 11:c10651055871 232 float pass=0;
Gerth 11:c10651055871 233 while(schiettimer.read()<=schiettijd) {
Gerth 11:c10651055871 234 // errors berekenen en naar de controller passen
Gerth 14:4c4f45a1dd23 235 float error1=(desired_angle[0]-counttorad*encoder1.getPulses());
Gerth 14:4c4f45a1dd23 236 float error2=(desired_angle[1]-counttorad*encoder2.getPulses());
Gerth 11:c10651055871 237 mycontroller.PI(error1, error2, Kp, Ki, Ts_control, error1_int, error2_int);
Gerth 11:c10651055871 238 scopedata();
Gerth 11:c10651055871 239 wait (Ts_control-(schiettimer.read()-Ts_control*pass)); // even wachten anders wordt de while loop te snel doorlopen en gaan de motoren wak
Gerth 14:4c4f45a1dd23 240 ledblue=!ledblue;
Gerth 11:c10651055871 241 pass++;
Gerth 11:c10651055871 242 }
Gerth 11:c10651055871 243 schiettimer.stop();
Gerth 14:4c4f45a1dd23 244 ledblue=0;
Gerth 11:c10651055871 245 //terug na schieten
Gerth 11:c10651055871 246 desired_angle[0]+=schiethoek;
Gerth 11:c10651055871 247 desired_angle[1]-=schiethoek;
Gerth 11:c10651055871 248 }
Gerth 11:c10651055871 249
Gerth 3:48438eea184e 250 void readsignal()
Gerth 3:48438eea184e 251 {
Gerth 11:c10651055871 252 //check if pod has to shoot
Gerth 4:bf7765b0f612 253 if (filteredsignal1>=threshold_value && filteredsignal2>=threshold_value) {
Gerth 4:bf7765b0f612 254 led1=led2=1;
Gerth 11:c10651055871 255 shoot();
Gerth 14:4c4f45a1dd23 256 // check if pod has to move to the right
Gerth 4:bf7765b0f612 257 } else if (filteredsignal1>=threshold_value && filteredsignal2<=threshold_value) {
Gerth 4:bf7765b0f612 258 led1=1;
Gerth 4:bf7765b0f612 259 led2=0;
Gerth 11:c10651055871 260 desired_position += (rad_per_sec_emg/readsignal_frequency);// move desiredposition right ADJUS TO MM IN LAST VERSEION
Gerth 11:c10651055871 261 if (desired_position>=maxdisplacement) {//check if the pod doesnt move too far and hit the edge
Gerth 11:c10651055871 262 desired_position=maxdisplacement;
Gerth 11:c10651055871 263 } else {
Gerth 11:c10651055871 264 desired_position=desired_position;
Gerth 11:c10651055871 265 }
Gerth 14:4c4f45a1dd23 266 // check if pod has to move to the left
Gerth 4:bf7765b0f612 267 } else if (filteredsignal1<=threshold_value && filteredsignal2>=threshold_value) {
Gerth 4:bf7765b0f612 268 led1=0;
Gerth 4:bf7765b0f612 269 led2=1;
Gerth 11:c10651055871 270 desired_position -= (rad_per_sec_emg/readsignal_frequency);//move desiredposition left ADJUST TO MM IN FINAL VERSION
Gerth 11:c10651055871 271 if (desired_position<=(-1*maxdisplacement)) {//check if the pod doesnt move too far and hit the edge
Gerth 14:4c4f45a1dd23 272 desired_position=(-1*maxdisplacement);
Gerth 11:c10651055871 273 } else {
Gerth 11:c10651055871 274 desired_position=desired_position;
Gerth 11:c10651055871 275 }
Gerth 3:48438eea184e 276 } else {
Gerth 4:bf7765b0f612 277 led1=led2=0;
Gerth 3:48438eea184e 278 }
Gerth 13:47b065aadae9 279 desired_angle[0]=(desired_position);// REMOVE IN FINAL VERSION
Gerth 11:c10651055871 280 desired_angle[1]=desired_position;//REMOVE IN FINAL VERSION
Gerth 3:48438eea184e 281 }
Gerth 3:48438eea184e 282
Gerth 6:37c94a5e205f 283 void changemode()
Gerth 6:37c94a5e205f 284 {
Gerth 10:9e96d14d7034 285 mycontroller.STOP();
Gerth 6:37c94a5e205f 286 switchedmode=true;
Gerth 6:37c94a5e205f 287 modecounter++;
Gerth 6:37c94a5e205f 288 if (modecounter==3) {
Gerth 6:37c94a5e205f 289 modecounter=0;
Gerth 6:37c94a5e205f 290 } else {
Gerth 6:37c94a5e205f 291 modecounter=modecounter;
Gerth 6:37c94a5e205f 292 }
Gerth 6:37c94a5e205f 293 wait(1);
Gerth 6:37c94a5e205f 294 }
Gerth 0:dd66fff537d7 295
Gerth 7:7fbb2c028778 296
Gerth 7:7fbb2c028778 297
Gerth 0:dd66fff537d7 298 int main()
Gerth 0:dd66fff537d7 299 {
Gerth 1:917c07a4f3ec 300 //tickers
Gerth 1:917c07a4f3ec 301 safetyandthreshold_ticker.attach(&safetyandthreshold_activate,1.0/safetyandthreshold_frequency);
Gerth 1:917c07a4f3ec 302 filter_ticker.attach(&filter_activate,1.0/filter_frequency);
Gerth 1:917c07a4f3ec 303 control_ticker.attach(&control_activate,1.0/control_frequency);
Gerth 1:917c07a4f3ec 304 scope_ticker.attach(&scopedata_activate,1.0/scope_frequency);
Gerth 3:48438eea184e 305 readsignal_ticker.attach(&readsignal_activate, 1.0/readsignal_frequency);
Gerth 7:7fbb2c028778 306 readbuttoncalibrate_ticker.attach(&readbuttoncalibrate_activate, 1.0/readbuttoncalibrate_frequency);
Gerth 14:4c4f45a1dd23 307 ledblink_ticker.attach(&ledblink_activate, 1.0/ledblink_frequency);
Gerth 14:4c4f45a1dd23 308
Gerth 14:4c4f45a1dd23 309 ledred=0;
Gerth 14:4c4f45a1dd23 310 ledgreen=0;
Gerth 14:4c4f45a1dd23 311 ledblue=0;
Gerth 1:917c07a4f3ec 312
Gerth 1:917c07a4f3ec 313 while(1) {
Gerth 6:37c94a5e205f 314 if (changemodebutton==0) {
Gerth 6:37c94a5e205f 315 changemode();
Gerth 1:917c07a4f3ec 316 }
Gerth 9:4ee354663560 317 if (scopedata_go==true) {
Gerth 9:4ee354663560 318 scopedata();
Gerth 9:4ee354663560 319 scopedata_go=false;
Gerth 9:4ee354663560 320 }
Gerth 9:4ee354663560 321 if (safetyandthreshold_go==true) {
Gerth 9:4ee354663560 322 safetyandthreshold();
Gerth 9:4ee354663560 323 safetyandthreshold_go=false;
Gerth 9:4ee354663560 324 }
Gerth 7:7fbb2c028778 325 ///////////////////////////////////////////NORMAL RUNNING MODE
Gerth 7:7fbb2c028778 326 if(modecounter==0) {
Gerth 6:37c94a5e205f 327 if (switchedmode==true) {
Gerth 6:37c94a5e205f 328 encoder1.reset();
Gerth 6:37c94a5e205f 329 encoder2.reset();
Gerth 6:37c94a5e205f 330 pc.printf("Program running\n");//
Gerth 14:4c4f45a1dd23 331 ledgreen=1;
Gerth 14:4c4f45a1dd23 332 led1=led2=ledred=0;
Gerth 6:37c94a5e205f 333 switchedmode=false;
Gerth 6:37c94a5e205f 334 }
Gerth 9:4ee354663560 335
Gerth 6:37c94a5e205f 336 if (filter_go==true) {
Gerth 6:37c94a5e205f 337 filtereverything();
Gerth 6:37c94a5e205f 338 filter_go=false;
Gerth 6:37c94a5e205f 339 }
Gerth 6:37c94a5e205f 340 if (control_go==true) {
Gerth 11:c10651055871 341 float error1=(desired_angle[0]-counttorad*encoder1.getPulses());
Gerth 11:c10651055871 342 float error2=(desired_angle[1]-counttorad*encoder2.getPulses());
Gerth 7:7fbb2c028778 343 mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
Gerth 6:37c94a5e205f 344 control_go=false;
Gerth 6:37c94a5e205f 345 }
Gerth 6:37c94a5e205f 346 if (readsignal_go==true) {
Gerth 6:37c94a5e205f 347 readsignal();
Gerth 6:37c94a5e205f 348 readsignal_go=false;
Gerth 6:37c94a5e205f 349 }
Gerth 6:37c94a5e205f 350 valuechangebutton.fall(&valuechange);
Gerth 1:917c07a4f3ec 351 }
Gerth 7:7fbb2c028778 352 ////////////////////////////////////////////////////CALIBRATE RIGHT ARM
Gerth 6:37c94a5e205f 353 if (modecounter==1) {
Gerth 6:37c94a5e205f 354 if(switchedmode==true) {
Gerth 6:37c94a5e205f 355 pc.printf("Calibration mode! Use buttons to move rigth arm to 0 degrees\n");
Gerth 6:37c94a5e205f 356 switchedmode=false;
Gerth 14:4c4f45a1dd23 357 ledred=1;
Gerth 14:4c4f45a1dd23 358 led1=led2=ledgreen=0;
Gerth 14:4c4f45a1dd23 359 }
Gerth 14:4c4f45a1dd23 360 if (ledblink_go==true) {
Gerth 14:4c4f45a1dd23 361 led1=!led1;
Gerth 6:37c94a5e205f 362 }
Gerth 7:7fbb2c028778 363 if (control_go==true) {
Gerth 11:c10651055871 364 float error1=(desired_angle[0]-counttorad*encoder1.getPulses());
Gerth 9:4ee354663560 365 float error2=0;// this is the error you want to use
Gerth 7:7fbb2c028778 366 mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
Gerth 7:7fbb2c028778 367 control_go=false;
Gerth 7:7fbb2c028778 368 }
Gerth 9:4ee354663560 369 if (readbuttoncalibrate_go==true) {
Gerth 9:4ee354663560 370 if (buttonR.read()==0 && buttonL.read()==1) {
Gerth 9:4ee354663560 371 desired_angle[0] += (radpersec_calibrate/readbuttoncalibrate_frequency);
Gerth 9:4ee354663560 372 readbuttoncalibrate_go=false;
Gerth 9:4ee354663560 373 }
Gerth 9:4ee354663560 374 if (buttonR.read()==1 && buttonL.read()==0) {
Gerth 9:4ee354663560 375 desired_angle[0] -= (radpersec_calibrate/readbuttoncalibrate_frequency);
Gerth 9:4ee354663560 376 readbuttoncalibrate_go=false;
Gerth 9:4ee354663560 377 }
Gerth 9:4ee354663560 378 }
Gerth 8:54a7da09ccad 379 }
Gerth 8:54a7da09ccad 380 ////////////////////////////////////////////CALIBRATE LEFT ARM
Gerth 8:54a7da09ccad 381 if (modecounter==2) {
Gerth 8:54a7da09ccad 382 if(switchedmode==true) {
Gerth 8:54a7da09ccad 383 pc.printf("Calibration mode! Use buttons to move left arm to 0 degrees\n");
Gerth 14:4c4f45a1dd23 384 ledred=1;
Gerth 14:4c4f45a1dd23 385 led1=led2=ledgreen=0;
Gerth 8:54a7da09ccad 386 switchedmode=false;
Gerth 8:54a7da09ccad 387 }
Gerth 14:4c4f45a1dd23 388 if (ledblink_go==true) {
Gerth 14:4c4f45a1dd23 389 led2=!led2;
Gerth 14:4c4f45a1dd23 390 }
Gerth 8:54a7da09ccad 391 if (control_go==true) {
Gerth 8:54a7da09ccad 392 float error1=0;
Gerth 8:54a7da09ccad 393 float error2=(desired_angle[1]-counttorad*encoder2.getPulses());// this is the error you want to use
Gerth 8:54a7da09ccad 394 mycontroller.PI(error1,error2,Kp,Ki,Ts_control,error1_int,error2_int);
Gerth 8:54a7da09ccad 395 control_go=false;
Gerth 8:54a7da09ccad 396 }
Gerth 8:54a7da09ccad 397 if (readbuttoncalibrate_go==true) {
Gerth 8:54a7da09ccad 398 if (buttonR.read()==0 && buttonL.read()==1) {
Gerth 8:54a7da09ccad 399 desired_angle[1] += (radpersec_calibrate/readbuttoncalibrate_frequency);
Gerth 8:54a7da09ccad 400 readbuttoncalibrate_go=false;
Gerth 7:7fbb2c028778 401 }
Gerth 8:54a7da09ccad 402 if (buttonR.read()==1 && buttonL.read()==0) {
Gerth 8:54a7da09ccad 403 desired_angle[1] -= (radpersec_calibrate/readbuttoncalibrate_frequency);
Gerth 8:54a7da09ccad 404 readbuttoncalibrate_go=false;
Gerth 7:7fbb2c028778 405 }
Gerth 6:37c94a5e205f 406 }
Gerth 3:48438eea184e 407 }
Gerth 0:dd66fff537d7 408 }
Gerth 8:54a7da09ccad 409 }