![](/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@9:454e7da8ab65, 2014-10-20 (annotated)
- Committer:
- JKleinRot
- Date:
- Mon Oct 20 20:06:06 2014 +0000
- Revision:
- 9:454e7da8ab65
- Parent:
- 7:dd3cba61b34b
- Child:
- 10:52b22bff409a
2014-10-20 Kalibratie van de arm werkt nog niet met de motor eraan. Wel alles in cases gezet zodat het overzichtelijk is en de abs in fabs veranderd zodat het een float blijft.
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 | 9:454e7da8ab65 | 8 | #define KP 0.001 //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 | 9:454e7da8ab65 | 16 | DigitalOut dir_motor_arm1(PTA4); //Richting van motor arm 1 |
JKleinRot | 9:454e7da8ab65 | 17 | Encoder puls_motor_arm1(PTD2, PTD0); //Encoder pulsen tellen van motor arm 1 |
JKleinRot | 9:454e7da8ab65 | 18 | PwmOut pwm_motor_arm2(PTC8); //PWM naar motor arm 2 |
JKleinRot | 9:454e7da8ab65 | 19 | DigitalOut dir_motor_arm2(PTC9); //Ricting van motor arm 2 |
JKleinRot | 9:454e7da8ab65 | 20 | Encoder puls_motor_arm2(PTD5, PTA13); //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 | 9:454e7da8ab65 | 27 | //States definiëren |
JKleinRot | 9:454e7da8ab65 | 28 | enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, KALIBRATIE_BICEPS, KALIBRATIE_TRICEPS}; //Alle states benoemen, ieder krijgt een getal beginnend met 0 |
JKleinRot | 9:454e7da8ab65 | 29 | uint8_t state = RUST; //State is rust aan het begin |
JKleinRot | 9:454e7da8ab65 | 30 | |
JKleinRot | 6:3b6fad529520 | 31 | //Gedefinieerde datatypen en naamgeving en beginwaarden |
JKleinRot | 6:3b6fad529520 | 32 | char *lcd_r1 = new char[16]; //Char voor tekst op eerste regel LCD scherm |
JKleinRot | 6:3b6fad529520 | 33 | char *lcd_r2 = new char[16]; //Char voor tekst op tweede regel LCD scherm |
JKleinRot | 0:859c89785d3f | 34 | |
JKleinRot | 7:dd3cba61b34b | 35 | int puls_arm1_home = 363; //Aantal pulsen die de encoder moet hebben geteld wanneer arm 1 op de thuispositie is |
JKleinRot | 7:dd3cba61b34b | 36 | int puls_arm2_home = 787; //Aantal pulsen die de encoder moet hebben geteld wanneer arm 2 op de thuispositie is |
JKleinRot | 7:dd3cba61b34b | 37 | float pwm_to_motor_arm1; //PWM output naar motor arm 1 |
JKleinRot | 7:dd3cba61b34b | 38 | float pwm_to_motor_arm2; //PWM output naar motor arm 2 |
JKleinRot | 7:dd3cba61b34b | 39 | float xbk; //Gemeten EMG waarde biceps in de kalibratie |
JKleinRot | 7:dd3cba61b34b | 40 | int i; // |
JKleinRot | 7:dd3cba61b34b | 41 | |
JKleinRot | 6:3b6fad529520 | 42 | volatile bool regelaar_ticker_flag; //Definiëren flag als bool die verandert kan worden in programma |
JKleinRot | 6:3b6fad529520 | 43 | void setregelaar_ticker_flag(){ //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true |
JKleinRot | 6:3b6fad529520 | 44 | regelaar_ticker_flag = true; |
JKleinRot | 1:e5e1eb9d0025 | 45 | } |
JKleinRot | 1:e5e1eb9d0025 | 46 | |
JKleinRot | 6:3b6fad529520 | 47 | volatile bool regelaar_EMG_flag; //Definiëren flag als bool die verandert kan worden in programma |
JKleinRot | 6:3b6fad529520 | 48 | void setregelaar_EMG_flag(){ //Setregelaar_EMG_flag zet de regelaar_EMG_flag op true |
JKleinRot | 6:3b6fad529520 | 49 | regelaar_EMG_flag = true; |
JKleinRot | 4:69540b9c0646 | 50 | } |
JKleinRot | 4:69540b9c0646 | 51 | |
JKleinRot | 6:3b6fad529520 | 52 | 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 | 53 | if (*in < min){ //Als de waarde kleiner is als het minimum wordt de waarde het minimum |
JKleinRot | 2:0cb899f2800a | 54 | *in = min; |
JKleinRot | 2:0cb899f2800a | 55 | } |
JKleinRot | 6:3b6fad529520 | 56 | if (*in > max){ //Als de waarde groter is dan het maximum is de waarde het maximum |
JKleinRot | 2:0cb899f2800a | 57 | *in = max; |
JKleinRot | 2:0cb899f2800a | 58 | } |
JKleinRot | 6:3b6fad529520 | 59 | else { //In alle andere gevallen is de waarde de waarde zelf |
JKleinRot | 2:0cb899f2800a | 60 | *in = *in; |
JKleinRot | 2:0cb899f2800a | 61 | } |
JKleinRot | 2:0cb899f2800a | 62 | } |
JKleinRot | 1:e5e1eb9d0025 | 63 | |
JKleinRot | 7:dd3cba61b34b | 64 | void arm1_naar_thuispositie(){ |
JKleinRot | 7:dd3cba61b34b | 65 | 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 | 7:dd3cba61b34b | 66 | keep_in_range(&pwm_to_motor_arm1, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle) |
JKleinRot | 7:dd3cba61b34b | 67 | |
JKleinRot | 7:dd3cba61b34b | 68 | if (pwm_to_motor_arm1 > 0){ //Als PWM is positief, dan richting 1 |
JKleinRot | 7:dd3cba61b34b | 69 | dir_motor_arm1.write(1); |
JKleinRot | 7:dd3cba61b34b | 70 | } |
JKleinRot | 7:dd3cba61b34b | 71 | else { //Anders richting nul |
JKleinRot | 7:dd3cba61b34b | 72 | dir_motor_arm1.write(0); |
JKleinRot | 7:dd3cba61b34b | 73 | } |
JKleinRot | 9:454e7da8ab65 | 74 | pwm_motor_arm1.write(fabs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM |
JKleinRot | 9:454e7da8ab65 | 75 | pc.printf("pulsmotorgetposition %f\n\r", puls_motor_arm1.getPosition()); |
JKleinRot | 9:454e7da8ab65 | 76 | pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm1); |
JKleinRot | 7:dd3cba61b34b | 77 | |
JKleinRot | 7:dd3cba61b34b | 78 | 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 | 9:454e7da8ab65 | 79 | state = KALIBRATIE_ARM2; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel |
JKleinRot | 9:454e7da8ab65 | 80 | pc.printf("KALIBRATIE_ARM1 afgerond"); //Tekst voor controle Arm 1 naar thuispositie |
JKleinRot | 7:dd3cba61b34b | 81 | } |
JKleinRot | 7:dd3cba61b34b | 82 | } |
JKleinRot | 7:dd3cba61b34b | 83 | |
JKleinRot | 7:dd3cba61b34b | 84 | void arm2_naar_thuispositie(){ |
JKleinRot | 7:dd3cba61b34b | 85 | 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 | 7:dd3cba61b34b | 86 | keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle) |
JKleinRot | 7:dd3cba61b34b | 87 | |
JKleinRot | 7:dd3cba61b34b | 88 | if (pwm_to_motor_arm2 > 0){ //Als PWM is positief, dan richting 1 |
JKleinRot | 7:dd3cba61b34b | 89 | dir_motor_arm2.write(1); |
JKleinRot | 7:dd3cba61b34b | 90 | } |
JKleinRot | 7:dd3cba61b34b | 91 | else { //Anders richting nul |
JKleinRot | 7:dd3cba61b34b | 92 | dir_motor_arm2.write(0); |
JKleinRot | 7:dd3cba61b34b | 93 | } |
JKleinRot | 9:454e7da8ab65 | 94 | pwm_motor_arm2.write(fabs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM |
JKleinRot | 7:dd3cba61b34b | 95 | |
JKleinRot | 7:dd3cba61b34b | 96 | 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 | 9:454e7da8ab65 | 97 | state = KALIBRATIE_BICEPS; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel |
JKleinRot | 9:454e7da8ab65 | 98 | pc.printf("KALIBRATIE_ARM2 afgerond"); //Tekst voor controle Arm 2 naar thuispositie |
JKleinRot | 7:dd3cba61b34b | 99 | } |
JKleinRot | 7:dd3cba61b34b | 100 | } |
JKleinRot | 0:859c89785d3f | 101 | |
JKleinRot | 0:859c89785d3f | 102 | |
JKleinRot | 0:859c89785d3f | 103 | int main() { |
JKleinRot | 0:859c89785d3f | 104 | |
JKleinRot | 0:859c89785d3f | 105 | //PC baud rate instellen |
JKleinRot | 0:859c89785d3f | 106 | pc.baud(38400); //PC baud rate is 38400 bits/seconde |
JKleinRot | 9:454e7da8ab65 | 107 | |
JKleinRot | 9:454e7da8ab65 | 108 | switch(state) //Switch benoemen, zorgt ervoor dat in de goede volgorde de dingen worden doorlopen, aan einde een case wordt de state de naam van de nieuwe case |
JKleinRot | 9:454e7da8ab65 | 109 | { |
JKleinRot | 0:859c89785d3f | 110 | |
JKleinRot | 9:454e7da8ab65 | 111 | case RUST: //Aanzetten |
JKleinRot | 9:454e7da8ab65 | 112 | { |
JKleinRot | 0:859c89785d3f | 113 | lcd_r1 = " BMT M9 GR. 4 "; //Tekst op eerste regel van LCD scherm |
JKleinRot | 0:859c89785d3f | 114 | lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst op tweede regel van LCD scherm |
JKleinRot | 0:859c89785d3f | 115 | wait(2); //Twee seconden wachten |
JKleinRot | 9:454e7da8ab65 | 116 | pc.printf("RUST afgerond"); //Tekst voor controle Aanzetten |
JKleinRot | 9:454e7da8ab65 | 117 | state = KALIBRATIE_ARM1; //State wordt kalibratie arm 1, zo door naar volgende onderdeel |
JKleinRot | 9:454e7da8ab65 | 118 | break; //Stopt acties in deze case |
JKleinRot | 0:859c89785d3f | 119 | } |
JKleinRot | 1:e5e1eb9d0025 | 120 | |
JKleinRot | 9:454e7da8ab65 | 121 | case KALIBRATIE_ARM1: //Arm 1 naar thuispositie |
JKleinRot | 9:454e7da8ab65 | 122 | { |
JKleinRot | 1:e5e1eb9d0025 | 123 | wait(1); //Een seconde wachten |
JKleinRot | 1:e5e1eb9d0025 | 124 | |
JKleinRot | 1:e5e1eb9d0025 | 125 | ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken |
JKleinRot | 1:e5e1eb9d0025 | 126 | |
JKleinRot | 9:454e7da8ab65 | 127 | while(state == KALIBRATIE_ARM1){ |
JKleinRot | 6:3b6fad529520 | 128 | 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 | 129 | regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 1:e5e1eb9d0025 | 130 | |
JKleinRot | 7:dd3cba61b34b | 131 | arm1_naar_thuispositie(); //Voer acties uit om arm 1 naar thuispositie te krijgen |
JKleinRot | 9:454e7da8ab65 | 132 | } |
JKleinRot | 9:454e7da8ab65 | 133 | wait(1); //Een seconde wachten |
JKleinRot | 9:454e7da8ab65 | 134 | break; //Stopt acties in deze case |
JKleinRot | 3:adc3052039e7 | 135 | } |
JKleinRot | 3:adc3052039e7 | 136 | |
JKleinRot | 9:454e7da8ab65 | 137 | case KALIBRATIE_ARM2: //Arm 2 naar thuispositie |
JKleinRot | 9:454e7da8ab65 | 138 | { |
JKleinRot | 3:adc3052039e7 | 139 | wait(1); //Een seconde wachten |
JKleinRot | 3:adc3052039e7 | 140 | |
JKleinRot | 3:adc3052039e7 | 141 | //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken |
JKleinRot | 3:adc3052039e7 | 142 | |
JKleinRot | 9:454e7da8ab65 | 143 | while(state == KALIBRATIE_ARM2){ |
JKleinRot | 6:3b6fad529520 | 144 | 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 | 145 | regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 3:adc3052039e7 | 146 | |
JKleinRot | 7:dd3cba61b34b | 147 | arm2_naar_thuispositie(); //Voer acties uit om arm 2 naar thuispositie te krijgen |
JKleinRot | 3:adc3052039e7 | 148 | } |
JKleinRot | 6:3b6fad529520 | 149 | wait(1); //Een seconde wachten |
JKleinRot | 9:454e7da8ab65 | 150 | ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer |
JKleinRot | 9:454e7da8ab65 | 151 | break; //Stopt acties in deze case |
JKleinRot | 3:adc3052039e7 | 152 | } |
JKleinRot | 4:69540b9c0646 | 153 | |
JKleinRot | 9:454e7da8ab65 | 154 | case KALIBRATIE_BICEPS: //Kalibratie EMG signaal biceps |
JKleinRot | 9:454e7da8ab65 | 155 | { |
JKleinRot | 9:454e7da8ab65 | 156 | wait(1); //Een seconde wachten |
JKleinRot | 9:454e7da8ab65 | 157 | |
JKleinRot | 9:454e7da8ab65 | 158 | ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Ticker iedere zoveel seconden de flag laten opsteken |
JKleinRot | 4:69540b9c0646 | 159 | |
JKleinRot | 6:3b6fad529520 | 160 | pc.printf("Ticker voor kalibratie compleet"); //Tekst voor controle Ticker voor kalibratie |
JKleinRot | 5:a1dcd605dd3d | 161 | |
JKleinRot | 5:a1dcd605dd3d | 162 | //5 seconden EMG biceps meten |
JKleinRot | 5:a1dcd605dd3d | 163 | |
JKleinRot | 6:3b6fad529520 | 164 | wait(1); //Een seconde wachten |
JKleinRot | 6:3b6fad529520 | 165 | lcd_r1 = " EMG kalibratie "; //Tekst op eerste regel van LCD scherm |
JKleinRot | 6:3b6fad529520 | 166 | lcd_r2 = " Span biceps aan"; //Tekst op tweede regel van LCD scherm |
JKleinRot | 5:a1dcd605dd3d | 167 | |
JKleinRot | 7:dd3cba61b34b | 168 | for (i=0; i<1000; i++){ |
JKleinRot | 7:dd3cba61b34b | 169 | while(regelaar_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 | 170 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 5:a1dcd605dd3d | 171 | |
JKleinRot | 6:3b6fad529520 | 172 | xbk = EMG_bi.read(); //EMG signaal uitlezen |
JKleinRot | 5:a1dcd605dd3d | 173 | } |
JKleinRot | 5:a1dcd605dd3d | 174 | |
JKleinRot | 4:69540b9c0646 | 175 | } |
JKleinRot | 4:69540b9c0646 | 176 | |
JKleinRot | 4:69540b9c0646 | 177 | |
JKleinRot | 9:454e7da8ab65 | 178 | } |
JKleinRot | 9:454e7da8ab65 | 179 | |
JKleinRot | 9:454e7da8ab65 | 180 | |
JKleinRot | 4:69540b9c0646 | 181 | |
JKleinRot | 4:69540b9c0646 | 182 | |
JKleinRot | 0:859c89785d3f | 183 | } |