![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2014-10-15 Arm 1 naar thuispositie. Eerste poging, fout in keep_in_range
Dependencies: Encoder MODSERIAL TextLCD mbed mbed-dsp
main.cpp@6:3b6fad529520, 2014-10-16 (annotated)
- Committer:
- JKleinRot
- Date:
- Thu Oct 16 15:09:46 2014 +0000
- Revision:
- 6:3b6fad529520
- Parent:
- 5:a1dcd605dd3d
- Child:
- 7:dd3cba61b34b
2014-10-16 Revision 5 met commentaar. Ticker werkt nu, maar de for loop om het EMG meten 5 seconden te laten duren werkt niet
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
JKleinRot | 0:859c89785d3f | 1 | #include "mbed.h" //Mbed bibliotheek inladen, standaard functies |
JKleinRot | 0:859c89785d3f | 2 | #include "MODSERIAL.h" //MODSERIAL bibliotheek inladen, communicatie met PC |
JKleinRot | 0:859c89785d3f | 3 | #include "encoder.h" //Encoder bibliotheek inladen, communicatie met encoder |
JKleinRot | 0:859c89785d3f | 4 | #include "TextLCD.h" //LCD scherm bibliotheek inladen, communicatie met LCD scherm |
JKleinRot | 0:859c89785d3f | 5 | |
JKleinRot | 0:859c89785d3f | 6 | //Constanten definiëren en waarde geven |
JKleinRot | 6:3b6fad529520 | 7 | #define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor |
JKleinRot | 6:3b6fad529520 | 8 | #define KP 1 //Factor proprotionele regelaar |
JKleinRot | 6:3b6fad529520 | 9 | #define SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten |
JKleinRot | 0:859c89785d3f | 10 | |
JKleinRot | 0:859c89785d3f | 11 | //Pinverdeling en naamgeving variabelen |
JKleinRot | 0:859c89785d3f | 12 | TextLCD lcd(PTD2, PTB8, PTB9, PTB10, PTB11, PTE2); //LCD scherm |
JKleinRot | 0:859c89785d3f | 13 | MODSERIAL pc(USBTX, USBRX); //PC |
JKleinRot | 0:859c89785d3f | 14 | |
JKleinRot | 1:e5e1eb9d0025 | 15 | PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1 |
JKleinRot | 1:e5e1eb9d0025 | 16 | DigitalOut dir_motor_arm1(PTD1); //Richting van motor arm 1 |
JKleinRot | 1:e5e1eb9d0025 | 17 | Encoder puls_motor_arm1(PTD0, PTC9); //Encoder pulsen tellen van motor arm 1 |
JKleinRot | 6:3b6fad529520 | 18 | PwmOut pwm_motor_arm2(PTA12); //PWM naar motor arm 2 |
JKleinRot | 6:3b6fad529520 | 19 | DigitalOut dir_motor_arm2(PTD3); //Ricting van motor arm 2 |
JKleinRot | 6:3b6fad529520 | 20 | Encoder puls_motor_arm2(PTD4, PTC8); //Encoder pulsen tellen van motor arm 2 |
JKleinRot | 6:3b6fad529520 | 21 | AnalogIn EMG_bi(PTB1); //Meten EMG signaal biceps |
JKleinRot | 1:e5e1eb9d0025 | 22 | |
JKleinRot | 4:69540b9c0646 | 23 | |
JKleinRot | 6:3b6fad529520 | 24 | Ticker ticker_regelaar; //Ticker voor regelaar motor |
JKleinRot | 6:3b6fad529520 | 25 | Ticker ticker_EMG; //Ticker voor EMG meten |
JKleinRot | 1:e5e1eb9d0025 | 26 | |
JKleinRot | 6:3b6fad529520 | 27 | //Gedefinieerde datatypen en naamgeving en beginwaarden |
JKleinRot | 0:859c89785d3f | 28 | bool rust = false; //Bool voor controleren volgorde in programma |
JKleinRot | 3:adc3052039e7 | 29 | bool kalibratie_positie_arm1 = false; //Bool voor controleren volgorde in programma |
JKleinRot | 6:3b6fad529520 | 30 | bool kalibratie_positie_arm2 = false; //Bool voor controleren volgorde in programma |
JKleinRot | 6:3b6fad529520 | 31 | bool kalibratie_EMG_bi = false; //Bool voor controleren volgorde in programma |
JKleinRot | 6:3b6fad529520 | 32 | bool kalibratie_EMG_tri = false; //Bool voor controleren volgorde in programma |
JKleinRot | 0:859c89785d3f | 33 | |
JKleinRot | 6:3b6fad529520 | 34 | char *lcd_r1 = new char[16]; //Char voor tekst op eerste regel LCD scherm |
JKleinRot | 6:3b6fad529520 | 35 | char *lcd_r2 = new char[16]; //Char voor tekst op tweede regel LCD scherm |
JKleinRot | 0:859c89785d3f | 36 | |
JKleinRot | 6:3b6fad529520 | 37 | volatile bool regelaar_ticker_flag; //Definiëren flag als bool die verandert kan worden in programma |
JKleinRot | 6:3b6fad529520 | 38 | void setregelaar_ticker_flag(){ //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true |
JKleinRot | 6:3b6fad529520 | 39 | regelaar_ticker_flag = true; |
JKleinRot | 1:e5e1eb9d0025 | 40 | } |
JKleinRot | 1:e5e1eb9d0025 | 41 | |
JKleinRot | 6:3b6fad529520 | 42 | volatile bool regelaar_EMG_flag; //Definiëren flag als bool die verandert kan worden in programma |
JKleinRot | 6:3b6fad529520 | 43 | void setregelaar_EMG_flag(){ //Setregelaar_EMG_flag zet de regelaar_EMG_flag op true |
JKleinRot | 6:3b6fad529520 | 44 | regelaar_EMG_flag = true; |
JKleinRot | 4:69540b9c0646 | 45 | } |
JKleinRot | 4:69540b9c0646 | 46 | |
JKleinRot | 6:3b6fad529520 | 47 | void keep_in_range(float * in, float min, float max){ //Zorgt ervoor dat een getal niet buiten een bepaald minimum en maximum komt |
JKleinRot | 6:3b6fad529520 | 48 | if (*in < min){ //Als de waarde kleiner is als het minimum wordt de waarde het minimum |
JKleinRot | 2:0cb899f2800a | 49 | *in = min; |
JKleinRot | 2:0cb899f2800a | 50 | } |
JKleinRot | 6:3b6fad529520 | 51 | if (*in > max){ //Als de waarde groter is dan het maximum is de waarde het maximum |
JKleinRot | 2:0cb899f2800a | 52 | *in = max; |
JKleinRot | 2:0cb899f2800a | 53 | } |
JKleinRot | 6:3b6fad529520 | 54 | else { //In alle andere gevallen is de waarde de waarde zelf |
JKleinRot | 2:0cb899f2800a | 55 | *in = *in; |
JKleinRot | 2:0cb899f2800a | 56 | } |
JKleinRot | 2:0cb899f2800a | 57 | } |
JKleinRot | 2:0cb899f2800a | 58 | |
JKleinRot | 1:e5e1eb9d0025 | 59 | |
JKleinRot | 6:3b6fad529520 | 60 | int puls_arm1_home = 363; //Aantal pulsen die de encoder moet hebben geteld wanneer arm 1 op de thuispositie is |
JKleinRot | 6:3b6fad529520 | 61 | int puls_arm2_home = 787; //Aantal pulsen die de encoder moet hebben geteld wanneer arm 2 op de thuispositie is |
JKleinRot | 6:3b6fad529520 | 62 | float pwm_to_motor_arm1; //PWM output naar motor arm 1 |
JKleinRot | 6:3b6fad529520 | 63 | float pwm_to_motor_arm2; //PWM output naar motor arm 2 |
JKleinRot | 6:3b6fad529520 | 64 | float xbk; //Gemeten EMG waarde biceps in de kalibratie |
JKleinRot | 6:3b6fad529520 | 65 | int i = 0; // |
JKleinRot | 0:859c89785d3f | 66 | |
JKleinRot | 0:859c89785d3f | 67 | |
JKleinRot | 0:859c89785d3f | 68 | int main() { |
JKleinRot | 0:859c89785d3f | 69 | |
JKleinRot | 0:859c89785d3f | 70 | //PC baud rate instellen |
JKleinRot | 0:859c89785d3f | 71 | pc.baud(38400); //PC baud rate is 38400 bits/seconde |
JKleinRot | 0:859c89785d3f | 72 | |
JKleinRot | 0:859c89785d3f | 73 | //Aanzetten |
JKleinRot | 4:69540b9c0646 | 74 | if (rust == false && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){ |
JKleinRot | 0:859c89785d3f | 75 | lcd_r1 = " BMT M9 GR. 4 "; //Tekst op eerste regel van LCD scherm |
JKleinRot | 0:859c89785d3f | 76 | lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst op tweede regel van LCD scherm |
JKleinRot | 0:859c89785d3f | 77 | wait(2); //Twee seconden wachten |
JKleinRot | 0:859c89785d3f | 78 | pc.printf("Aanzetten compleet"); //Tekst voor controle Aanzetten |
JKleinRot | 0:859c89785d3f | 79 | rust = true; //Rust wordt waar zodat door kan worden gegaan naar het volgende deel |
JKleinRot | 0:859c89785d3f | 80 | } |
JKleinRot | 1:e5e1eb9d0025 | 81 | |
JKleinRot | 1:e5e1eb9d0025 | 82 | |
JKleinRot | 1:e5e1eb9d0025 | 83 | |
JKleinRot | 1:e5e1eb9d0025 | 84 | //Arm 1 naar thuispositie |
JKleinRot | 4:69540b9c0646 | 85 | if (rust == true && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){ |
JKleinRot | 1:e5e1eb9d0025 | 86 | wait(1); //Een seconde wachten |
JKleinRot | 1:e5e1eb9d0025 | 87 | |
JKleinRot | 1:e5e1eb9d0025 | 88 | ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken |
JKleinRot | 1:e5e1eb9d0025 | 89 | |
JKleinRot | 4:69540b9c0646 | 90 | while(rust == true && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false) { |
JKleinRot | 6:3b6fad529520 | 91 | while(regelaar_ticker_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel |
JKleinRot | 6:3b6fad529520 | 92 | regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 1:e5e1eb9d0025 | 93 | |
JKleinRot | 6:3b6fad529520 | 94 | pwm_to_motor_arm1 = (puls_arm1_home - puls_motor_arm1.getPosition())*KP; //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor |
JKleinRot | 6:3b6fad529520 | 95 | keep_in_range(&pwm_to_motor_arm1, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle) |
JKleinRot | 1:e5e1eb9d0025 | 96 | |
JKleinRot | 6:3b6fad529520 | 97 | if (pwm_to_motor_arm1 > 0){ //Als PWM is positief, dan richting 1 |
JKleinRot | 1:e5e1eb9d0025 | 98 | dir_motor_arm1.write(1); |
JKleinRot | 1:e5e1eb9d0025 | 99 | } |
JKleinRot | 6:3b6fad529520 | 100 | else { //Anders richting nul |
JKleinRot | 1:e5e1eb9d0025 | 101 | dir_motor_arm1.write(0); |
JKleinRot | 1:e5e1eb9d0025 | 102 | } |
JKleinRot | 6:3b6fad529520 | 103 | pwm_motor_arm1.write(abs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM |
JKleinRot | 2:0cb899f2800a | 104 | |
JKleinRot | 6:3b6fad529520 | 105 | if (pwm_to_motor_arm1 == 0) { //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer) |
JKleinRot | 3:adc3052039e7 | 106 | kalibratie_positie_arm1 = true; |
JKleinRot | 6:3b6fad529520 | 107 | pc.printf("Arm 1 naar thuispositie compleet"); //Tekst voor controle Arm 1 naar thuispositie |
JKleinRot | 2:0cb899f2800a | 108 | } |
JKleinRot | 6:3b6fad529520 | 109 | wait(1); //Een seconde wachten |
JKleinRot | 2:0cb899f2800a | 110 | |
JKleinRot | 1:e5e1eb9d0025 | 111 | } |
JKleinRot | 3:adc3052039e7 | 112 | } |
JKleinRot | 3:adc3052039e7 | 113 | |
JKleinRot | 3:adc3052039e7 | 114 | //Arm 2 naar thuispositie |
JKleinRot | 4:69540b9c0646 | 115 | if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){ |
JKleinRot | 3:adc3052039e7 | 116 | wait(1); //Een seconde wachten |
JKleinRot | 3:adc3052039e7 | 117 | |
JKleinRot | 3:adc3052039e7 | 118 | //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken |
JKleinRot | 3:adc3052039e7 | 119 | |
JKleinRot | 4:69540b9c0646 | 120 | while(rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false) { |
JKleinRot | 6:3b6fad529520 | 121 | while(regelaar_ticker_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel |
JKleinRot | 6:3b6fad529520 | 122 | regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 3:adc3052039e7 | 123 | |
JKleinRot | 6:3b6fad529520 | 124 | pwm_to_motor_arm2 = (puls_arm2_home - puls_motor_arm2.getPosition())*KP; //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor |
JKleinRot | 6:3b6fad529520 | 125 | keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle) |
JKleinRot | 3:adc3052039e7 | 126 | |
JKleinRot | 6:3b6fad529520 | 127 | if (pwm_to_motor_arm2 > 0){ //Als PWM is positief, dan richting 1 |
JKleinRot | 3:adc3052039e7 | 128 | dir_motor_arm2.write(1); |
JKleinRot | 3:adc3052039e7 | 129 | } |
JKleinRot | 6:3b6fad529520 | 130 | else { //Anders richting nul |
JKleinRot | 3:adc3052039e7 | 131 | dir_motor_arm2.write(0); |
JKleinRot | 3:adc3052039e7 | 132 | } |
JKleinRot | 6:3b6fad529520 | 133 | pwm_motor_arm2.write(abs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM |
JKleinRot | 3:adc3052039e7 | 134 | |
JKleinRot | 6:3b6fad529520 | 135 | if (pwm_to_motor_arm2 == 0) { //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer) |
JKleinRot | 3:adc3052039e7 | 136 | kalibratie_positie_arm2 = true; |
JKleinRot | 6:3b6fad529520 | 137 | pc.printf("Arm 2 naar thuispositie compleet"); //Tekst voor controle Arm 2 naar thuispositie |
JKleinRot | 3:adc3052039e7 | 138 | } |
JKleinRot | 3:adc3052039e7 | 139 | } |
JKleinRot | 6:3b6fad529520 | 140 | wait(1); //Een seconde wachten |
JKleinRot | 6:3b6fad529520 | 141 | ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer |
JKleinRot | 3:adc3052039e7 | 142 | } |
JKleinRot | 4:69540b9c0646 | 143 | |
JKleinRot | 4:69540b9c0646 | 144 | //Ticker voor kalibratie |
JKleinRot | 4:69540b9c0646 | 145 | if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == true && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){ |
JKleinRot | 4:69540b9c0646 | 146 | |
JKleinRot | 6:3b6fad529520 | 147 | ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Ticker iedere zoveel seconden de flag laten opsteken |
JKleinRot | 6:3b6fad529520 | 148 | pc.printf("Ticker voor kalibratie compleet"); //Tekst voor controle Ticker voor kalibratie |
JKleinRot | 5:a1dcd605dd3d | 149 | |
JKleinRot | 5:a1dcd605dd3d | 150 | //5 seconden EMG biceps meten |
JKleinRot | 5:a1dcd605dd3d | 151 | |
JKleinRot | 6:3b6fad529520 | 152 | wait(1); //Een seconde wachten |
JKleinRot | 6:3b6fad529520 | 153 | lcd_r1 = " EMG kalibratie "; //Tekst op eerste regel van LCD scherm |
JKleinRot | 6:3b6fad529520 | 154 | lcd_r2 = " Span biceps aan"; //Tekst op tweede regel van LCD scherm |
JKleinRot | 6:3b6fad529520 | 155 | wait(2); //Twee seconden wachten |
JKleinRot | 5:a1dcd605dd3d | 156 | |
JKleinRot | 5:a1dcd605dd3d | 157 | for (i=0 ,i<5000, i++){ |
JKleinRot | 6:3b6fad529520 | 158 | while(setregelaar_EMG_flag != true) ; //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel |
JKleinRot | 6:3b6fad529520 | 159 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 5:a1dcd605dd3d | 160 | |
JKleinRot | 6:3b6fad529520 | 161 | xbk = EMG_bi.read(); //EMG signaal uitlezen |
JKleinRot | 5:a1dcd605dd3d | 162 | } |
JKleinRot | 5:a1dcd605dd3d | 163 | |
JKleinRot | 4:69540b9c0646 | 164 | } |
JKleinRot | 4:69540b9c0646 | 165 | |
JKleinRot | 4:69540b9c0646 | 166 | |
JKleinRot | 4:69540b9c0646 | 167 | |
JKleinRot | 4:69540b9c0646 | 168 | |
JKleinRot | 0:859c89785d3f | 169 | } |