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@21:1f0fae4687af, 2016-10-24 (annotated)

Committer:
sivuu
Date:
Mon Oct 24 10:09:43 2016 +0000
Revision:
21:1f0fae4687af
Parent:
20:44d3f0e6e75a
Child:
22:7c4902b80c16
de reset button is er in gezet maar doet motor 1 en twee draaien nog niet gelijk terug. deze versie is verbeterd maar nog niet getest.;

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 12:7903c0e55cd7 25
sivuu 17:f44f41cab151 26
sivuu 17:f44f41cab151 27
sivuu 14:4b934ac37656 28 //constanten voor de encoder
sivuu 14:4b934ac37656 29 const int CW = 2.5; //definitie rechtsom 'lage waarde'
sivuu 17:f44f41cab151 30 const int CCW =-1; //definitie linksom 'hoge waarde'
sivuu 17:f44f41cab151 31 const float gearboxratio=131.25; // gearboxratio van encoder naar motor
sivuu 14:4b934ac37656 32 const int rev_rond=32; // aantal revoluties per omgang van de encoder
sivuu 17:f44f41cab151 33
sivuu 17:f44f41cab151 34 volatile double current_position_motor1 = 0; // current position is 0 op het begin
sivuu 17:f44f41cab151 35 volatile double previous_position_motor1 = 0; // previous position is 0 op het begin
sivuu 17:f44f41cab151 36 volatile double current_position_motor2 = 0;
sivuu 17:f44f41cab151 37 volatile double previous_position_motor2 = 0;
sivuu 17:f44f41cab151 38 volatile bool tickerflag; //bool zorgt er voor dat de tickerflag alleen 1 of 0 kan zijn (true or false)
sivuu 21:1f0fae4687af 39 volatile double snelheid_motor1; // snelheid van motor1 wordt later berekend door waardes uit de encoder is in rad/s
sivuu 21:1f0fae4687af 40 volatile double snelheid_motor2; // snelheid van motor2 wordt later berekend door waardes uit de encoder is in rad/s
sivuu 21:1f0fae4687af 41 double ticker_TS=0.1; // zorgt voor een tijdstap van de ticker van 0.1 seconde
sivuu 17:f44f41cab151 42 volatile double timepassed=0; //de waarde van hoeveel tijd er verstreken is
sivuu 17:f44f41cab151 43 Ticker t; // maakt ticker t aan
sivuu 21:1f0fae4687af 44 volatile double value1_resetbutton = 0;
sivuu 21:1f0fae4687af 45 volatile double value2_resetbutton = 0;
sivuu 17:f44f41cab151 46
sivuu 17:f44f41cab151 47 void tickerfunctie() // maakt een ticker functie aan
sivuu 17:f44f41cab151 48 {
sivuu 17:f44f41cab151 49 tickerflag = 1; // het enige wat deze functie doet is zorgen dat tickerflag 1 is
sivuu 17:f44f41cab151 50 }
sivuu 2:4dcdf7755a04 51
sivuu 0:b7cb5d3978b5 52 int main()
sivuu 0:b7cb5d3978b5 53 {
sivuu 12:7903c0e55cd7 54 pc.baud(115200); // zorgt voor de link voor putty, 115200 is snelheid
sivuu 17:f44f41cab151 55 QEI Encoder2(D12,D13, NC, rev_rond); // maakt een encoder aan! D12/D13 ingangen, rev_rond zijn aantal pulsen per revolutie! Bovenaan in te stellen.
sivuu 17:f44f41cab151 56 QEI Encoder1(D10,D11, NC, rev_rond);
sivuu 14:4b934ac37656 57 float counts_encoder1; //variabele counts aanmaken
sivuu 14:4b934ac37656 58 float rev_counts_motor1; //variabele motor rondjes aanmaken in radialen!!
sivuu 14:4b934ac37656 59 float counts_encoder2;
sivuu 14:4b934ac37656 60 float rev_counts_motor2;
sivuu 17:f44f41cab151 61 t.attach(&tickerfunctie,ticker_TS); // attacht de ticker met een tick van 0.01 seconde (gelijk aan de tijdstap)
sivuu 21:1f0fae4687af 62 volatile float voltage_motor1=0.27; //voltingang motor 1
sivuu 19:58c24260e08d 63 volatile float voltage_motor2=1;//voltingang motor 2
sivuu 21:1f0fae4687af 64
sivuu 21:1f0fae4687af 65
sivuu 21:1f0fae4687af 66
sivuu 21:1f0fae4687af 67
sivuu 21:1f0fae4687af 68
sivuu 12:7903c0e55cd7 69
sivuu 3:34f7c16a6a7f 70 while (true) { // zorgt er voor dat de code oneindig doorgelopen wordt
sivuu 2:4dcdf7755a04 71
sivuu 12:7903c0e55cd7 72 sw3.fall(&SwitchN); // zorgt er voor dat void switch wordt gedaan als switch 3 wordt ingedrukt
sivuu 12:7903c0e55cd7 73
sivuu 21:1f0fae4687af 74 while(sw2==0 && rev_counts_motor1<0 && value1_resetbutton >= 0){
sivuu 21:1f0fae4687af 75 pc.printf("switch 2 is ingedrukt \r\n");
sivuu 21:1f0fae4687af 76 pc.printf("aantal rondjes is kleiner dan 0\r\n");
sivuu 21:1f0fae4687af 77 richting_motor1 = 1;
sivuu 21:1f0fae4687af 78 pwm_motor1 = voltage_motor1;
sivuu 21:1f0fae4687af 79 pc.printf("richting motor is %f \r\n", richting_motor1);
sivuu 21:1f0fae4687af 80 pc.printf("voltage motor 1 is %f \r\n", voltage_motor1);
sivuu 21:1f0fae4687af 81 rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);
sivuu 21:1f0fae4687af 82 counts_encoder1 = Encoder1.getPulses();
sivuu 21:1f0fae4687af 83 pc.printf("rondjes motor is %f \r\n", rev_counts_motor1);
sivuu 21:1f0fae4687af 84 value1_resetbutton = 1;
sivuu 21:1f0fae4687af 85
sivuu 21:1f0fae4687af 86 }
sivuu 21:1f0fae4687af 87 while (sw2==0 && rev_counts_motor1>0 && value1_resetbutton <=0){
sivuu 21:1f0fae4687af 88 pc.printf("aantal rondjes is groter dan 0 \r\n");
sivuu 21:1f0fae4687af 89 richting_motor1 = 0;
sivuu 21:1f0fae4687af 90 pwm_motor1 = voltage_motor1;
sivuu 21:1f0fae4687af 91 pc.printf("richting motor is %f \r\n", richting_motor1);
sivuu 21:1f0fae4687af 92 pc.printf("voltage motor 1 is %f \r\n", voltage_motor1);
sivuu 21:1f0fae4687af 93 counts_encoder1 = Encoder1.getPulses();
sivuu 21:1f0fae4687af 94 rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);
sivuu 21:1f0fae4687af 95 pc.printf("rondjes motor is %f \r\n", rev_counts_motor1);
sivuu 21:1f0fae4687af 96 value1_resetbutton = -1;
sivuu 21:1f0fae4687af 97 }
sivuu 21:1f0fae4687af 98 while(sw2==0 && rev_counts_motor2<0 && value2_resetbutton >= 0){
sivuu 21:1f0fae4687af 99 pc.printf("aantal rondjes motor 2 is kleiner dan 0\r\n");
sivuu 21:1f0fae4687af 100 richting_motor2 = 0;
sivuu 21:1f0fae4687af 101 pwm_motor2 = voltage_motor2;
sivuu 21:1f0fae4687af 102 pc.printf("richting motor 2 is %f \r\n", richting_motor2);
sivuu 21:1f0fae4687af 103 pc.printf("voltage motor 2 is %f \r\n", voltage_motor2);
sivuu 21:1f0fae4687af 104 rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);
sivuu 21:1f0fae4687af 105 counts_encoder2 = Encoder2.getPulses();
sivuu 21:1f0fae4687af 106 pc.printf("rondjes motor 2is %f \r\n", rev_counts_motor2);
sivuu 21:1f0fae4687af 107 value2_resetbutton = 1;
sivuu 21:1f0fae4687af 108
sivuu 21:1f0fae4687af 109 }
sivuu 21:1f0fae4687af 110 while (sw2==0 && rev_counts_motor2>0 && value2_resetbutton <=0){
sivuu 21:1f0fae4687af 111 richting_motor2 = 1;
sivuu 21:1f0fae4687af 112 pwm_motor2 = voltage_motor2;
sivuu 21:1f0fae4687af 113 pc.printf("richting motor 2is %f \r\n", richting_motor2);
sivuu 21:1f0fae4687af 114 pc.printf("voltage motor 2 is %f \r\n", voltage_motor2);
sivuu 21:1f0fae4687af 115 counts_encoder2 = Encoder2.getPulses();
sivuu 21:1f0fae4687af 116 rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);
sivuu 21:1f0fae4687af 117 pc.printf("rondjes motor2 is %f \r\n", rev_counts_motor2);
sivuu 21:1f0fae4687af 118 value2_resetbutton = -1;
sivuu 21:1f0fae4687af 119 }
sivuu 0:b7cb5d3978b5 120
sivuu 12:7903c0e55cd7 121 if (button_cw==0) // als s ingedrukt wordt gebeurd het volgende
sivuu 3:34f7c16a6a7f 122 {
sivuu 17:f44f41cab151 123 if ( tickerflag == 1) // als de ticker flag 1 is gaat dit loopje lopen.
sivuu 17:f44f41cab151 124 {
sivuu 3:34f7c16a6a7f 125 if (n%2==0) // als s ingedrukt wordt en het getal is even gebeurd het onderstaande
sivuu 3:34f7c16a6a7f 126 {
sivuu 3:34f7c16a6a7f 127 pc.printf("n is even \n\r"); // print lijn "n is even"
sivuu 4:2b3fbd7ef1cf 128 pc.printf("up \n\r"); // print lijn "up"
sivuu 12:7903c0e55cd7 129 richting_motor1 = 1;
sivuu 19:58c24260e08d 130 pwm_motor1 = voltage_motor1;
sivuu 15:706e18b43dd6 131 counts_encoder1 = Encoder1.getPulses(); //tellen van de pulsen in
sivuu 14:4b934ac37656 132 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 133 pc.printf("motor rondjes omhoog: %f \r\n", rev_counts_motor1); //weergeven
sivuu 21:1f0fae4687af 134 value1_resetbutton = 0;
sivuu 21:1f0fae4687af 135 value2_resetbutton = 0;
sivuu 3:34f7c16a6a7f 136 }
sivuu 7:9dc08a9a5991 137
sivuu 3:34f7c16a6a7f 138 else // als s is ingedrukt maar het getal is niet even (dus oneven) gebeurdt het onderstaande
sivuu 3:34f7c16a6a7f 139 {
sivuu 3:34f7c16a6a7f 140 pc.printf("n is odd \n\r"); // print lijn "n is odd"
sivuu 3:34f7c16a6a7f 141 pc.printf("left \n\r"); // print lijn "left"
sivuu 12:7903c0e55cd7 142 richting_motor2 = 1;
sivuu 19:58c24260e08d 143 pwm_motor2 = voltage_motor2;
sivuu 15:706e18b43dd6 144 counts_encoder2 = Encoder2.getPulses(); //tellen van de pulsen in
sivuu 14:4b934ac37656 145 rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond); //weergeven van het aantal rondjes
sivuu 14:4b934ac37656 146 pc.printf("motor rondjes omhoog: %f \r\n", rev_counts_motor2);
sivuu 21:1f0fae4687af 147 value1_resetbutton = 0;
sivuu 21:1f0fae4687af 148 value2_resetbutton = 0;
sivuu 4:2b3fbd7ef1cf 149 }
sivuu 19:58c24260e08d 150
sivuu 17:f44f41cab151 151 previous_position_motor1 = current_position_motor1;
sivuu 17:f44f41cab151 152 current_position_motor1 = rev_counts_motor1;
sivuu 17:f44f41cab151 153 previous_position_motor2 = current_position_motor2;
sivuu 17:f44f41cab151 154 current_position_motor2 = rev_counts_motor2;
sivuu 17:f44f41cab151 155
sivuu 17:f44f41cab151 156 snelheid_motor1 = ((current_position_motor1 - previous_position_motor1)*6.28318530718) / ticker_TS;
sivuu 17:f44f41cab151 157 pc.printf("snelheid motor 1 is: %f \r\n", snelheid_motor1);
sivuu 17:f44f41cab151 158
sivuu 17:f44f41cab151 159 snelheid_motor2 = ((current_position_motor2 - previous_position_motor2)*6.28318530718) / ticker_TS;
sivuu 17:f44f41cab151 160 pc.printf("snelheid motor 2 is: %f \r\n", snelheid_motor2);
sivuu 17:f44f41cab151 161
sivuu 21:1f0fae4687af 162 if (abs(snelheid_motor1) > 3.0){
sivuu 21:1f0fae4687af 163 voltage_motor1 = voltage_motor1-0.005;
sivuu 19:58c24260e08d 164 pc.printf("motor1 draaid te snel voltage is nu %f \r\n",voltage_motor1);
sivuu 19:58c24260e08d 165 }
sivuu 21:1f0fae4687af 166 else if (abs(snelheid_motor1) < 3.0 && snelheid_motor1 != 0){
sivuu 21:1f0fae4687af 167 voltage_motor1 = voltage_motor1+0.005;
sivuu 19:58c24260e08d 168 pc.printf("motor1 draaid te langzaam voltage is nu %f \r\n",voltage_motor1);
sivuu 19:58c24260e08d 169 }
sivuu 20:44d3f0e6e75a 170 if (abs(snelheid_motor2) > 5.0){
sivuu 21:1f0fae4687af 171 voltage_motor2 = voltage_motor2-0.005;
sivuu 20:44d3f0e6e75a 172 pc.printf("motor1 draaid te snel voltage is nu %f \r\n",voltage_motor2);
sivuu 20:44d3f0e6e75a 173 }
sivuu 20:44d3f0e6e75a 174 else if (abs(snelheid_motor2) < 5.0 && snelheid_motor2 != 0){
sivuu 21:1f0fae4687af 175 voltage_motor2 = voltage_motor2+0.005;
sivuu 20:44d3f0e6e75a 176 pc.printf("motor1 draaid te langzaam voltage is nu %f \r\n",voltage_motor2);
sivuu 20:44d3f0e6e75a 177 }
sivuu 17:f44f41cab151 178 tickerflag = 0;
sivuu 17:f44f41cab151 179 }
sivuu 3:34f7c16a6a7f 180 }
sivuu 12:7903c0e55cd7 181 else if (button_ccw==0) // als d ingedrukt wordt gebeurd het volgende
sivuu 3:34f7c16a6a7f 182 {
sivuu 17:f44f41cab151 183 if ( tickerflag == 1) // als de ticker flag 1 is gaat dit loopje lopen.
sivuu 17:f44f41cab151 184 {
sivuu 3:34f7c16a6a7f 185 if (n%2==0) // als d is ingedrukt en n is even dan gebeurd het volgende
sivuu 3:34f7c16a6a7f 186 {
sivuu 3:34f7c16a6a7f 187 pc.printf("n is even \n\r"); // print lijn "n is even"
sivuu 3:34f7c16a6a7f 188 pc.printf("down \n\r"); // print lijn "down"
sivuu 12:7903c0e55cd7 189 richting_motor1 = 0;
sivuu 19:58c24260e08d 190 pwm_motor1 = voltage_motor1;
sivuu 15:706e18b43dd6 191 counts_encoder1 = Encoder1.getPulses(); //tellen van de pulsen in
sivuu 14:4b934ac37656 192 rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond); //weergeven van het aantal rondjes
sivuu 14:4b934ac37656 193 pc.printf("motor rondjes omhoog: %f \r\n", rev_counts_motor1);
sivuu 21:1f0fae4687af 194 value1_resetbutton = 0;
sivuu 21:1f0fae4687af 195 value2_resetbutton = 0;
sivuu 12:7903c0e55cd7 196
sivuu 12:7903c0e55cd7 197
sivuu 3:34f7c16a6a7f 198 }
sivuu 3:34f7c16a6a7f 199 else // als d is ingedrukt maar het getal is niet even (dus oneven) gebeurdt het onderstaande
sivuu 3:34f7c16a6a7f 200 {
sivuu 3:34f7c16a6a7f 201 pc.printf("n is odd \n\r"); // print lijn "n is odd"
sivuu 3:34f7c16a6a7f 202 pc.printf("right \n\r"); // print lijn "right"
sivuu 12:7903c0e55cd7 203 richting_motor2 = 0;
sivuu 19:58c24260e08d 204 pwm_motor2 = voltage_motor2;
sivuu 15:706e18b43dd6 205 counts_encoder2 = Encoder2.getPulses(); //tellen van de pulsen in
sivuu 14:4b934ac37656 206 rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond); //weergeven van het aantal rondjes
sivuu 21:1f0fae4687af 207 pc.printf("motor rondjes naar rechts: %f \r\n", rev_counts_motor2);
sivuu 21:1f0fae4687af 208 value1_resetbutton = 0;
sivuu 21:1f0fae4687af 209 value2_resetbutton = 0;
sivuu 12:7903c0e55cd7 210
sivuu 12:7903c0e55cd7 211 }
sivuu 17:f44f41cab151 212 previous_position_motor1 = current_position_motor1;
sivuu 17:f44f41cab151 213 current_position_motor1 = rev_counts_motor1;
sivuu 17:f44f41cab151 214 previous_position_motor2 = current_position_motor2;
sivuu 17:f44f41cab151 215 current_position_motor2 = rev_counts_motor2;
sivuu 17:f44f41cab151 216
sivuu 17:f44f41cab151 217 snelheid_motor1 = ((current_position_motor1 - previous_position_motor1)*6.28318530718) / ticker_TS;
sivuu 17:f44f41cab151 218 pc.printf("snelheid motor 1 is: %f \r\n", snelheid_motor1);
sivuu 17:f44f41cab151 219
sivuu 17:f44f41cab151 220 snelheid_motor2 = ((current_position_motor2 - previous_position_motor2)*6.28318530718) / ticker_TS;
sivuu 17:f44f41cab151 221 pc.printf("snelheid motor 2 is: %f \r\n", snelheid_motor2);
sivuu 17:f44f41cab151 222
sivuu 21:1f0fae4687af 223 if (abs(snelheid_motor1) > 3.0){
sivuu 21:1f0fae4687af 224 voltage_motor1 = voltage_motor1-0.005;
sivuu 20:44d3f0e6e75a 225 pc.printf("motor1 draaid te snel voltage is nu %f \r\n",voltage_motor1);
sivuu 20:44d3f0e6e75a 226 }
sivuu 21:1f0fae4687af 227 else if (abs(snelheid_motor1) < 3.0 && snelheid_motor1 != 0){
sivuu 21:1f0fae4687af 228 voltage_motor1 = voltage_motor1+0.005;
sivuu 20:44d3f0e6e75a 229 pc.printf("motor1 draaid te langzaam voltage is nu %f \r\n",voltage_motor1);
sivuu 20:44d3f0e6e75a 230 }
sivuu 20:44d3f0e6e75a 231 if (abs(snelheid_motor2) > 5.0){
sivuu 21:1f0fae4687af 232 voltage_motor2 = voltage_motor2-0.005;
sivuu 20:44d3f0e6e75a 233 pc.printf("motor1 draaid te snel voltage is nu %f \r\n",voltage_motor2);
sivuu 20:44d3f0e6e75a 234 }
sivuu 20:44d3f0e6e75a 235 else if (abs(snelheid_motor2) < 5.0 && snelheid_motor2 != 0){
sivuu 21:1f0fae4687af 236 voltage_motor2 = voltage_motor2+0.005;
sivuu 20:44d3f0e6e75a 237 pc.printf("motor1 draaid te langzaam voltage is nu %f \r\n",voltage_motor2);
sivuu 20:44d3f0e6e75a 238 }
sivuu 20:44d3f0e6e75a 239
sivuu 17:f44f41cab151 240 tickerflag = 0;
sivuu 17:f44f41cab151 241 }
sivuu 12:7903c0e55cd7 242 }
sivuu 12:7903c0e55cd7 243 else{
sivuu 15:706e18b43dd6 244 // pc.printf("motor staat stil \n\r");
sivuu 12:7903c0e55cd7 245 pwm_motor2=0;
sivuu 12:7903c0e55cd7 246 pwm_motor1=0;
sivuu 12:7903c0e55cd7 247 }
sivuu 12:7903c0e55cd7 248
sivuu 3:34f7c16a6a7f 249
sivuu 0:b7cb5d3978b5 250 }
sivuu 3:34f7c16a6a7f 251 }
sivuu 3:34f7c16a6a7f 252