Peter Knoben / Mbed 2 deprecated Oudverslag

Oud verslag voor Biquad.

Dependencies:   Biquad HIDScope QEI angleandposition controlandadjust mbed

Fork of includeair by Jasper Gerth

main.cpp@11:c10651055871, 2015-10-19 (annotated)

Committer:
Gerth
Date:
Mon Oct 19 08:23:58 2015 +0000
Revision:
11:c10651055871
Parent:
10:9e96d14d7034
Child:
12:b9f0b92bd659
controllable with emg signals

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