sibren vuurberg / Mbed 2 deprecated Project_code_groep12

EMG check. met knopjes en toetsenboard emg signalen simuleren om de code te testen. groepje 12

Dependencies:   MODSERIAL QEI mbed

Fork of EMG_check by sibren vuurberg

main.cpp@30:356902e752e4, 2016-10-31 (annotated)

Committer:
sivuu
Date:
Mon Oct 31 15:20:40 2016 +0000
Revision:
30:356902e752e4
Parent:
29:5de90b30c68d 
goede script met alles werkend bij een hogere frequentie ticker;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sivuu 12:7903c0e55cd7 1 #include "mbed.h" //standaard bieb mbed
sivuu 12:7903c0e55cd7 2 #include "QEI.h" //bieb voor encoderfuncties in c++
sivuu 12:7903c0e55cd7 3 #include "MODSERIAL.h" //bieb voor modserial
sivuu 2:4dcdf7755a04 4 InterruptIn sw3(SW3);
sivuu 21:1f0fae4687af 5 DigitalIn sw2(SW2);
sivuu 17:f44f41cab151 6 DigitalIn encoder1A(D13);
sivuu 12:7903c0e55cd7 7 DigitalIn encoder1B(D12);
sivuu 17:f44f41cab151 8 DigitalIn encoder2A(D11);
sivuu 17:f44f41cab151 9 DigitalIn encoder2B(D10);
sivuu 17:f44f41cab151 10 DigitalIn button_cw(D9);
sivuu 17:f44f41cab151 11 DigitalIn button_ccw(PTC12);
sivuu 12:7903c0e55cd7 12 MODSERIAL pc(USBTX, USBRX);
sivuu 12:7903c0e55cd7 13 DigitalOut richting_motor1(D4);
sivuu 4:2b3fbd7ef1cf 14 PwmOut pwm_motor1(D5);
sivuu 12:7903c0e55cd7 15 DigitalOut richting_motor2(D7);
sivuu 4:2b3fbd7ef1cf 16 PwmOut pwm_motor2(D6);
sivuu 14:4b934ac37656 17
sivuu 14:4b934ac37656 18
sivuu 14:4b934ac37656 19
sivuu 12:7903c0e55cd7 20 int n = 0; //start van de teller wordt op nul gesteld
sivuu 2:4dcdf7755a04 21
sivuu 3:34f7c16a6a7f 22 void SwitchN() { // maakt simpele functie die 1 bij n optelt
sivuu 2:4dcdf7755a04 23 n++;
sivuu 2:4dcdf7755a04 24 }
sivuu 23:010d8edccc5b 25
sivuu 14:4b934ac37656 26 //constanten voor de encoder
sivuu 14:4b934ac37656 27 const int CW = 2.5; //definitie rechtsom 'lage waarde'
sivuu 17:f44f41cab151 28 const int CCW =-1; //definitie linksom 'hoge waarde'
sivuu 17:f44f41cab151 29 const float gearboxratio=131.25; // gearboxratio van encoder naar motor
sivuu 24:2b47887bfd78 30 const float rev_rond=64.0; // aantal revoluties per omgang van de encoder
sivuu 17:f44f41cab151 31
sivuu 17:f44f41cab151 32 volatile double current_position_motor1 = 0; // current position is 0 op het begin
sivuu 17:f44f41cab151 33 volatile double previous_position_motor1 = 0; // previous position is 0 op het begin
sivuu 17:f44f41cab151 34 volatile double current_position_motor2 = 0;
sivuu 17:f44f41cab151 35 volatile double previous_position_motor2 = 0;
sivuu 17:f44f41cab151 36 volatile bool tickerflag; //bool zorgt er voor dat de tickerflag alleen 1 of 0 kan zijn (true or false)
sivuu 21:1f0fae4687af 37 volatile double snelheid_motor1; // snelheid van motor1 wordt later berekend door waardes uit de encoder is in rad/s
sivuu 21:1f0fae4687af 38 volatile double snelheid_motor2; // snelheid van motor2 wordt later berekend door waardes uit de encoder is in rad/s
sivuu 30:356902e752e4 39 double ticker_TS=0.025; // zorgt voor een tijdstap van de ticker van 0.1 seconde
sivuu 17:f44f41cab151 40 volatile double timepassed=0; //de waarde van hoeveel tijd er verstreken is
sivuu 17:f44f41cab151 41 Ticker t; // maakt ticker t aan
sivuu 22:7c4902b80c16 42 volatile double value1_resetbutton = 0; // deze value wordt gebruikt zodat als er bij de reset button na het bereiken van de waarde nul. De motor stopt met draaien.
sivuu 21:1f0fae4687af 43 volatile double value2_resetbutton = 0;
sivuu 17:f44f41cab151 44
sivuu 17:f44f41cab151 45 void tickerfunctie() // maakt een ticker functie aan
sivuu 17:f44f41cab151 46 {
sivuu 17:f44f41cab151 47 tickerflag = 1; // het enige wat deze functie doet is zorgen dat tickerflag 1 is
sivuu 17:f44f41cab151 48 }
sivuu 2:4dcdf7755a04 49
sivuu 0:b7cb5d3978b5 50 int main()
sivuu 0:b7cb5d3978b5 51 {
sivuu 12:7903c0e55cd7 52 pc.baud(115200); // zorgt voor de link voor putty, 115200 is snelheid
sivuu 24:2b47887bfd78 53 QEI Encoder2(D12,D13, NC, rev_rond,QEI::X4_ENCODING); // maakt een encoder aan! D12/D13 ingangen, rev_rond zijn aantal pulsen per revolutie! Bovenaan in te stellen.
sivuu 24:2b47887bfd78 54 QEI Encoder1(D10,D11, NC, rev_rond,QEI::X4_ENCODING);
sivuu 14:4b934ac37656 55 float counts_encoder1; //variabele counts aanmaken
sivuu 22:7c4902b80c16 56 float rev_counts_motor1; //variabele motor rondjes aanmaken in radialen!!
sivuu 14:4b934ac37656 57 float counts_encoder2;
sivuu 14:4b934ac37656 58 float rev_counts_motor2;
sivuu 22:7c4902b80c16 59 t.attach(&tickerfunctie,ticker_TS); // attacht de ticker met een tick van 0.1 seconde (gelijk aan de tijdstap)
sivuu 30:356902e752e4 60 volatile float voltage_motor1=0.18; //pwm is de pulse with tussen geen ampere en wel ampere motor 1
sivuu 22:7c4902b80c16 61 volatile float voltage_motor2=1;//pwm is de pulse with tussen geen ampere en wel ampere motor 1
sivuu 21:1f0fae4687af 62
sivuu 21:1f0fae4687af 63
sivuu 21:1f0fae4687af 64
sivuu 21:1f0fae4687af 65
sivuu 21:1f0fae4687af 66
sivuu 12:7903c0e55cd7 67
sivuu 3:34f7c16a6a7f 68 while (true) { // zorgt er voor dat de code oneindig doorgelopen wordt
sivuu 2:4dcdf7755a04 69
sivuu 12:7903c0e55cd7 70 sw3.fall(&SwitchN); // zorgt er voor dat void switch wordt gedaan als switch 3 wordt ingedrukt
sivuu 12:7903c0e55cd7 71
sivuu 29:5de90b30c68d 72 while(sw2==0 && rev_counts_motor1<-0.1 && value1_resetbutton >= 0){ // zorgt er voor dat de loop wordt doorlopen als switch 2 wordt ingedrukt, aantal rondejs van de motor kleiner is dan 0 en de value resetbutton groter of gelijk is aan 0
sivuu 21:1f0fae4687af 73 pc.printf("switch 2 is ingedrukt \r\n");
sivuu 24:2b47887bfd78 74 pc.printf("aantal rondjes motor 1 is kleiner dan 0\r\n");
sivuu 22:7c4902b80c16 75 richting_motor1 = 1; // zorgt er voor dat de motor tegenovergestelde richting gaat draaien zodat het totaal aantal rondjes gedraaid op 0 komt te staan
sivuu 29:5de90b30c68d 76 pwm_motor1 = 0.1; // zorgt er voor dat er een ampere is voor de motor om te draaien
sivuu 21:1f0fae4687af 77 pc.printf("richting motor is %f \r\n", richting_motor1);
sivuu 21:1f0fae4687af 78 pc.printf("voltage motor 1 is %f \r\n", voltage_motor1);
sivuu 24:2b47887bfd78 79 counts_encoder1 = Encoder1.getPulses(); // zorgt er voor dat het aantal rondjes geteld worden
sivuu 22:7c4902b80c16 80 rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond); // zorgt er voor dat rev counts niet het aantal pulsen is maar het aantal rondjes
sivuu 21:1f0fae4687af 81 pc.printf("rondjes motor is %f \r\n", rev_counts_motor1);
sivuu 22:7c4902b80c16 82 value1_resetbutton = 1; // veranderd de value1_resetbutton in 1 hierdoor kan er niet meer in de andere loop gekomen worden maar nog wel in dezelfde. dit voorkomt dat als de loop 0 heeft bereikt die daar blijft staan en niet oneindig om nul blijft draaien.
sivuu 21:1f0fae4687af 83 }
sivuu 29:5de90b30c68d 84 while (sw2==0 && rev_counts_motor1>0.1 && value1_resetbutton <=0){ // werkt het zelfde als de vorige loop maar dan met tegengestelde richting.
sivuu 24:2b47887bfd78 85 pc.printf("aantal rondjes motor 1 is groter dan 0 \r\n");
sivuu 21:1f0fae4687af 86 richting_motor1 = 0;
sivuu 29:5de90b30c68d 87 pwm_motor1 = 0.1;
sivuu 21:1f0fae4687af 88 pc.printf("richting motor is %f \r\n", richting_motor1);
sivuu 21:1f0fae4687af 89 pc.printf("voltage motor 1 is %f \r\n", voltage_motor1);
sivuu 21:1f0fae4687af 90 counts_encoder1 = Encoder1.getPulses();
sivuu 21:1f0fae4687af 91 rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);
sivuu 21:1f0fae4687af 92 pc.printf("rondjes motor is %f \r\n", rev_counts_motor1);
sivuu 21:1f0fae4687af 93 value1_resetbutton = -1;
sivuu 21:1f0fae4687af 94 }
sivuu 29:5de90b30c68d 95 while(sw2==0 && rev_counts_motor2<-0.1 && value2_resetbutton >= 0){ // werkt het zelfde maar dan voor motor2
sivuu 21:1f0fae4687af 96 pc.printf("aantal rondjes motor 2 is kleiner dan 0\r\n");
sivuu 21:1f0fae4687af 97 richting_motor2 = 0;
sivuu 29:5de90b30c68d 98 pwm_motor2 = 0.1;
sivuu 24:2b47887bfd78 99 pwm_motor1 = 0;
sivuu 21:1f0fae4687af 100 pc.printf("richting motor 2 is %f \r\n", richting_motor2);
sivuu 21:1f0fae4687af 101 pc.printf("voltage motor 2 is %f \r\n", voltage_motor2);
sivuu 24:2b47887bfd78 102 counts_encoder2 = Encoder2.getPulses();
sivuu 21:1f0fae4687af 103 rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);
sivuu 21:1f0fae4687af 104 pc.printf("rondjes motor 2is %f \r\n", rev_counts_motor2);
sivuu 24:2b47887bfd78 105 value2_resetbutton = 1;
sivuu 21:1f0fae4687af 106 }
sivuu 29:5de90b30c68d 107 while (sw2==0 && rev_counts_motor2>0.1 && value2_resetbutton <=0){
sivuu 24:2b47887bfd78 108 pc.printf("aantal rondjes motor 2 is groter dan 0\r\n");
sivuu 21:1f0fae4687af 109 richting_motor2 = 1;
sivuu 29:5de90b30c68d 110 pwm_motor2 = 0.1;
sivuu 24:2b47887bfd78 111 pwm_motor1=0;
sivuu 21:1f0fae4687af 112 pc.printf("richting motor 2is %f \r\n", richting_motor2);
sivuu 21:1f0fae4687af 113 pc.printf("voltage motor 2 is %f \r\n", voltage_motor2);
sivuu 21:1f0fae4687af 114 counts_encoder2 = Encoder2.getPulses();
sivuu 21:1f0fae4687af 115 rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);
sivuu 21:1f0fae4687af 116 pc.printf("rondjes motor2 is %f \r\n", rev_counts_motor2);
sivuu 21:1f0fae4687af 117 value2_resetbutton = -1;
sivuu 21:1f0fae4687af 118 }
sivuu 0:b7cb5d3978b5 119
sivuu 12:7903c0e55cd7 120 if (button_cw==0) // als s ingedrukt wordt gebeurd het volgende
sivuu 3:34f7c16a6a7f 121 {
sivuu 17:f44f41cab151 122 if ( tickerflag == 1) // als de ticker flag 1 is gaat dit loopje lopen.
sivuu 17:f44f41cab151 123 {
sivuu 25:31179ebc3582 124 if (n%2==0) // als s ingedrukt wordt en het getal is even gebeurd het onderstaande
sivuu 3:34f7c16a6a7f 125 {
sivuu 3:34f7c16a6a7f 126 pc.printf("n is even \n\r"); // print lijn "n is even"
sivuu 4:2b3fbd7ef1cf 127 pc.printf("up \n\r"); // print lijn "up"
sivuu 22:7c4902b80c16 128 richting_motor1 = 1; // geeft richting van motor 1 aan
sivuu 22:7c4902b80c16 129 pwm_motor1 = voltage_motor1; // geeft pulse lengte tussen ampere uit en aan dit bepaald de snelheid
sivuu 15:706e18b43dd6 130 counts_encoder1 = Encoder1.getPulses(); //tellen van de pulsen in
sivuu 14:4b934ac37656 131 rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond); //berekenen van het aantal rondjes van motor. Gedeeld door gearboxratio en rev rond, om naar motorrondjes te gaan in plaats van pulsen van encoder!
sivuu 14:4b934ac37656 132 pc.printf("motor rondjes omhoog: %f \r\n", rev_counts_motor1); //weergeven
sivuu 22:7c4902b80c16 133 value1_resetbutton = 0; // reset de value van de resetbutton zodat als je hebt gereset en dan weer gaat draaien dat je gewoon normaal kan resetten.
sivuu 3:34f7c16a6a7f 134 }
sivuu 7:9dc08a9a5991 135
sivuu 3:34f7c16a6a7f 136 else // als s is ingedrukt maar het getal is niet even (dus oneven) gebeurdt het onderstaande
sivuu 3:34f7c16a6a7f 137 {
sivuu 3:34f7c16a6a7f 138 pc.printf("n is odd \n\r"); // print lijn "n is odd"
sivuu 3:34f7c16a6a7f 139 pc.printf("left \n\r"); // print lijn "left"
sivuu 22:7c4902b80c16 140 richting_motor2 = 1; // motor 2 richting
sivuu 22:7c4902b80c16 141 pwm_motor2 = voltage_motor2; // geeft pulse lengte tussen ampere uit en aan dit bepaald de snelheid
sivuu 15:706e18b43dd6 142 counts_encoder2 = Encoder2.getPulses(); //tellen van de pulsen in
sivuu 14:4b934ac37656 143 rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond); //weergeven van het aantal rondjes
sivuu 14:4b934ac37656 144 pc.printf("motor rondjes omhoog: %f \r\n", rev_counts_motor2);
sivuu 21:1f0fae4687af 145 value2_resetbutton = 0;
sivuu 4:2b3fbd7ef1cf 146 }
sivuu 19:58c24260e08d 147
sivuu 22:7c4902b80c16 148 previous_position_motor1 = current_position_motor1; // zorgt er voor dat de huidige positie wordt gedefineerd als de vorige positie is
sivuu 22:7c4902b80c16 149 current_position_motor1 = rev_counts_motor1; // zorgt dat de huidige positie wordt gedefineerd als het huidige aantal rondejs dat gedraaid is
sivuu 22:7c4902b80c16 150 previous_position_motor2 = current_position_motor2; // zelfde maar dan voor motor2
sivuu 17:f44f41cab151 151 current_position_motor2 = rev_counts_motor2;
sivuu 17:f44f41cab151 152
sivuu 22:7c4902b80c16 153 snelheid_motor1 = ((current_position_motor1 - previous_position_motor1)*6.28318530718) / ticker_TS; // berekend de snelheid van de motor door te kijken naar het aantal rondjes dat gedraaid is in radialen (dus *2pi) delen door de tijd waarin dat gedraaid is (die is altijd 0.1 want de ticker laat het script iedere 0.1 seconde lopen).
sivuu 17:f44f41cab151 154 pc.printf("snelheid motor 1 is: %f \r\n", snelheid_motor1);
sivuu 17:f44f41cab151 155
sivuu 22:7c4902b80c16 156 snelheid_motor2 = ((current_position_motor2 - previous_position_motor2)*6.28318530718) / ticker_TS; // doet het zelfde voor motor 2
sivuu 17:f44f41cab151 157 pc.printf("snelheid motor 2 is: %f \r\n", snelheid_motor2);
sivuu 17:f44f41cab151 158
sivuu 22:7c4902b80c16 159 if (abs(snelheid_motor1) > 3.0){ // zorgt dat als de absolute van de snelheid van motor 1 boven de target snelheid zit ( in dit geval 3.0 rad/s) dat er in dit loopje gelopen wordt
sivuu 22:7c4902b80c16 160 voltage_motor1 = voltage_motor1-0.005; // zorgt er voor dat de pwm verlaagd word hierdoor word de puls lengte kleiner en zal de motor langzamer gaan draaien.
sivuu 19:58c24260e08d 161 pc.printf("motor1 draaid te snel voltage is nu %f \r\n",voltage_motor1);
sivuu 19:58c24260e08d 162 }
sivuu 22:7c4902b80c16 163 else if (abs(snelheid_motor1) < 3.0 && snelheid_motor1 != 0){ // doet het zelfde maar dan als de snelheid lager is dan de target speed
sivuu 22:7c4902b80c16 164 voltage_motor1 = voltage_motor1+0.005; // verhoogd dus ook de pwm
sivuu 19:58c24260e08d 165 pc.printf("motor1 draaid te langzaam voltage is nu %f \r\n",voltage_motor1);
sivuu 19:58c24260e08d 166 }
sivuu 22:7c4902b80c16 167 if (abs(snelheid_motor2) > 5.0){// zelfde maar dan voor motor 2
sivuu 21:1f0fae4687af 168 voltage_motor2 = voltage_motor2-0.005;
sivuu 20:44d3f0e6e75a 169 pc.printf("motor1 draaid te snel voltage is nu %f \r\n",voltage_motor2);
sivuu 20:44d3f0e6e75a 170 }
sivuu 20:44d3f0e6e75a 171 else if (abs(snelheid_motor2) < 5.0 && snelheid_motor2 != 0){
sivuu 21:1f0fae4687af 172 voltage_motor2 = voltage_motor2+0.005;
sivuu 20:44d3f0e6e75a 173 pc.printf("motor1 draaid te langzaam voltage is nu %f \r\n",voltage_motor2);
sivuu 20:44d3f0e6e75a 174 }
sivuu 22:7c4902b80c16 175 tickerflag = 0; // reset de tickerflag weer op 0 zodat het loopje niet wordt doorlopen tot de volgende tick zo kan de tijd tussen het lopen van ieder loopje gecontroleerd worden
sivuu 17:f44f41cab151 176 }
sivuu 3:34f7c16a6a7f 177 }
sivuu 22:7c4902b80c16 178 else if (button_ccw==0) // als d ingedrukt wordt gebeurd het volgende // dit doet het zelfde als bovenstaande alleen dan voor het andere knopje.
sivuu 3:34f7c16a6a7f 179 {
sivuu 17:f44f41cab151 180 if ( tickerflag == 1) // als de ticker flag 1 is gaat dit loopje lopen.
sivuu 17:f44f41cab151 181 {
sivuu 25:31179ebc3582 182 if (n%2==0) // als d is ingedrukt en n is even dan gebeurd het volgende
sivuu 3:34f7c16a6a7f 183 {
sivuu 3:34f7c16a6a7f 184 pc.printf("n is even \n\r"); // print lijn "n is even"
sivuu 3:34f7c16a6a7f 185 pc.printf("down \n\r"); // print lijn "down"
sivuu 12:7903c0e55cd7 186 richting_motor1 = 0;
sivuu 19:58c24260e08d 187 pwm_motor1 = voltage_motor1;
sivuu 15:706e18b43dd6 188 counts_encoder1 = Encoder1.getPulses(); //tellen van de pulsen in
sivuu 14:4b934ac37656 189 rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond); //weergeven van het aantal rondjes
sivuu 14:4b934ac37656 190 pc.printf("motor rondjes omhoog: %f \r\n", rev_counts_motor1);
sivuu 21:1f0fae4687af 191 value1_resetbutton = 0;
sivuu 24:2b47887bfd78 192
sivuu 12:7903c0e55cd7 193
sivuu 12:7903c0e55cd7 194
sivuu 3:34f7c16a6a7f 195 }
sivuu 3:34f7c16a6a7f 196 else // als d is ingedrukt maar het getal is niet even (dus oneven) gebeurdt het onderstaande
sivuu 3:34f7c16a6a7f 197 {
sivuu 3:34f7c16a6a7f 198 pc.printf("n is odd \n\r"); // print lijn "n is odd"
sivuu 3:34f7c16a6a7f 199 pc.printf("right \n\r"); // print lijn "right"
sivuu 12:7903c0e55cd7 200 richting_motor2 = 0;
sivuu 19:58c24260e08d 201 pwm_motor2 = voltage_motor2;
sivuu 15:706e18b43dd6 202 counts_encoder2 = Encoder2.getPulses(); //tellen van de pulsen in
sivuu 14:4b934ac37656 203 rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond); //weergeven van het aantal rondjes
sivuu 21:1f0fae4687af 204 pc.printf("motor rondjes naar rechts: %f \r\n", rev_counts_motor2);
sivuu 21:1f0fae4687af 205 value2_resetbutton = 0;
sivuu 12:7903c0e55cd7 206
sivuu 12:7903c0e55cd7 207 }
sivuu 17:f44f41cab151 208 previous_position_motor1 = current_position_motor1;
sivuu 17:f44f41cab151 209 current_position_motor1 = rev_counts_motor1;
sivuu 17:f44f41cab151 210 previous_position_motor2 = current_position_motor2;
sivuu 17:f44f41cab151 211 current_position_motor2 = rev_counts_motor2;
sivuu 17:f44f41cab151 212
sivuu 17:f44f41cab151 213 snelheid_motor1 = ((current_position_motor1 - previous_position_motor1)*6.28318530718) / ticker_TS;
sivuu 17:f44f41cab151 214 pc.printf("snelheid motor 1 is: %f \r\n", snelheid_motor1);
sivuu 17:f44f41cab151 215
sivuu 17:f44f41cab151 216 snelheid_motor2 = ((current_position_motor2 - previous_position_motor2)*6.28318530718) / ticker_TS;
sivuu 17:f44f41cab151 217 pc.printf("snelheid motor 2 is: %f \r\n", snelheid_motor2);
sivuu 17:f44f41cab151 218
sivuu 21:1f0fae4687af 219 if (abs(snelheid_motor1) > 3.0){
sivuu 21:1f0fae4687af 220 voltage_motor1 = voltage_motor1-0.005;
sivuu 20:44d3f0e6e75a 221 pc.printf("motor1 draaid te snel voltage is nu %f \r\n",voltage_motor1);
sivuu 20:44d3f0e6e75a 222 }
sivuu 21:1f0fae4687af 223 else if (abs(snelheid_motor1) < 3.0 && snelheid_motor1 != 0){
sivuu 21:1f0fae4687af 224 voltage_motor1 = voltage_motor1+0.005;
sivuu 20:44d3f0e6e75a 225 pc.printf("motor1 draaid te langzaam voltage is nu %f \r\n",voltage_motor1);
sivuu 20:44d3f0e6e75a 226 }
sivuu 20:44d3f0e6e75a 227 if (abs(snelheid_motor2) > 5.0){
sivuu 21:1f0fae4687af 228 voltage_motor2 = voltage_motor2-0.005;
sivuu 20:44d3f0e6e75a 229 pc.printf("motor1 draaid te snel voltage is nu %f \r\n",voltage_motor2);
sivuu 20:44d3f0e6e75a 230 }
sivuu 20:44d3f0e6e75a 231 else if (abs(snelheid_motor2) < 5.0 && snelheid_motor2 != 0){
sivuu 21:1f0fae4687af 232 voltage_motor2 = voltage_motor2+0.005;
sivuu 20:44d3f0e6e75a 233 pc.printf("motor1 draaid te langzaam voltage is nu %f \r\n",voltage_motor2);
sivuu 20:44d3f0e6e75a 234 }
sivuu 20:44d3f0e6e75a 235
sivuu 17:f44f41cab151 236 tickerflag = 0;
sivuu 17:f44f41cab151 237 }
sivuu 12:7903c0e55cd7 238 }
sivuu 12:7903c0e55cd7 239 else{
sivuu 29:5de90b30c68d 240 pc.printf("rondjes motor1 is %f:",rev_counts_motor1);
sivuu 29:5de90b30c68d 241 pc.printf("rondjes motor2 is %f: \n\r",rev_counts_motor2);
sivuu 24:2b47887bfd78 242 counts_encoder1 = Encoder1.getPulses(); //tellen van de pulsen in
sivuu 24:2b47887bfd78 243 rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);
sivuu 24:2b47887bfd78 244 counts_encoder2 = Encoder2.getPulses(); //tellen van de pulsen in
sivuu 24:2b47887bfd78 245 rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);
sivuu 12:7903c0e55cd7 246 pwm_motor2=0;
sivuu 12:7903c0e55cd7 247 pwm_motor1=0;
sivuu 12:7903c0e55cd7 248 }
sivuu 26:a6b8bad5b0e2 249 if (rev_counts_motor1 <-3.4 && richting_motor1 == 0){
sivuu 26:a6b8bad5b0e2 250 pwm_motor1=0;
sivuu 26:a6b8bad5b0e2 251 }
sivuu 26:a6b8bad5b0e2 252 else if(rev_counts_motor1 >3.4 && richting_motor1 == 1){
sivuu 26:a6b8bad5b0e2 253 pwm_motor1 =0;
sivuu 26:a6b8bad5b0e2 254 }
sivuu 26:a6b8bad5b0e2 255
sivuu 29:5de90b30c68d 256 if (rev_counts_motor2 < -2.0 && richting_motor2 == 1){
sivuu 29:5de90b30c68d 257 pc.printf("rondjes motor kleinder dan -2.0 dus ik stop \r\n");
sivuu 26:a6b8bad5b0e2 258 pwm_motor2=0;
sivuu 29:5de90b30c68d 259 richting_motor2=0;
sivuu 26:a6b8bad5b0e2 260 }
sivuu 29:5de90b30c68d 261 else if (rev_counts_motor2 > 2.0 && richting_motor2 == 0){
sivuu 29:5de90b30c68d 262 pc.printf("rondjes motor groter dan 2.0 dus ik stop \r\n");
sivuu 29:5de90b30c68d 263 pwm_motor2=0;
sivuu 29:5de90b30c68d 264 richting_motor2=1;
sivuu 26:a6b8bad5b0e2 265 }
sivuu 3:34f7c16a6a7f 266
sivuu 0:b7cb5d3978b5 267 }
sivuu 3:34f7c16a6a7f 268 }
sivuu 3:34f7c16a6a7f 269