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@18:cd292bac4524, 2016-10-19 (annotated)

Committer:
sivuu
Date:
Wed Oct 19 15:33:19 2016 +0000
Revision:
18:cd292bac4524
Parent:
17:f44f41cab151 
met eerste poging van de snelheids controlle en regulatie

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 17:f44f41cab151 5 DigitalIn encoder1A(D13);
sivuu 12:7903c0e55cd7 6 DigitalIn encoder1B(D12);
sivuu 17:f44f41cab151 7 DigitalIn encoder2A(D11);
sivuu 17:f44f41cab151 8 DigitalIn encoder2B(D10);
sivuu 17:f44f41cab151 9 DigitalIn button_cw(D9);
sivuu 17:f44f41cab151 10 DigitalIn button_ccw(PTC12);
sivuu 12:7903c0e55cd7 11 MODSERIAL pc(USBTX, USBRX);
sivuu 12:7903c0e55cd7 12 DigitalOut richting_motor1(D4);
sivuu 4:2b3fbd7ef1cf 13 PwmOut pwm_motor1(D5);
sivuu 12:7903c0e55cd7 14 DigitalOut richting_motor2(D7);
sivuu 4:2b3fbd7ef1cf 15 PwmOut pwm_motor2(D6);
sivuu 14:4b934ac37656 16
sivuu 14:4b934ac37656 17
sivuu 14:4b934ac37656 18
sivuu 12:7903c0e55cd7 19 int n = 0; //start van de teller wordt op nul gesteld
sivuu 2:4dcdf7755a04 20
sivuu 3:34f7c16a6a7f 21 void SwitchN() { // maakt simpele functie die 1 bij n optelt
sivuu 2:4dcdf7755a04 22 n++;
sivuu 2:4dcdf7755a04 23 }
sivuu 12:7903c0e55cd7 24
sivuu 17:f44f41cab151 25
sivuu 17:f44f41cab151 26
sivuu 14:4b934ac37656 27 //constanten voor de encoder
sivuu 14:4b934ac37656 28 const int CW = 2.5; //definitie rechtsom 'lage waarde'
sivuu 17:f44f41cab151 29 const int CCW =-1; //definitie linksom 'hoge waarde'
sivuu 17:f44f41cab151 30 const float gearboxratio=131.25; // gearboxratio van encoder naar motor
sivuu 14:4b934ac37656 31 const int rev_rond=32; // aantal revoluties per omgang van de encoder
sivuu 17:f44f41cab151 32
sivuu 17:f44f41cab151 33 volatile double current_position_motor1 = 0; // current position is 0 op het begin
sivuu 17:f44f41cab151 34 volatile double previous_position_motor1 = 0; // previous position is 0 op het begin
sivuu 17:f44f41cab151 35 volatile double current_position_motor2 = 0;
sivuu 17:f44f41cab151 36 volatile double previous_position_motor2 = 0;
sivuu 17:f44f41cab151 37 volatile bool tickerflag; //bool zorgt er voor dat de tickerflag alleen 1 of 0 kan zijn (true or false)
sivuu 17:f44f41cab151 38 volatile double snelheid_motor1;
sivuu 17:f44f41cab151 39 volatile double snelheid_motor2;
sivuu 17:f44f41cab151 40 double ticker_TS=0.01; // zorgt voor een tijdstap van de ticker van 0.01 seconde
sivuu 17:f44f41cab151 41 volatile double timepassed=0; //de waarde van hoeveel tijd er verstreken is
sivuu 17:f44f41cab151 42 Ticker t; // maakt ticker t aan
sivuu 17:f44f41cab151 43
sivuu 17:f44f41cab151 44 void tickerfunctie() // maakt een ticker functie aan
sivuu 17:f44f41cab151 45 {
sivuu 17:f44f41cab151 46 tickerflag = 1; // het enige wat deze functie doet is zorgen dat tickerflag 1 is
sivuu 17:f44f41cab151 47 }
sivuu 2:4dcdf7755a04 48
sivuu 0:b7cb5d3978b5 49 int main()
sivuu 0:b7cb5d3978b5 50 {
sivuu 12:7903c0e55cd7 51 pc.baud(115200); // zorgt voor de link voor putty, 115200 is snelheid
sivuu 17:f44f41cab151 52 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 53 QEI Encoder1(D10,D11, NC, rev_rond);
sivuu 14:4b934ac37656 54 float counts_encoder1; //variabele counts aanmaken
sivuu 14:4b934ac37656 55 float rev_counts_motor1; //variabele motor rondjes aanmaken in radialen!!
sivuu 14:4b934ac37656 56 float counts_encoder2;
sivuu 14:4b934ac37656 57 float rev_counts_motor2;
sivuu 17:f44f41cab151 58 t.attach(&tickerfunctie,ticker_TS); // attacht de ticker met een tick van 0.01 seconde (gelijk aan de tijdstap)
sivuu 17:f44f41cab151 59 const float a=12.0; //voltingang motor 1
sivuu 17:f44f41cab151 60 const float b=12.0;//voltingang motor 2
sivuu 12:7903c0e55cd7 61
sivuu 3:34f7c16a6a7f 62 while (true) { // zorgt er voor dat de code oneindig doorgelopen wordt
sivuu 2:4dcdf7755a04 63
sivuu 12:7903c0e55cd7 64 sw3.fall(&SwitchN); // zorgt er voor dat void switch wordt gedaan als switch 3 wordt ingedrukt
sivuu 12:7903c0e55cd7 65
sivuu 0:b7cb5d3978b5 66
sivuu 12:7903c0e55cd7 67
sivuu 12:7903c0e55cd7 68 if (button_cw==0) // als s ingedrukt wordt gebeurd het volgende
sivuu 3:34f7c16a6a7f 69 {
sivuu 17:f44f41cab151 70 if ( tickerflag == 1) // als de ticker flag 1 is gaat dit loopje lopen.
sivuu 17:f44f41cab151 71 {
sivuu 3:34f7c16a6a7f 72 if (n%2==0) // als s ingedrukt wordt en het getal is even gebeurd het onderstaande
sivuu 3:34f7c16a6a7f 73 {
sivuu 3:34f7c16a6a7f 74 pc.printf("n is even \n\r"); // print lijn "n is even"
sivuu 4:2b3fbd7ef1cf 75 pc.printf("up \n\r"); // print lijn "up"
sivuu 12:7903c0e55cd7 76 richting_motor1 = 1;
sivuu 12:7903c0e55cd7 77 pwm_motor1 = a;
sivuu 15:706e18b43dd6 78 counts_encoder1 = Encoder1.getPulses(); //tellen van de pulsen in
sivuu 14:4b934ac37656 79 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 80 pc.printf("motor rondjes omhoog: %f \r\n", rev_counts_motor1); //weergeven
sivuu 12:7903c0e55cd7 81
sivuu 3:34f7c16a6a7f 82 }
sivuu 7:9dc08a9a5991 83
sivuu 3:34f7c16a6a7f 84 else // als s is ingedrukt maar het getal is niet even (dus oneven) gebeurdt het onderstaande
sivuu 3:34f7c16a6a7f 85 {
sivuu 3:34f7c16a6a7f 86 pc.printf("n is odd \n\r"); // print lijn "n is odd"
sivuu 3:34f7c16a6a7f 87 pc.printf("left \n\r"); // print lijn "left"
sivuu 12:7903c0e55cd7 88 richting_motor2 = 1;
sivuu 12:7903c0e55cd7 89 pwm_motor2 = b;
sivuu 15:706e18b43dd6 90 counts_encoder2 = Encoder2.getPulses(); //tellen van de pulsen in
sivuu 14:4b934ac37656 91 rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond); //weergeven van het aantal rondjes
sivuu 14:4b934ac37656 92 pc.printf("motor rondjes omhoog: %f \r\n", rev_counts_motor2);
sivuu 4:2b3fbd7ef1cf 93 }
sivuu 17:f44f41cab151 94 previous_position_motor1 = current_position_motor1;
sivuu 17:f44f41cab151 95 current_position_motor1 = rev_counts_motor1;
sivuu 17:f44f41cab151 96 previous_position_motor2 = current_position_motor2;
sivuu 17:f44f41cab151 97 current_position_motor2 = rev_counts_motor2;
sivuu 17:f44f41cab151 98
sivuu 17:f44f41cab151 99 snelheid_motor1 = ((current_position_motor1 - previous_position_motor1)*6.28318530718) / ticker_TS;
sivuu 17:f44f41cab151 100 pc.printf("snelheid motor 1 is: %f \r\n", snelheid_motor1);
sivuu 17:f44f41cab151 101
sivuu 17:f44f41cab151 102 snelheid_motor2 = ((current_position_motor2 - previous_position_motor2)*6.28318530718) / ticker_TS;
sivuu 17:f44f41cab151 103 pc.printf("snelheid motor 2 is: %f \r\n", snelheid_motor2);
sivuu 17:f44f41cab151 104
sivuu 18:cd292bac4524 105 if (snelheid_motor1 > 4.9) && (snelheid_motor1 != 0){ // dit moet ook in minder if loops kunnen moet nog ff uitvogelen hoe
sivuu 18:cd292bac4524 106 a=a-0.02;
sivuu 18:cd292bac4524 107 }
sivuu 18:cd292bac4524 108 else if (snelheid_motor1 < 4.9) && (snelheid_motor1 != 0){
sivuu 18:cd292bac4524 109 a=a+0.02;
sivuu 18:cd292bac4524 110 }
sivuu 18:cd292bac4524 111 if (snelheid_motor2 > 4.9) && (snelheid_motor2 != 0){
sivuu 18:cd292bac4524 112 b=b-0.02;
sivuu 18:cd292bac4524 113 }
sivuu 18:cd292bac4524 114 else if (snelheid_motor2 < 4.9) && (snelheid_motor2 != 0){
sivuu 18:cd292bac4524 115 b=b+0.02;
sivuu 18:cd292bac4524 116 }
sivuu 18:cd292bac4524 117 else {
sivuu 18:cd292bac4524 118 a=a;
sivuu 18:cd292bac4524 119 b=b;
sivuu 18:cd292bac4524 120 }
sivuu 17:f44f41cab151 121
sivuu 17:f44f41cab151 122 tickerflag = 0;
sivuu 17:f44f41cab151 123 }
sivuu 3:34f7c16a6a7f 124 }
sivuu 12:7903c0e55cd7 125 else if (button_ccw==0) // als d ingedrukt wordt gebeurd het volgende
sivuu 3:34f7c16a6a7f 126 {
sivuu 17:f44f41cab151 127 if ( tickerflag == 1) // als de ticker flag 1 is gaat dit loopje lopen.
sivuu 17:f44f41cab151 128 {
sivuu 3:34f7c16a6a7f 129 if (n%2==0) // als d is ingedrukt en n is even dan gebeurd het volgende
sivuu 3:34f7c16a6a7f 130 {
sivuu 3:34f7c16a6a7f 131 pc.printf("n is even \n\r"); // print lijn "n is even"
sivuu 3:34f7c16a6a7f 132 pc.printf("down \n\r"); // print lijn "down"
sivuu 12:7903c0e55cd7 133 richting_motor1 = 0;
sivuu 12:7903c0e55cd7 134 pwm_motor1 = a;
sivuu 15:706e18b43dd6 135 counts_encoder1 = Encoder1.getPulses(); //tellen van de pulsen in
sivuu 14:4b934ac37656 136 rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond); //weergeven van het aantal rondjes
sivuu 14:4b934ac37656 137 pc.printf("motor rondjes omhoog: %f \r\n", rev_counts_motor1);
sivuu 12:7903c0e55cd7 138
sivuu 12:7903c0e55cd7 139
sivuu 3:34f7c16a6a7f 140 }
sivuu 3:34f7c16a6a7f 141 else // als d is ingedrukt maar het getal is niet even (dus oneven) gebeurdt het onderstaande
sivuu 3:34f7c16a6a7f 142 {
sivuu 3:34f7c16a6a7f 143 pc.printf("n is odd \n\r"); // print lijn "n is odd"
sivuu 3:34f7c16a6a7f 144 pc.printf("right \n\r"); // print lijn "right"
sivuu 12:7903c0e55cd7 145 richting_motor2 = 0;
sivuu 12:7903c0e55cd7 146 pwm_motor2 = b;
sivuu 15:706e18b43dd6 147 counts_encoder2 = Encoder2.getPulses(); //tellen van de pulsen in
sivuu 14:4b934ac37656 148 rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond); //weergeven van het aantal rondjes
sivuu 14:4b934ac37656 149 pc.printf("motor rondjes naar rechts: %f \r\n", rev_counts_motor2);
sivuu 12:7903c0e55cd7 150
sivuu 12:7903c0e55cd7 151 }
sivuu 17:f44f41cab151 152 previous_position_motor1 = current_position_motor1;
sivuu 17:f44f41cab151 153 current_position_motor1 = rev_counts_motor1;
sivuu 17:f44f41cab151 154 previous_position_motor2 = current_position_motor2;
sivuu 17:f44f41cab151 155 current_position_motor2 = rev_counts_motor2;
sivuu 17:f44f41cab151 156
sivuu 17:f44f41cab151 157 snelheid_motor1 = ((current_position_motor1 - previous_position_motor1)*6.28318530718) / ticker_TS;
sivuu 17:f44f41cab151 158 pc.printf("snelheid motor 1 is: %f \r\n", snelheid_motor1);
sivuu 17:f44f41cab151 159
sivuu 17:f44f41cab151 160 snelheid_motor2 = ((current_position_motor2 - previous_position_motor2)*6.28318530718) / ticker_TS;
sivuu 17:f44f41cab151 161 pc.printf("snelheid motor 2 is: %f \r\n", snelheid_motor2);
sivuu 17:f44f41cab151 162
sivuu 18:cd292bac4524 163 if (snelheid_motor1 > 4.9) && (snelheid_motor1 != 0){ // dit moet ook in minder if loops kunnen moet nog ff uitvogelen hoe
sivuu 18:cd292bac4524 164 a=a-0.02;
sivuu 18:cd292bac4524 165 }
sivuu 18:cd292bac4524 166 else if (snelheid_motor1 < 4.9) && (snelheid_motor1 != 0){
sivuu 18:cd292bac4524 167 a=a+0.02;
sivuu 18:cd292bac4524 168 }
sivuu 18:cd292bac4524 169 if (snelheid_motor2 > 4.9) && (snelheid_motor2 != 0){
sivuu 18:cd292bac4524 170 b=b-0.02;
sivuu 18:cd292bac4524 171 }
sivuu 18:cd292bac4524 172 else if (snelheid_motor2 < 4.9) && (snelheid_motor2 != 0){
sivuu 18:cd292bac4524 173 b=b+0.02;
sivuu 18:cd292bac4524 174 }
sivuu 18:cd292bac4524 175 else {
sivuu 18:cd292bac4524 176 a=a;
sivuu 18:cd292bac4524 177 b=b;
sivuu 18:cd292bac4524 178 }
sivuu 18:cd292bac4524 179
sivuu 17:f44f41cab151 180 tickerflag = 0;
sivuu 17:f44f41cab151 181 }
sivuu 12:7903c0e55cd7 182 }
sivuu 12:7903c0e55cd7 183 else{
sivuu 15:706e18b43dd6 184 // pc.printf("motor staat stil \n\r");
sivuu 12:7903c0e55cd7 185 pwm_motor2=0;
sivuu 12:7903c0e55cd7 186 pwm_motor1=0;
sivuu 12:7903c0e55cd7 187 }
sivuu 12:7903c0e55cd7 188
sivuu 3:34f7c16a6a7f 189
sivuu 0:b7cb5d3978b5 190 }
sivuu 3:34f7c16a6a7f 191 }
sivuu 3:34f7c16a6a7f 192