Boudewijn van den Berg / Mbed 2 deprecated HuidigePositieV3

..

Dependencies:   MODSERIAL TextLCD mbed

main.cpp@0:58c11abe4785, 2013-11-03 (annotated)

Committer:
bouvdberg
Date:
Sun Nov 03 10:55:53 2013 +0000
Revision:
0:58c11abe4785
Versie 3

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bouvdberg 0:58c11abe4785 1 // Hello World! for the TextLCD*//
bouvdberg 0:58c11abe4785 2
bouvdberg 0:58c11abe4785 3 #include "mbed.h"
bouvdberg 0:58c11abe4785 4 #include "TextLCD.h"
bouvdberg 0:58c11abe4785 5 #include "MODSERIAL.h"
bouvdberg 0:58c11abe4785 6 #include "encoder.h"
bouvdberg 0:58c11abe4785 7
bouvdberg 0:58c11abe4785 8 // definieren constanten
bouvdberg 0:58c11abe4785 9 #define PI 3.1415926
bouvdberg 0:58c11abe4785 10 //plant
bouvdberg 0:58c11abe4785 11 #define ARM1 0.36
bouvdberg 0:58c11abe4785 12 #define ARM2 0.26
bouvdberg 0:58c11abe4785 13 //PD
bouvdberg 0:58c11abe4785 14 #define CI 0.012
bouvdberg 0:58c11abe4785 15 #define CP 0.001
bouvdberg 0:58c11abe4785 16 #define CD 0.02
bouvdberg 0:58c11abe4785 17 #define CLP1 0.9975
bouvdberg 0:58c11abe4785 18 #define CLP2 0.001
bouvdberg 0:58c11abe4785 19 //Snelheid
bouvdberg 0:58c11abe4785 20 #define SNELHEID 0.05
bouvdberg 0:58c11abe4785 21 //LOOPTIME
bouvdberg 0:58c11abe4785 22 #define LOOPTIME 0.006667
bouvdberg 0:58c11abe4785 23 //Filtering EMG
bouvdberg 0:58c11abe4785 24 #define HP1 0.905
bouvdberg 0:58c11abe4785 25 #define HP2 20.0
bouvdberg 0:58c11abe4785 26 #define HP3 20.0
bouvdberg 0:58c11abe4785 27 #define LP1 0.9753
bouvdberg 0:58c11abe4785 28 #define LP2 0.0247
bouvdberg 0:58c11abe4785 29 //EMG threshold
bouvdberg 0:58c11abe4785 30 #define SET_EMG_MAX1 3.0 //bovenarm rechts > beweging naar rechts
bouvdberg 0:58c11abe4785 31 #define SET_EMG_MIN1 1.5
bouvdberg 0:58c11abe4785 32 #define SET_EMG_MAX2 2.0 //bovenarm links > beweging naar links
bouvdberg 0:58c11abe4785 33 #define SET_EMG_MIN2 0.75
bouvdberg 0:58c11abe4785 34 #define SET_EMG_MAX3 3.5 //onderarm rechts > beweging naar boven
bouvdberg 0:58c11abe4785 35 #define SET_EMG_MIN3 1.5
bouvdberg 0:58c11abe4785 36 #define SET_EMG_MAX4 2.0 //onderarm links > beweging naar onder
bouvdberg 0:58c11abe4785 37 #define SET_EMG_MIN4 1.0
bouvdberg 0:58c11abe4785 38
bouvdberg 0:58c11abe4785 39 void aansturing(void);
bouvdberg 0:58c11abe4785 40 void uitzetten(void);
bouvdberg 0:58c11abe4785 41 void setlooptimerflag(void);
bouvdberg 0:58c11abe4785 42 void keep_in_range(float * in, float min, float max);
bouvdberg 0:58c11abe4785 43
bouvdberg 0:58c11abe4785 44 volatile bool looptimerflag;
bouvdberg 0:58c11abe4785 45
bouvdberg 0:58c11abe4785 46 Serial pc(USBTX, USBRX);
bouvdberg 0:58c11abe4785 47 TextLCD lcd(PTE5, PTE3, PTE2, PTB11, PTB10, PTB9, TextLCD::LCD16x2,NC,NC,TextLCD::HD44780); // rs, e, d4-d7-/*+-9
bouvdberg 0:58c11abe4785 48 AnalogIn EMG1(PTB0); //EMG
bouvdberg 0:58c11abe4785 49 AnalogIn EMG2(PTB1);
bouvdberg 0:58c11abe4785 50 AnalogIn EMG3(PTB2);
bouvdberg 0:58c11abe4785 51 AnalogIn EMG4(PTB3);
bouvdberg 0:58c11abe4785 52 AnalogIn potmeter(PTC2); //potmeter
bouvdberg 0:58c11abe4785 53 DigitalIn ButtonSTOP(PTE21); //Knopjes voor kalibratie
bouvdberg 0:58c11abe4785 54 DigitalIn ButtonSELECT(PTE20);
bouvdberg 0:58c11abe4785 55 DigitalIn ButtonUP(PTE23);
bouvdberg 0:58c11abe4785 56 DigitalIn ButtonDOWN(PTE22);
bouvdberg 0:58c11abe4785 57 DigitalOut Solenoid(PTD4); //Solenoid
bouvdberg 0:58c11abe4785 58 Encoder motor1(PTD0,PTC8); //Encoder
bouvdberg 0:58c11abe4785 59 Encoder motor2(PTD2,PTC9);
bouvdberg 0:58c11abe4785 60 PwmOut pwm_motor1(PTA12); //motor
bouvdberg 0:58c11abe4785 61 DigitalOut motordir1(PTD3);
bouvdberg 0:58c11abe4785 62 PwmOut pwm_motor2(PTA5);
bouvdberg 0:58c11abe4785 63 DigitalOut motordir2(PTD1);
bouvdberg 0:58c11abe4785 64
bouvdberg 0:58c11abe4785 65 float numberx = 9;
bouvdberg 0:58c11abe4785 66 int menu=0, t;
bouvdberg 0:58c11abe4785 67 float EMGmax1=SET_EMG_MAX1, EMGmin1=SET_EMG_MIN1, EMGmax2=SET_EMG_MAX2, EMGmin2=SET_EMG_MIN2;
bouvdberg 0:58c11abe4785 68 float EMGmax3=SET_EMG_MAX3, EMGmin3=SET_EMG_MIN3, EMGmax4=SET_EMG_MAX4, EMGmin4=SET_EMG_MIN4;
bouvdberg 0:58c11abe4785 69 float drawspeed=SNELHEID;
bouvdberg 0:58c11abe4785 70
bouvdberg 0:58c11abe4785 71
bouvdberg 0:58c11abe4785 72 //Variabelen verwerking EMG
bouvdberg 0:58c11abe4785 73 float emg_value1, emg_value2, emg_value3, emg_value4;
bouvdberg 0:58c11abe4785 74 float emg_value1min1=0.5, emg_value2min1=0.5, emg_value3min1=0.5, emg_value4min1=0.5;
bouvdberg 0:58c11abe4785 75 float EMGhp1, EMGhp2, EMGhp3, EMGhp4, EMGlp1, EMGlp2, EMGlp3, EMGlp4;
bouvdberg 0:58c11abe4785 76 float EMGhp1min1=0.5, EMGhp2min1=0.5, EMGhp3min1=0.5, EMGhp4min1=0.5, EMGlp1min1=0.5, EMGlp2min1=0.5, EMGlp3min1=0.5, EMGlp4min1=0.5;
bouvdberg 0:58c11abe4785 77
bouvdberg 0:58c11abe4785 78 //Variabelen bepaling input systeem
bouvdberg 0:58c11abe4785 79 float input;
bouvdberg 0:58c11abe4785 80 float w1, w2, wM2, phi1, phi2, theta;
bouvdberg 0:58c11abe4785 81 float a, b, c, d, ai, bi, ci, di;
bouvdberg 0:58c11abe4785 82 float v1, v2, v3, v4, vx, vy, snelheid;
bouvdberg 0:58c11abe4785 83 float M1position, M2position, M2phi;
bouvdberg 0:58c11abe4785 84 float Px, Py;
bouvdberg 0:58c11abe4785 85
bouvdberg 0:58c11abe4785 86 //Variabelen motoraansturing
bouvdberg 0:58c11abe4785 87 float setpointM1, setpointM2;
bouvdberg 0:58c11abe4785 88 float setpointmin1M1=800.0, setpointmin1M2=2400.0;
bouvdberg 0:58c11abe4785 89 float pwm_to_motor1, pwm_to_motor2;
bouvdberg 0:58c11abe4785 90 float foutM1, foutM2;
bouvdberg 0:58c11abe4785 91 float foutmin1M1=0.0, foutmin1M2=0.0;
bouvdberg 0:58c11abe4785 92 float foutverschilM1, foutverschilM2;
bouvdberg 0:58c11abe4785 93 float foutverschilmin1M1=0.0, foutverschilmin1M2=0.0;
bouvdberg 0:58c11abe4785 94 float foutImin1=0.0, foutImin2=0.0, foutI1, foutI2;
bouvdberg 0:58c11abe4785 95 float CDloop=CD/LOOPTIME;
bouvdberg 0:58c11abe4785 96 float t_sin=0.0;
bouvdberg 0:58c11abe4785 97 float t_timer=0.0;
bouvdberg 0:58c11abe4785 98
bouvdberg 0:58c11abe4785 99 int main() {
bouvdberg 0:58c11abe4785 100 //set buttons PULLDOWN
bouvdberg 0:58c11abe4785 101 ButtonSTOP.mode(PullNone);
bouvdberg 0:58c11abe4785 102 ButtonSELECT.mode(PullNone);
bouvdberg 0:58c11abe4785 103 ButtonUP.mode(PullNone);
bouvdberg 0:58c11abe4785 104 ButtonDOWN.mode(PullNone);
bouvdberg 0:58c11abe4785 105 pc.baud(57600);
bouvdberg 0:58c11abe4785 106 //Aanstuur timing
bouvdberg 0:58c11abe4785 107 Ticker looptimer;
bouvdberg 0:58c11abe4785 108 looptimer.attach(setlooptimerflag,LOOPTIME);
bouvdberg 0:58c11abe4785 109 while(1)
bouvdberg 0:58c11abe4785 110 {
bouvdberg 0:58c11abe4785 111 switch (menu)
bouvdberg 0:58c11abe4785 112 {
bouvdberg 0:58c11abe4785 113 case 0:
bouvdberg 0:58c11abe4785 114 lcd.cls();
bouvdberg 0:58c11abe4785 115 lcd.printf("> DRAW");
bouvdberg 0:58c11abe4785 116 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 117 lcd.printf(" SETTINGS");
bouvdberg 0:58c11abe4785 118 while(menu==0)
bouvdberg 0:58c11abe4785 119 {
bouvdberg 0:58c11abe4785 120 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:58c11abe4785 121 if (ButtonSELECT.read()==1)
bouvdberg 0:58c11abe4785 122 {
bouvdberg 0:58c11abe4785 123 motor1.setPosition(800);
bouvdberg 0:58c11abe4785 124 motor2.setPosition(2400);
bouvdberg 0:58c11abe4785 125 menu=55;
bouvdberg 0:58c11abe4785 126 lcd.cls();
bouvdberg 0:58c11abe4785 127 lcd.printf(" Drawing ...");
bouvdberg 0:58c11abe4785 128 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 129 lcd.printf("UP: Pause");
bouvdberg 0:58c11abe4785 130 }
bouvdberg 0:58c11abe4785 131 }
bouvdberg 0:58c11abe4785 132 break;
bouvdberg 0:58c11abe4785 133 case 1:
bouvdberg 0:58c11abe4785 134 lcd.cls();
bouvdberg 0:58c11abe4785 135 lcd.printf("> SETTINGS");
bouvdberg 0:58c11abe4785 136 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 137 lcd.printf(" RESET ALL");
bouvdberg 0:58c11abe4785 138 while(menu==1)
bouvdberg 0:58c11abe4785 139 {
bouvdberg 0:58c11abe4785 140 if (ButtonUP.read()==1) menu--;
bouvdberg 0:58c11abe4785 141 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:58c11abe4785 142 if (ButtonSELECT.read()==1) menu=20;
bouvdberg 0:58c11abe4785 143 }
bouvdberg 0:58c11abe4785 144 break;
bouvdberg 0:58c11abe4785 145 case 2:
bouvdberg 0:58c11abe4785 146 lcd.cls();
bouvdberg 0:58c11abe4785 147 lcd.printf("> RESET ALL");
bouvdberg 0:58c11abe4785 148 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 149 lcd.printf(" ");
bouvdberg 0:58c11abe4785 150 while(menu==2)
bouvdberg 0:58c11abe4785 151 {
bouvdberg 0:58c11abe4785 152 if (ButtonUP.read()==1) menu--;
bouvdberg 0:58c11abe4785 153 if (ButtonSELECT.read()==1)
bouvdberg 0:58c11abe4785 154 {
bouvdberg 0:58c11abe4785 155 EMGmax1=SET_EMG_MAX1; EMGmin1=SET_EMG_MIN1;
bouvdberg 0:58c11abe4785 156 EMGmax2=SET_EMG_MAX2; EMGmin2=SET_EMG_MIN2;
bouvdberg 0:58c11abe4785 157 EMGmax3=SET_EMG_MAX3; EMGmin3=SET_EMG_MIN3;
bouvdberg 0:58c11abe4785 158 EMGmax4=SET_EMG_MAX4; EMGmin4=SET_EMG_MIN4;
bouvdberg 0:58c11abe4785 159 drawspeed=SNELHEID;
bouvdberg 0:58c11abe4785 160 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 161 lcd.printf(" Reset Completed");
bouvdberg 0:58c11abe4785 162 wait(1);
bouvdberg 0:58c11abe4785 163 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 164 lcd.printf(" ");
bouvdberg 0:58c11abe4785 165 }
bouvdberg 0:58c11abe4785 166 }
bouvdberg 0:58c11abe4785 167 break;
bouvdberg 0:58c11abe4785 168 case 20:
bouvdberg 0:58c11abe4785 169 lcd.cls();
bouvdberg 0:58c11abe4785 170 lcd.printf("> EMG1-MAX: %.2f", (EMGmax1+ (((potmeter.read()+0.0005)*2)-1)));
bouvdberg 0:58c11abe4785 171 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 172 lcd.printf(" EMG1-MIN: ");
bouvdberg 0:58c11abe4785 173 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:58c11abe4785 174 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:58c11abe4785 175 if (ButtonSELECT.read()==1)
bouvdberg 0:58c11abe4785 176 {
bouvdberg 0:58c11abe4785 177 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 178 lcd.printf(" SAVED! ");
bouvdberg 0:58c11abe4785 179 EMGmax1=(EMGmax1+(((potmeter.read()+0.0005)*2)-1));
bouvdberg 0:58c11abe4785 180 wait(0.5);
bouvdberg 0:58c11abe4785 181 }
bouvdberg 0:58c11abe4785 182 break;
bouvdberg 0:58c11abe4785 183 case 21:
bouvdberg 0:58c11abe4785 184 lcd.cls();
bouvdberg 0:58c11abe4785 185 lcd.printf("> EMG1-MIN: %.2f", (EMGmin1+ (((potmeter.read()+0.0005)*2)-1)));
bouvdberg 0:58c11abe4785 186 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 187 lcd.printf(" EMG2-MAX: ");
bouvdberg 0:58c11abe4785 188 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:58c11abe4785 189 if (ButtonUP.read()==1) menu--;
bouvdberg 0:58c11abe4785 190 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:58c11abe4785 191 if (ButtonSELECT.read()==1)
bouvdberg 0:58c11abe4785 192 {
bouvdberg 0:58c11abe4785 193 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 194 lcd.printf(" SAVED! ");
bouvdberg 0:58c11abe4785 195 EMGmin1=(EMGmin1+ (((potmeter.read()+0.0005)*2)-1));
bouvdberg 0:58c11abe4785 196 wait(0.5);
bouvdberg 0:58c11abe4785 197 }
bouvdberg 0:58c11abe4785 198 break;
bouvdberg 0:58c11abe4785 199 case 22:
bouvdberg 0:58c11abe4785 200 lcd.cls();
bouvdberg 0:58c11abe4785 201 lcd.printf("> EMG2-MAX: %.2f", (EMGmax1+ (((potmeter.read()+0.0005)*2)-1)));
bouvdberg 0:58c11abe4785 202 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 203 lcd.printf(" EMG2-MIN: ");
bouvdberg 0:58c11abe4785 204 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:58c11abe4785 205 if (ButtonUP.read()==1) menu--;
bouvdberg 0:58c11abe4785 206 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:58c11abe4785 207 if (ButtonSELECT.read()==1)
bouvdberg 0:58c11abe4785 208 {
bouvdberg 0:58c11abe4785 209 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 210 lcd.printf(" SAVED! ");
bouvdberg 0:58c11abe4785 211 EMGmax2=(EMGmax1+ (((potmeter.read()+0.0005)*2)-1));
bouvdberg 0:58c11abe4785 212 wait(0.5);
bouvdberg 0:58c11abe4785 213 }
bouvdberg 0:58c11abe4785 214 break;
bouvdberg 0:58c11abe4785 215 case 23:
bouvdberg 0:58c11abe4785 216 lcd.cls();
bouvdberg 0:58c11abe4785 217 lcd.printf("> EMG2-MIN: %.2f", (EMGmin2+ (((potmeter.read()+0.0005)*2)-1)));
bouvdberg 0:58c11abe4785 218 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 219 lcd.printf(" EMG3-MAX: ");
bouvdberg 0:58c11abe4785 220 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:58c11abe4785 221 if (ButtonUP.read()==1) menu--;
bouvdberg 0:58c11abe4785 222 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:58c11abe4785 223 if (ButtonSELECT.read()==1)
bouvdberg 0:58c11abe4785 224 {
bouvdberg 0:58c11abe4785 225 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 226 lcd.printf(" SAVED! ");
bouvdberg 0:58c11abe4785 227 EMGmin2=(EMGmin2+ (((potmeter.read()+0.0005)*2)-1));
bouvdberg 0:58c11abe4785 228 wait(0.5);
bouvdberg 0:58c11abe4785 229 }
bouvdberg 0:58c11abe4785 230 break;
bouvdberg 0:58c11abe4785 231 case 24:
bouvdberg 0:58c11abe4785 232 lcd.cls();
bouvdberg 0:58c11abe4785 233 lcd.printf("> EMG3-MAX: %.2f", (EMGmax3+ (((potmeter.read()+0.0005)*2)-1)));
bouvdberg 0:58c11abe4785 234 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 235 lcd.printf(" EMG3-MIN: ");
bouvdberg 0:58c11abe4785 236 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:58c11abe4785 237 if (ButtonUP.read()==1) menu--;
bouvdberg 0:58c11abe4785 238 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:58c11abe4785 239 if (ButtonSELECT.read()==1)
bouvdberg 0:58c11abe4785 240 {
bouvdberg 0:58c11abe4785 241 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 242 lcd.printf(" SAVED! ");
bouvdberg 0:58c11abe4785 243 EMGmax3=(EMGmax3+ (((potmeter.read()+0.0005)*2)-1));
bouvdberg 0:58c11abe4785 244 wait(0.5);
bouvdberg 0:58c11abe4785 245 }
bouvdberg 0:58c11abe4785 246 break;
bouvdberg 0:58c11abe4785 247 case 25:
bouvdberg 0:58c11abe4785 248 lcd.cls();
bouvdberg 0:58c11abe4785 249 lcd.printf("> EMG3-MIN: %.2f", (EMGmin3+ (((potmeter.read()+0.0005)*2)-1)));
bouvdberg 0:58c11abe4785 250 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 251 lcd.printf(" EMG4-MAX: ");
bouvdberg 0:58c11abe4785 252 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:58c11abe4785 253 if (ButtonUP.read()==1) menu--;
bouvdberg 0:58c11abe4785 254 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:58c11abe4785 255 if (ButtonSELECT.read()==1)
bouvdberg 0:58c11abe4785 256 {
bouvdberg 0:58c11abe4785 257 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 258 lcd.printf(" SAVED! ");
bouvdberg 0:58c11abe4785 259 EMGmin3=(EMGmin3+ (((potmeter.read()+0.0005)*2)-1));
bouvdberg 0:58c11abe4785 260 wait(0.5);
bouvdberg 0:58c11abe4785 261 }
bouvdberg 0:58c11abe4785 262 break;
bouvdberg 0:58c11abe4785 263 case 26:
bouvdberg 0:58c11abe4785 264 lcd.cls();
bouvdberg 0:58c11abe4785 265 lcd.printf("> EMG4-MAX: %.2f", (EMGmax4+ (((potmeter.read()+0.0005)*2)-1)));
bouvdberg 0:58c11abe4785 266 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 267 lcd.printf(" EMG4-MIN: ");
bouvdberg 0:58c11abe4785 268 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:58c11abe4785 269 if (ButtonUP.read()==1) menu--;
bouvdberg 0:58c11abe4785 270 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:58c11abe4785 271 if (ButtonSELECT.read()==1)
bouvdberg 0:58c11abe4785 272 {
bouvdberg 0:58c11abe4785 273 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 274 lcd.printf(" SAVED! ");
bouvdberg 0:58c11abe4785 275 EMGmax4=(EMGmax4+ (((potmeter.read()+0.0005)*2)-1));
bouvdberg 0:58c11abe4785 276 wait(0.5);
bouvdberg 0:58c11abe4785 277 }
bouvdberg 0:58c11abe4785 278 break;
bouvdberg 0:58c11abe4785 279 case 27:
bouvdberg 0:58c11abe4785 280 lcd.cls();
bouvdberg 0:58c11abe4785 281 lcd.printf("> EMG4-MIN: %.2f", (EMGmin4+ (((potmeter.read()+0.0005)*2)-1)));
bouvdberg 0:58c11abe4785 282 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 283 lcd.printf(" SPEED :");
bouvdberg 0:58c11abe4785 284 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:58c11abe4785 285 if (ButtonUP.read()==1) menu--;
bouvdberg 0:58c11abe4785 286 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:58c11abe4785 287 if (ButtonSELECT.read()==1)
bouvdberg 0:58c11abe4785 288 {
bouvdberg 0:58c11abe4785 289 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 290 lcd.printf(" SAVED! ");
bouvdberg 0:58c11abe4785 291 EMGmin4=(EMGmin4+ (((potmeter.read()+0.0005)*2)-1));
bouvdberg 0:58c11abe4785 292 wait(0.5);
bouvdberg 0:58c11abe4785 293 }
bouvdberg 0:58c11abe4785 294 break;
bouvdberg 0:58c11abe4785 295 case 28:
bouvdberg 0:58c11abe4785 296 lcd.cls();
bouvdberg 0:58c11abe4785 297 lcd.printf("> SPEED : %.2f", (drawspeed+ (((potmeter.read()+0.0005)/10)-0.05)));
bouvdberg 0:58c11abe4785 298 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 299 lcd.printf(" SOLENOID:");
bouvdberg 0:58c11abe4785 300 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:58c11abe4785 301 if (ButtonUP.read()==1) menu--;
bouvdberg 0:58c11abe4785 302 if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:58c11abe4785 303 if (ButtonSELECT.read()==1)
bouvdberg 0:58c11abe4785 304 {
bouvdberg 0:58c11abe4785 305 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 306 lcd.printf(" SAVED! ");
bouvdberg 0:58c11abe4785 307 drawspeed=(drawspeed+ (((potmeter.read()+0.0005)/10)-0.05));
bouvdberg 0:58c11abe4785 308 wait(0.5);
bouvdberg 0:58c11abe4785 309 }
bouvdberg 0:58c11abe4785 310 break;
bouvdberg 0:58c11abe4785 311 case 29:
bouvdberg 0:58c11abe4785 312 lcd.cls();
bouvdberg 0:58c11abe4785 313 lcd.printf("> SOLENOID: OFF");
bouvdberg 0:58c11abe4785 314 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 315 lcd.printf(" ");
bouvdberg 0:58c11abe4785 316 if (ButtonSTOP.read()==1) menu=0;
bouvdberg 0:58c11abe4785 317 if (ButtonUP.read()==1) menu--;
bouvdberg 0:58c11abe4785 318 //if (ButtonDOWN.read()==1) menu++;
bouvdberg 0:58c11abe4785 319 if (ButtonSELECT.read()==1)
bouvdberg 0:58c11abe4785 320 {
bouvdberg 0:58c11abe4785 321 lcd.cls();
bouvdberg 0:58c11abe4785 322 lcd.printf("> SOLENOID: ON");
bouvdberg 0:58c11abe4785 323 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 324 lcd.printf(" ");
bouvdberg 0:58c11abe4785 325 Solenoid=1;
bouvdberg 0:58c11abe4785 326 wait(1);
bouvdberg 0:58c11abe4785 327 Solenoid=0;
bouvdberg 0:58c11abe4785 328 }
bouvdberg 0:58c11abe4785 329 break;
bouvdberg 0:58c11abe4785 330 case 50: //tekenen afsluiten
bouvdberg 0:58c11abe4785 331 lcd.cls();
bouvdberg 0:58c11abe4785 332 lcd.printf(" Shutting Down!");
bouvdberg 0:58c11abe4785 333 menu=0;
bouvdberg 0:58c11abe4785 334
bouvdberg 0:58c11abe4785 335 uitzetten();
bouvdberg 0:58c11abe4785 336
bouvdberg 0:58c11abe4785 337 break;
bouvdberg 0:58c11abe4785 338 case 55:
bouvdberg 0:58c11abe4785 339 if (ButtonSTOP.read()==1) menu=50;
bouvdberg 0:58c11abe4785 340 if (ButtonUP.read()==1) menu++;
bouvdberg 0:58c11abe4785 341
bouvdberg 0:58c11abe4785 342
bouvdberg 0:58c11abe4785 343 aansturing(); //aansturing
bouvdberg 0:58c11abe4785 344
bouvdberg 0:58c11abe4785 345
bouvdberg 0:58c11abe4785 346 break;
bouvdberg 0:58c11abe4785 347 case 56:
bouvdberg 0:58c11abe4785 348 wait(0.2);
bouvdberg 0:58c11abe4785 349 lcd.cls();
bouvdberg 0:58c11abe4785 350 lcd.printf(" PAUSE ");
bouvdberg 0:58c11abe4785 351 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 352 lcd.printf("> RESUME");
bouvdberg 0:58c11abe4785 353
bouvdberg 0:58c11abe4785 354 Solenoid=1;
bouvdberg 0:58c11abe4785 355 pwm_motor1.write(0);
bouvdberg 0:58c11abe4785 356 pwm_motor2.write(0);
bouvdberg 0:58c11abe4785 357
bouvdberg 0:58c11abe4785 358 while(ButtonSELECT.read()==1);
bouvdberg 0:58c11abe4785 359 while(menu==56)
bouvdberg 0:58c11abe4785 360 {
bouvdberg 0:58c11abe4785 361 if (ButtonSTOP.read()==1) menu=50;
bouvdberg 0:58c11abe4785 362 if (ButtonSELECT.read()==1)
bouvdberg 0:58c11abe4785 363 {
bouvdberg 0:58c11abe4785 364 menu--;
bouvdberg 0:58c11abe4785 365 lcd.cls();
bouvdberg 0:58c11abe4785 366 lcd.printf(" Drawing ...");
bouvdberg 0:58c11abe4785 367 lcd.locate(0,1);
bouvdberg 0:58c11abe4785 368 lcd.printf("UP: Pause");
bouvdberg 0:58c11abe4785 369 }
bouvdberg 0:58c11abe4785 370 }
bouvdberg 0:58c11abe4785 371
bouvdberg 0:58c11abe4785 372
bouvdberg 0:58c11abe4785 373
bouvdberg 0:58c11abe4785 374 break;
bouvdberg 0:58c11abe4785 375 }
bouvdberg 0:58c11abe4785 376
bouvdberg 0:58c11abe4785 377 if (menu!=55) wait(0.2);
bouvdberg 0:58c11abe4785 378 }
bouvdberg 0:58c11abe4785 379
bouvdberg 0:58c11abe4785 380
bouvdberg 0:58c11abe4785 381 }
bouvdberg 0:58c11abe4785 382
bouvdberg 0:58c11abe4785 383 void aansturing(void)
bouvdberg 0:58c11abe4785 384 {
bouvdberg 0:58c11abe4785 385 while(looptimerflag != true);
bouvdberg 0:58c11abe4785 386 looptimerflag = false;
bouvdberg 0:58c11abe4785 387
bouvdberg 0:58c11abe4785 388 //uitlezen
bouvdberg 0:58c11abe4785 389 emg_value1 = EMG1.read();
bouvdberg 0:58c11abe4785 390 emg_value2 = EMG2.read();
bouvdberg 0:58c11abe4785 391 emg_value3 = EMG3.read();
bouvdberg 0:58c11abe4785 392 emg_value4 = EMG4.read();
bouvdberg 0:58c11abe4785 393
bouvdberg 0:58c11abe4785 394 //filtering
bouvdberg 0:58c11abe4785 395 EMGhp1=HP1*EMGhp1min1+HP2*emg_value1-HP3*emg_value1min1;
bouvdberg 0:58c11abe4785 396 EMGhp2=HP1*EMGhp2min1+HP2*emg_value2-HP3*emg_value2min1;
bouvdberg 0:58c11abe4785 397 EMGhp3=HP1*EMGhp3min1+HP2*emg_value3-HP3*emg_value3min1;
bouvdberg 0:58c11abe4785 398 EMGhp4=HP1*EMGhp4min1+HP2*emg_value4-HP3*emg_value4min1;
bouvdberg 0:58c11abe4785 399 EMGhp1=abs(EMGhp1);
bouvdberg 0:58c11abe4785 400 EMGhp2=abs(EMGhp2);
bouvdberg 0:58c11abe4785 401 EMGhp3=abs(EMGhp3);
bouvdberg 0:58c11abe4785 402 EMGhp4=abs(EMGhp4);
bouvdberg 0:58c11abe4785 403 EMGlp1=LP1*EMGlp1min1+LP2*EMGhp1min1;
bouvdberg 0:58c11abe4785 404 EMGlp2=LP1*EMGlp2min1+LP2*EMGhp2min1;
bouvdberg 0:58c11abe4785 405 EMGlp3=LP1*EMGlp3min1+LP2*EMGhp3min1;
bouvdberg 0:58c11abe4785 406 EMGlp4=LP1*EMGlp4min1+LP2*EMGhp4min1;
bouvdberg 0:58c11abe4785 407 //pc.printf("%.2f ",emg_value1);
bouvdberg 0:58c11abe4785 408 //pc.printf("%.2f ",emg_value2);
bouvdberg 0:58c11abe4785 409 //pc.printf("%.2f ",emg_value3);
bouvdberg 0:58c11abe4785 410 //pc.printf("%.2f ",emg_value4);
bouvdberg 0:58c11abe4785 411 //pc.printf("%.2f ",EMGlp1);
bouvdberg 0:58c11abe4785 412 //pc.printf("%.2f ",EMGlp2);
bouvdberg 0:58c11abe4785 413 //pc.printf("%.2f ",EMGlp3);
bouvdberg 0:58c11abe4785 414 //pc.printf("%.2f ",EMGlp4);
bouvdberg 0:58c11abe4785 415
bouvdberg 0:58c11abe4785 416 //berekenen setpoint
bouvdberg 0:58c11abe4785 417 //hoekinput
bouvdberg 0:58c11abe4785 418
bouvdberg 0:58c11abe4785 419 if((EMGlp1-EMGmin1)<=0.0) v1=0.0;
bouvdberg 0:58c11abe4785 420 else v1=(EMGlp1-EMGmin1)/(EMGmax1-EMGmin1);
bouvdberg 0:58c11abe4785 421 if((EMGlp2-EMGmin2)<=0.0) v2=0.0;
bouvdberg 0:58c11abe4785 422 else v2=(EMGlp2-EMGmin2)/(EMGmax2-EMGmin2);
bouvdberg 0:58c11abe4785 423 if((EMGlp3-EMGmin3)<=0.0) v3=0.0;
bouvdberg 0:58c11abe4785 424 else v3=(EMGlp3-EMGmin3)/(EMGmax3-EMGmin3);
bouvdberg 0:58c11abe4785 425 if((EMGlp4-EMGmin4)<=0.0) v4=0.0;
bouvdberg 0:58c11abe4785 426 else v4=(EMGlp4-EMGmin4)/(EMGmax4-EMGmin4);
bouvdberg 0:58c11abe4785 427
bouvdberg 0:58c11abe4785 428 t_timer=t_timer+LOOPTIME;
bouvdberg 0:58c11abe4785 429
bouvdberg 0:58c11abe4785 430 pc.printf("%.2f ",v1);
bouvdberg 0:58c11abe4785 431 pc.printf("%.2f ",v2);
bouvdberg 0:58c11abe4785 432 pc.printf("%.2f ",v3);
bouvdberg 0:58c11abe4785 433 pc.printf("%.2f ",v4);
bouvdberg 0:58c11abe4785 434 if(v1<=0.1 && v2<=0.1 && v3<=0.1 && v4<=0.1) {
bouvdberg 0:58c11abe4785 435 Solenoid=1; //Pen van papier
bouvdberg 0:58c11abe4785 436 input=0.0;
bouvdberg 0:58c11abe4785 437 snelheid=0.0;
bouvdberg 0:58c11abe4785 438 }
bouvdberg 0:58c11abe4785 439 else {
bouvdberg 0:58c11abe4785 440 Solenoid=0; //Pen op papier
bouvdberg 0:58c11abe4785 441 snelheid=drawspeed;
bouvdberg 0:58c11abe4785 442 if(v2>v1) {
bouvdberg 0:58c11abe4785 443 input=(atan((v3-v4)/(v1-v2))+PI);
bouvdberg 0:58c11abe4785 444 }
bouvdberg 0:58c11abe4785 445 else {
bouvdberg 0:58c11abe4785 446 input=(atan((v3-v4)/(v1-v2)));
bouvdberg 0:58c11abe4785 447 }
bouvdberg 0:58c11abe4785 448 }
bouvdberg 0:58c11abe4785 449 pc.printf("%.2f \n\r",input);
bouvdberg 0:58c11abe4785 450 //snelheid=drawspeed;
bouvdberg 0:58c11abe4785 451 //input=t_timer*0.8+PI;
bouvdberg 0:58c11abe4785 452
bouvdberg 0:58c11abe4785 453 //snelheidsvector met beperking positie / encoder uitlezen
bouvdberg 0:58c11abe4785 454 M1position = motor1.getPosition();
bouvdberg 0:58c11abe4785 455 M2position = motor2.getPosition();
bouvdberg 0:58c11abe4785 456 //M2phi=M1position-M2position+1600.0; //phi2 = phi1 - theta + 1600
bouvdberg 0:58c11abe4785 457
bouvdberg 0:58c11abe4785 458 //Px=cos((M1position/3200.0)*2.0*PI)*ARM1+cos((M2position/3200.0)*2.0*PI)*ARM2;
bouvdberg 0:58c11abe4785 459 //Py=sin((M1position/3200.0)*2.0*PI)*ARM1+sin((M2position/3200.0)*2.0*PI)*ARM2;
bouvdberg 0:58c11abe4785 460
bouvdberg 0:58c11abe4785 461 //if(Px > 0.29 || Px < 0.125) vx=0;
bouvdberg 0:58c11abe4785 462 //else
bouvdberg 0:58c11abe4785 463 vx=cos(input)*snelheid;
bouvdberg 0:58c11abe4785 464 //if(Py < -0.425 || Py > -0.125) vy=0;
bouvdberg 0:58c11abe4785 465 //else
bouvdberg 0:58c11abe4785 466 vy=sin(input)*snelheid;
bouvdberg 0:58c11abe4785 467
bouvdberg 0:58c11abe4785 468 //input positie
bouvdberg 0:58c11abe4785 469 phi1=(motor1.getPosition()/3200.0)*2.0*PI;
bouvdberg 0:58c11abe4785 470 theta=(motor2.getPosition()/3200.0)*2.0*PI;
bouvdberg 0:58c11abe4785 471 phi2=theta+phi1-PI;
bouvdberg 0:58c11abe4785 472
bouvdberg 0:58c11abe4785 473 //Jacobiaan
bouvdberg 0:58c11abe4785 474 // [a b]
bouvdberg 0:58c11abe4785 475 // [c d]
bouvdberg 0:58c11abe4785 476 a=-sin(phi1)*ARM1;
bouvdberg 0:58c11abe4785 477 b=-sin(phi2)*ARM2;
bouvdberg 0:58c11abe4785 478 c=cos(phi1)*ARM1;
bouvdberg 0:58c11abe4785 479 d=cos(phi2)*ARM2;
bouvdberg 0:58c11abe4785 480
bouvdberg 0:58c11abe4785 481 //inverse
bouvdberg 0:58c11abe4785 482 // [ai bi]
bouvdberg 0:58c11abe4785 483 // [ci di]
bouvdberg 0:58c11abe4785 484 ai=d/(a*d-b*c);
bouvdberg 0:58c11abe4785 485 bi=-b/(a*d-b*c);
bouvdberg 0:58c11abe4785 486 ci=-c/(a*d-b*c);
bouvdberg 0:58c11abe4785 487 di=a/(a*d-b*c);
bouvdberg 0:58c11abe4785 488
bouvdberg 0:58c11abe4785 489 //vermenigvuldiging
bouvdberg 0:58c11abe4785 490 // [ai bi] [vx] [w1]
bouvdberg 0:58c11abe4785 491 // [ci di] * [vy] = [w2]
bouvdberg 0:58c11abe4785 492 w1=ai*vx+bi*vy; //=wM1 hoeksnelheid van motor 1
bouvdberg 0:58c11abe4785 493 w2=ci*vx+di*vy;
bouvdberg 0:58c11abe4785 494 wM2=w2-w1;//hoeksnelheid motor 2
bouvdberg 0:58c11abe4785 495
bouvdberg 0:58c11abe4785 496 //Beveiliging hoeksnelheden
bouvdberg 0:58c11abe4785 497 keep_in_range(&w1, -1000,1000);
bouvdberg 0:58c11abe4785 498 keep_in_range(&wM2, -1000,1000);
bouvdberg 0:58c11abe4785 499
bouvdberg 0:58c11abe4785 500 //Motoraansturing
bouvdberg 0:58c11abe4785 501 //t_sin=t_sin + 0.05;
bouvdberg 0:58c11abe4785 502 //if (t_sin>=2*PI) t_sin=0.0;
bouvdberg 0:58c11abe4785 503 setpointM1 = (w1/(2.0*PI))*3200.0*LOOPTIME+setpointmin1M1; //sin(t_sin)*1600;
bouvdberg 0:58c11abe4785 504 setpointM2 = (wM2/(2.0*PI))*3200.0*LOOPTIME+setpointmin1M2;
bouvdberg 0:58c11abe4785 505
bouvdberg 0:58c11abe4785 506 //Beperking hoeken
bouvdberg 0:58c11abe4785 507 keep_in_range(&setpointM1, 0,1600);//Heel rondje = 3200 pulsen, Half rondje = 1600 pulsen
bouvdberg 0:58c11abe4785 508 keep_in_range(&setpointM2, 1600,3050);// Begrensd op 20 graden minimaal, werkelijke minimale waarde is 15 graden
bouvdberg 0:58c11abe4785 509
bouvdberg 0:58c11abe4785 510 foutM1 = setpointM1-M1position;
bouvdberg 0:58c11abe4785 511 foutM2 = setpointM2-M2position;
bouvdberg 0:58c11abe4785 512 foutI1 = foutImin1 + foutM1*LOOPTIME;
bouvdberg 0:58c11abe4785 513 foutI2 = foutImin2 + foutM2*LOOPTIME;
bouvdberg 0:58c11abe4785 514 foutverschilM1 = foutM1-foutmin1M1;
bouvdberg 0:58c11abe4785 515 foutverschilM2 = foutM2-foutmin1M2;
bouvdberg 0:58c11abe4785 516 foutverschilM1 = CLP1*foutverschilmin1M1+CLP2*foutverschilM1;
bouvdberg 0:58c11abe4785 517 foutverschilM2 = CLP1*foutverschilmin1M2+CLP2*foutverschilM2;
bouvdberg 0:58c11abe4785 518 pwm_to_motor1 = foutM1*CP+foutverschilM1*CDloop+foutI1*CI;
bouvdberg 0:58c11abe4785 519 pwm_to_motor2 = foutM2*CP+foutverschilM2*CDloop+foutI2*CI;//foutM2*CP+foutverschilM2*CDloop;
bouvdberg 0:58c11abe4785 520 keep_in_range(&pwm_to_motor1, -0.2,0.2);
bouvdberg 0:58c11abe4785 521 keep_in_range(&pwm_to_motor2, -0.2,0.2);
bouvdberg 0:58c11abe4785 522
bouvdberg 0:58c11abe4785 523
bouvdberg 0:58c11abe4785 524 if(pwm_to_motor1 > 0) {
bouvdberg 0:58c11abe4785 525 motordir1 = 1;
bouvdberg 0:58c11abe4785 526 pwm_to_motor1=pwm_to_motor1+0.08;
bouvdberg 0:58c11abe4785 527 }
bouvdberg 0:58c11abe4785 528 else {
bouvdberg 0:58c11abe4785 529 motordir1 = 0;
bouvdberg 0:58c11abe4785 530 pwm_to_motor1=pwm_to_motor1-0.08;
bouvdberg 0:58c11abe4785 531 }
bouvdberg 0:58c11abe4785 532 if(pwm_to_motor2 > 0) {
bouvdberg 0:58c11abe4785 533 motordir2 = 1;
bouvdberg 0:58c11abe4785 534 pwm_to_motor2=pwm_to_motor2+0.08;
bouvdberg 0:58c11abe4785 535 }
bouvdberg 0:58c11abe4785 536 else {
bouvdberg 0:58c11abe4785 537 motordir2 = 0;
bouvdberg 0:58c11abe4785 538 pwm_to_motor2=pwm_to_motor2-0.08;
bouvdberg 0:58c11abe4785 539 }
bouvdberg 0:58c11abe4785 540
bouvdberg 0:58c11abe4785 541
bouvdberg 0:58c11abe4785 542 //WRITE VALUE TO MOTOR
bouvdberg 0:58c11abe4785 543 pwm_motor1.write(abs(pwm_to_motor1));
bouvdberg 0:58c11abe4785 544 pwm_motor2.write(abs(pwm_to_motor2));
bouvdberg 0:58c11abe4785 545
bouvdberg 0:58c11abe4785 546 //Definieren waarden in de verleden tijd
bouvdberg 0:58c11abe4785 547 foutmin1M1=foutM1;
bouvdberg 0:58c11abe4785 548 foutmin1M2=foutM2;
bouvdberg 0:58c11abe4785 549 foutverschilmin1M1=foutverschilM1;
bouvdberg 0:58c11abe4785 550 foutverschilmin1M2=foutverschilM2;
bouvdberg 0:58c11abe4785 551 foutImin1=foutI1;
bouvdberg 0:58c11abe4785 552 foutImin2=foutI2;
bouvdberg 0:58c11abe4785 553 setpointmin1M1=setpointM1;
bouvdberg 0:58c11abe4785 554 setpointmin1M2=setpointM2;
bouvdberg 0:58c11abe4785 555 emg_value1min1=emg_value1;
bouvdberg 0:58c11abe4785 556 emg_value2min1=emg_value2;
bouvdberg 0:58c11abe4785 557 emg_value3min1=emg_value3;
bouvdberg 0:58c11abe4785 558 emg_value4min1=emg_value4;
bouvdberg 0:58c11abe4785 559 EMGhp1min1=EMGhp1;
bouvdberg 0:58c11abe4785 560 EMGhp2min1=EMGhp2;
bouvdberg 0:58c11abe4785 561 EMGhp3min1=EMGhp3;
bouvdberg 0:58c11abe4785 562 EMGhp4min1=EMGhp4;
bouvdberg 0:58c11abe4785 563 EMGlp1min1=EMGlp1;
bouvdberg 0:58c11abe4785 564 EMGlp2min1=EMGlp2;
bouvdberg 0:58c11abe4785 565 EMGlp3min1=EMGlp3;
bouvdberg 0:58c11abe4785 566 EMGlp4min1=EMGlp4;
bouvdberg 0:58c11abe4785 567
bouvdberg 0:58c11abe4785 568
bouvdberg 0:58c11abe4785 569 }
bouvdberg 0:58c11abe4785 570 void uitzetten(void)
bouvdberg 0:58c11abe4785 571 {
bouvdberg 0:58c11abe4785 572 float BeginM1 = 800;
bouvdberg 0:58c11abe4785 573 float BeginM2 = 2400;
bouvdberg 0:58c11abe4785 574 Solenoid=1;
bouvdberg 0:58c11abe4785 575 while(BeginM1 - motor1.getPosition() >= 10 || BeginM1 - motor1.getPosition() <= -10 || BeginM2 - motor2.getPosition() >= 10 || BeginM2 - motor2.getPosition() <= -10)
bouvdberg 0:58c11abe4785 576 {
bouvdberg 0:58c11abe4785 577 Ticker looptimer;
bouvdberg 0:58c11abe4785 578 looptimer.attach(setlooptimerflag,LOOPTIME);
bouvdberg 0:58c11abe4785 579 while(looptimerflag != true);
bouvdberg 0:58c11abe4785 580 looptimerflag = false;
bouvdberg 0:58c11abe4785 581 M1position=motor1.getPosition();
bouvdberg 0:58c11abe4785 582 M2position=motor2.getPosition();
bouvdberg 0:58c11abe4785 583 pc.printf(" %f ",M2position);
bouvdberg 0:58c11abe4785 584 pwm_to_motor1 = (BeginM1 - M1position)*.002;
bouvdberg 0:58c11abe4785 585 pwm_to_motor2 = (BeginM2 - M2position)*.002;
bouvdberg 0:58c11abe4785 586 keep_in_range(&pwm_to_motor1, -0.05,0.05);
bouvdberg 0:58c11abe4785 587 if(pwm_to_motor1 > 0)
bouvdberg 0:58c11abe4785 588 motordir1 = 1;
bouvdberg 0:58c11abe4785 589 else
bouvdberg 0:58c11abe4785 590 motordir1 = 0;
bouvdberg 0:58c11abe4785 591 keep_in_range(&pwm_to_motor2, -0.05,0.05);
bouvdberg 0:58c11abe4785 592 if(pwm_to_motor2 > 0)
bouvdberg 0:58c11abe4785 593 motordir2 = 1;
bouvdberg 0:58c11abe4785 594 else
bouvdberg 0:58c11abe4785 595 motordir2 = 0;
bouvdberg 0:58c11abe4785 596 //WRITE VALUE TO MOTOR
bouvdberg 0:58c11abe4785 597 pwm_motor1.write(abs(pwm_to_motor1));
bouvdberg 0:58c11abe4785 598 pwm_motor2.write(abs(pwm_to_motor2));
bouvdberg 0:58c11abe4785 599 float sent_pwm = abs(pwm_to_motor2);
bouvdberg 0:58c11abe4785 600 pc.printf(" %f ",sent_pwm);
bouvdberg 0:58c11abe4785 601
bouvdberg 0:58c11abe4785 602 }
bouvdberg 0:58c11abe4785 603
bouvdberg 0:58c11abe4785 604 pwm_motor1.write(0);
bouvdberg 0:58c11abe4785 605 pwm_motor2.write(0);
bouvdberg 0:58c11abe4785 606 menu=0;
bouvdberg 0:58c11abe4785 607 }
bouvdberg 0:58c11abe4785 608
bouvdberg 0:58c11abe4785 609 void keep_in_range(float * in, float min, float max)
bouvdberg 0:58c11abe4785 610 {
bouvdberg 0:58c11abe4785 611 *in > min ? *in < max? : *in = max: *in = min;
bouvdberg 0:58c11abe4785 612 }
bouvdberg 0:58c11abe4785 613
bouvdberg 0:58c11abe4785 614 void setlooptimerflag(void)
bouvdberg 0:58c11abe4785 615 {
bouvdberg 0:58c11abe4785 616 looptimerflag = true;
bouvdberg 0:58c11abe4785 617 }