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
main.cpp
- Committer:
- sivuu
- Date:
- 2016-10-28
- Revision:
- 28:edfab1cb48df
- Parent:
- 27:be2be9600e2d
File content as of revision 28:edfab1cb48df:
#include "mbed.h" //standaard bieb mbed #include "QEI.h" //bieb voor encoderfuncties in c++ #include "MODSERIAL.h" //bieb voor modserial InterruptIn sw3(SW3); DigitalIn sw2(SW2); DigitalIn encoder1A(D13); DigitalIn encoder1B(D12); DigitalIn encoder2A(D11); DigitalIn encoder2B(D10); DigitalIn button_cw(D9); DigitalIn button_ccw(PTC12); MODSERIAL pc(USBTX, USBRX); DigitalOut richting_motor1(D4); PwmOut pwm_motor1(D5); DigitalOut richting_motor2(D7); PwmOut pwm_motor2(D6); int n = 0; //start van de teller wordt op nul gesteld void SwitchN() { // maakt simpele functie die 1 bij n optelt n++; } //constanten voor de encoder const int CW = 2.5; //definitie rechtsom 'lage waarde' const int CCW =-1; //definitie linksom 'hoge waarde' const float gearboxratio=131.25; // gearboxratio van encoder naar motor const float rev_rond=64.0; // aantal revoluties per omgang van de encoder volatile double current_position_motor1 = 0; // current position is 0 op het begin volatile double previous_position_motor1 = 0; // previous position is 0 op het begin volatile double current_position_motor2 = 0; volatile double previous_position_motor2 = 0; volatile bool tickerflag; //bool zorgt er voor dat de tickerflag alleen 1 of 0 kan zijn (true or false) volatile double snelheid_motor1; // snelheid van motor1 wordt later berekend door waardes uit de encoder is in rad/s volatile double snelheid_motor2; // snelheid van motor2 wordt later berekend door waardes uit de encoder is in rad/s double ticker_TS=0.1; // zorgt voor een tijdstap van de ticker van 0.1 seconde volatile double timepassed=0; //de waarde van hoeveel tijd er verstreken is Ticker t; // maakt ticker t aan 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. volatile double value2_resetbutton = 0; void tickerfunctie() // maakt een ticker functie aan { tickerflag = 1; // het enige wat deze functie doet is zorgen dat tickerflag 1 is } int main() { pc.baud(115200); // zorgt voor de link voor putty, 115200 is snelheid 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. QEI Encoder1(D10,D11, NC, rev_rond,QEI::X4_ENCODING); float counts_encoder1; //variabele counts aanmaken float rev_counts_motor1; //variabele motor rondjes aanmaken in radialen!! float counts_encoder2; float rev_counts_motor2; t.attach(&tickerfunctie,ticker_TS); // attacht de ticker met een tick van 0.1 seconde (gelijk aan de tijdstap) volatile float voltage_motor1=0.27; //pwm is de pulse with tussen geen ampere en wel ampere motor 1 volatile float voltage_motor2=1;//pwm is de pulse with tussen geen ampere en wel ampere motor 1 while (true) { // zorgt er voor dat de code oneindig doorgelopen wordt sw3.fall(&SwitchN); // zorgt er voor dat void switch wordt gedaan als switch 3 wordt ingedrukt while(sw2==0 && rev_counts_motor1<0 && 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 pc.printf("switch 2 is ingedrukt \r\n"); pc.printf("aantal rondjes motor 1 is kleiner dan 0\r\n"); richting_motor1 = 1; // zorgt er voor dat de motor tegenovergestelde richting gaat draaien zodat het totaal aantal rondjes gedraaid op 0 komt te staan pwm_motor1 = voltage_motor1; // zorgt er voor dat er een ampere is voor de motor om te draaien pc.printf("richting motor is %f \r\n", richting_motor1); pc.printf("voltage motor 1 is %f \r\n", voltage_motor1); counts_encoder1 = Encoder1.getPulses(); // zorgt er voor dat het aantal rondjes geteld worden rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond); // zorgt er voor dat rev counts niet het aantal pulsen is maar het aantal rondjes pc.printf("rondjes motor is %f \r\n", rev_counts_motor1); 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. } while (sw2==0 && rev_counts_motor1>0 && value1_resetbutton <=0){ // werkt het zelfde als de vorige loop maar dan met tegengestelde richting. pc.printf("aantal rondjes motor 1 is groter dan 0 \r\n"); richting_motor1 = 0; pwm_motor1 = voltage_motor1; pc.printf("richting motor is %f \r\n", richting_motor1); pc.printf("voltage motor 1 is %f \r\n", voltage_motor1); counts_encoder1 = Encoder1.getPulses(); rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond); pc.printf("rondjes motor is %f \r\n", rev_counts_motor1); value1_resetbutton = -1; } while(sw2==0 && rev_counts_motor2<0 && value2_resetbutton >= 0){ // werkt het zelfde maar dan voor motor2 pc.printf("aantal rondjes motor 2 is kleiner dan 0\r\n"); richting_motor2 = 0; pwm_motor2 = voltage_motor2; pwm_motor1 = 0; pc.printf("richting motor 2 is %f \r\n", richting_motor2); pc.printf("voltage motor 2 is %f \r\n", voltage_motor2); counts_encoder2 = Encoder2.getPulses(); rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond); pc.printf("rondjes motor 2is %f \r\n", rev_counts_motor2); value2_resetbutton = 1; } while (sw2==0 && rev_counts_motor2>0 && value2_resetbutton <=0){ pc.printf("aantal rondjes motor 2 is groter dan 0\r\n"); richting_motor2 = 1; pwm_motor2 = voltage_motor2; pwm_motor1=0; pc.printf("richting motor 2is %f \r\n", richting_motor2); pc.printf("voltage motor 2 is %f \r\n", voltage_motor2); counts_encoder2 = Encoder2.getPulses(); rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond); pc.printf("rondjes motor2 is %f \r\n", rev_counts_motor2); value2_resetbutton = -1; } if (n%2==0 && button_cw==0 && tickerflag == 1) // als s ingedrukt wordt en het getal is even gebeurd het onderstaande { pc.printf("n is even \n\r"); // print lijn "n is even" pc.printf("up \n\r"); // print lijn "up" richting_motor1 = 1; // geeft richting van motor 1 aan pwm_motor1 = voltage_motor1; // geeft pulse lengte tussen ampere uit en aan dit bepaald de snelheid counts_encoder1 = Encoder1.getPulses(); //tellen van de pulsen in 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! pc.printf("motor rondjes omhoog: %f \r\n", rev_counts_motor1); //weergeven 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. } else if (n%2!=0 && button_cw==0 && tickerflag ==1) // als s is ingedrukt maar het getal is niet even (dus oneven) gebeurdt het onderstaande { pc.printf("n is odd \n\r"); // print lijn "n is odd" pc.printf("left \n\r"); // print lijn "left" richting_motor2 = 1; // motor 2 richting pwm_motor2 = voltage_motor2; // geeft pulse lengte tussen ampere uit en aan dit bepaald de snelheid counts_encoder2 = Encoder2.getPulses(); //tellen van de pulsen in rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond); //weergeven van het aantal rondjes pc.printf("motor rondjes omhoog: %f \r\n", rev_counts_motor2); value2_resetbutton = 0; } if (n%2==0 && button_ccw==0 && tickerflag == 1) // als d is ingedrukt en n is even dan gebeurd het volgende { pc.printf("n is even \n\r"); // print lijn "n is even" pc.printf("down \n\r"); // print lijn "down" richting_motor1 = 0; pwm_motor1 = voltage_motor1; counts_encoder1 = Encoder1.getPulses(); //tellen van de pulsen in rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond); //weergeven van het aantal rondjes pc.printf("motor rondjes omhoog: %f \r\n", rev_counts_motor1); value1_resetbutton = 0; } else if ( n%2!=0 && button_ccw==0 && tickerflag==1) // als d is ingedrukt maar het getal is niet even (dus oneven) gebeurdt het onderstaande { pc.printf("n is odd \n\r"); // print lijn "n is odd" pc.printf("right \n\r"); // print lijn "right" richting_motor2 = 0; pwm_motor2 = voltage_motor2; counts_encoder2 = Encoder2.getPulses(); //tellen van de pulsen in rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond); //weergeven van het aantal rondjes pc.printf("motor rondjes naar rechts: %f \r\n", rev_counts_motor2); value2_resetbutton = 0; } else { pc.printf("rondjes motor1 is %f: \n\r",rev_counts_motor1); counts_encoder1 = Encoder1.getPulses(); //tellen van de pulsen in rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond); counts_encoder2 = Encoder2.getPulses(); //tellen van de pulsen in rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond); pwm_motor2=0; pwm_motor1=0; } previous_position_motor1 = current_position_motor1; current_position_motor1 = rev_counts_motor1; previous_position_motor2 = current_position_motor2; current_position_motor2 = rev_counts_motor2; snelheid_motor1 = ((current_position_motor1 - previous_position_motor1)*6.28318530718) / ticker_TS; pc.printf("snelheid motor 1 is: %f \r\n", snelheid_motor1); snelheid_motor2 = ((current_position_motor2 - previous_position_motor2)*6.28318530718) / ticker_TS; pc.printf("snelheid motor 2 is: %f \r\n", snelheid_motor2); if (abs(snelheid_motor1) > 3.0){ voltage_motor1 = voltage_motor1-0.005; pc.printf("motor1 draaid te snel voltage is nu %f \r\n",voltage_motor1); } else if (abs(snelheid_motor1) < 3.0 && snelheid_motor1 != 0){ voltage_motor1 = voltage_motor1+0.005; pc.printf("motor1 draaid te langzaam voltage is nu %f \r\n",voltage_motor1); } if (abs(snelheid_motor2) > 5.0){ voltage_motor2 = voltage_motor2-0.005; pc.printf("motor1 draaid te snel voltage is nu %f \r\n",voltage_motor2); } else if (abs(snelheid_motor2) < 5.0 && snelheid_motor2 != 0){ voltage_motor2 = voltage_motor2+0.005; pc.printf("motor1 draaid te langzaam voltage is nu %f \r\n",voltage_motor2); } tickerflag = 0; if (rev_counts_motor1 <-3.4 && richting_motor1 == 0){ pwm_motor1=0; } else if(rev_counts_motor1 >3.4 && richting_motor1 == 1){ pwm_motor1 =0; } if (rev_counts_motor2 < -4.0 && richting_motor2 == 1){ pwm_motor2=0; } else if (rev_counts_motor2 > 4.0 && richting_motor2 == 0){ pwm_motor2==0; } } }