2014-10-15 Arm 1 naar thuispositie. Eerste poging, fout in keep_in_range
Dependencies: Encoder MODSERIAL TextLCD mbed mbed-dsp
main.cpp@12:996f9f8e3acc, 2014-10-23 (annotated)
- Committer:
- JKleinRot
- Date:
- Thu Oct 23 11:46:37 2014 +0000
- Revision:
- 12:996f9f8e3acc
- Parent:
- 11:e9b906b5f572
- Child:
- 13:54ee98850a15
2014-10-23, protoshield gesoldeerd. Encoderpinnen andersom gezet van motor 1, werkt nu tot ticker kalibratie
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 | 11:e9b906b5f572 | 7 | #define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor |
JKleinRot | 11:e9b906b5f572 | 8 | #define KP 0.001 //Factor proprotionele regelaar |
JKleinRot | 11:e9b906b5f572 | 9 | #define SAMPLETIME_EMG 0.005 //Sampletijd ticker EMG meten |
JKleinRot | 11:e9b906b5f572 | 10 | #define PULS_ARM1_HOME_KALIBRATIE 180 //Aantal pulsen die de encoder moet tellen voordat de arm de goede positie heeft |
JKleinRot | 11:e9b906b5f572 | 11 | #define PULS_ARM2_HOME_KALIBRATIE 393 //Aantal pulsen die de encoder moet tellen voordat de arm de goede positie heeft |
JKleinRot | 0:859c89785d3f | 12 | |
JKleinRot | 0:859c89785d3f | 13 | //Pinverdeling en naamgeving variabelen |
JKleinRot | 0:859c89785d3f | 14 | TextLCD lcd(PTD2, PTB8, PTB9, PTB10, PTB11, PTE2); //LCD scherm |
JKleinRot | 0:859c89785d3f | 15 | MODSERIAL pc(USBTX, USBRX); //PC |
JKleinRot | 0:859c89785d3f | 16 | |
JKleinRot | 1:e5e1eb9d0025 | 17 | PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1 |
JKleinRot | 9:454e7da8ab65 | 18 | DigitalOut dir_motor_arm1(PTA4); //Richting van motor arm 1 |
JKleinRot | 12:996f9f8e3acc | 19 | Encoder puls_motor_arm1(PTD0, PTD2); //Encoder pulsen tellen van motor arm 1, (geel, wit) |
JKleinRot | 11:e9b906b5f572 | 20 | PwmOut pwm_motor_arm2(PTC8); //PWM naar motor arm 2 |
JKleinRot | 9:454e7da8ab65 | 21 | DigitalOut dir_motor_arm2(PTC9); //Ricting van motor arm 2 |
JKleinRot | 12:996f9f8e3acc | 22 | Encoder puls_motor_arm2(PTD5, PTA13); //Encoder pulsen tellen van motor arm 2, (geel, wit) |
JKleinRot | 6:3b6fad529520 | 23 | AnalogIn EMG_bi(PTB1); //Meten EMG signaal biceps |
JKleinRot | 12:996f9f8e3acc | 24 | //Blauw op 3,3 V en groen op GND |
JKleinRot | 1:e5e1eb9d0025 | 25 | |
JKleinRot | 6:3b6fad529520 | 26 | Ticker ticker_regelaar; //Ticker voor regelaar motor |
JKleinRot | 6:3b6fad529520 | 27 | Ticker ticker_EMG; //Ticker voor EMG meten |
JKleinRot | 1:e5e1eb9d0025 | 28 | |
JKleinRot | 9:454e7da8ab65 | 29 | //States definiëren |
JKleinRot | 9:454e7da8ab65 | 30 | enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, KALIBRATIE_BICEPS, KALIBRATIE_TRICEPS}; //Alle states benoemen, ieder krijgt een getal beginnend met 0 |
JKleinRot | 9:454e7da8ab65 | 31 | uint8_t state = RUST; //State is rust aan het begin |
JKleinRot | 9:454e7da8ab65 | 32 | |
JKleinRot | 6:3b6fad529520 | 33 | //Gedefinieerde datatypen en naamgeving en beginwaarden |
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 | 11:e9b906b5f572 | 37 | float pwm_to_motor_arm1; //PWM output naar motor arm 1 |
JKleinRot | 11:e9b906b5f572 | 38 | float pwm_to_motor_arm2; //PWM output naar motor arm 2 |
JKleinRot | 11:e9b906b5f572 | 39 | float xbk; //Gemeten EMG waarde biceps in de kalibratie |
JKleinRot | 11:e9b906b5f572 | 40 | int i; //Voor for-loop |
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 | 10:52b22bff409a | 43 | void setregelaar_ticker_flag() //Setregelaar_ticker_flag zet de regelaar_ticker_flag op true |
JKleinRot | 10:52b22bff409a | 44 | { |
JKleinRot | 10:52b22bff409a | 45 | regelaar_ticker_flag = true; |
JKleinRot | 1:e5e1eb9d0025 | 46 | } |
JKleinRot | 1:e5e1eb9d0025 | 47 | |
JKleinRot | 6:3b6fad529520 | 48 | volatile bool regelaar_EMG_flag; //Definiëren flag als bool die verandert kan worden in programma |
JKleinRot | 10:52b22bff409a | 49 | void setregelaar_EMG_flag() //Setregelaar_EMG_flag zet de regelaar_EMG_flag op true |
JKleinRot | 10:52b22bff409a | 50 | { |
JKleinRot | 10:52b22bff409a | 51 | regelaar_EMG_flag = true; |
JKleinRot | 4:69540b9c0646 | 52 | } |
JKleinRot | 4:69540b9c0646 | 53 | |
JKleinRot | 10:52b22bff409a | 54 | void keep_in_range(float * in, float min, float max) //Zorgt ervoor dat een getal niet buiten een bepaald minimum en maximum komt |
JKleinRot | 10:52b22bff409a | 55 | { |
JKleinRot | 10:52b22bff409a | 56 | if (*in < min) { //Als de waarde kleiner is als het minimum wordt de waarde het minimum |
JKleinRot | 2:0cb899f2800a | 57 | *in = min; |
JKleinRot | 2:0cb899f2800a | 58 | } |
JKleinRot | 10:52b22bff409a | 59 | if (*in > max) { //Als de waarde groter is dan het maximum is de waarde het maximum |
JKleinRot | 2:0cb899f2800a | 60 | *in = max; |
JKleinRot | 10:52b22bff409a | 61 | } else { //In alle andere gevallen is de waarde de waarde zelf |
JKleinRot | 2:0cb899f2800a | 62 | *in = *in; |
JKleinRot | 2:0cb899f2800a | 63 | } |
JKleinRot | 2:0cb899f2800a | 64 | } |
JKleinRot | 1:e5e1eb9d0025 | 65 | |
JKleinRot | 10:52b22bff409a | 66 | void arm1_naar_thuispositie() |
JKleinRot | 10:52b22bff409a | 67 | { |
JKleinRot | 11:e9b906b5f572 | 68 | pwm_to_motor_arm1 = (PULS_ARM1_HOME_KALIBRATIE - 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 | 69 | keep_in_range(&pwm_to_motor_arm1, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle) |
JKleinRot | 10:52b22bff409a | 70 | |
JKleinRot | 11:e9b906b5f572 | 71 | if (pwm_to_motor_arm1 > 0) { //Als PWM is positief, dan richting 1 |
JKleinRot | 10:52b22bff409a | 72 | dir_motor_arm1.write(1); |
JKleinRot | 11:e9b906b5f572 | 73 | } else { //Anders richting nul |
JKleinRot | 7:dd3cba61b34b | 74 | dir_motor_arm1.write(0); |
JKleinRot | 7:dd3cba61b34b | 75 | } |
JKleinRot | 11:e9b906b5f572 | 76 | pwm_motor_arm1.write(fabs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM |
JKleinRot | 10:52b22bff409a | 77 | pc.printf("pulsmotorgetposition %d ", puls_motor_arm1.getPosition()); |
JKleinRot | 9:454e7da8ab65 | 78 | pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm1); |
JKleinRot | 10:52b22bff409a | 79 | |
JKleinRot | 11:e9b906b5f572 | 80 | 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 | 11:e9b906b5f572 | 81 | state = KALIBRATIE_ARM2; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel |
JKleinRot | 11:e9b906b5f572 | 82 | pc.printf("KALIBRATIE_ARM1 afgerond"); //Tekst voor controle Arm 1 naar thuispositie |
JKleinRot | 10:52b22bff409a | 83 | } |
JKleinRot | 10:52b22bff409a | 84 | } |
JKleinRot | 7:dd3cba61b34b | 85 | |
JKleinRot | 10:52b22bff409a | 86 | void arm2_naar_thuispositie() |
JKleinRot | 10:52b22bff409a | 87 | { |
JKleinRot | 11:e9b906b5f572 | 88 | pwm_to_motor_arm2 = (PULS_ARM2_HOME_KALIBRATIE - 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 | 89 | keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle) |
JKleinRot | 10:52b22bff409a | 90 | |
JKleinRot | 11:e9b906b5f572 | 91 | if (pwm_to_motor_arm2 > 0) { //Als PWM is positief, dan richting 1 |
JKleinRot | 7:dd3cba61b34b | 92 | dir_motor_arm2.write(1); |
JKleinRot | 11:e9b906b5f572 | 93 | } else { //Anders richting nul |
JKleinRot | 7:dd3cba61b34b | 94 | dir_motor_arm2.write(0); |
JKleinRot | 7:dd3cba61b34b | 95 | } |
JKleinRot | 11:e9b906b5f572 | 96 | pwm_motor_arm2.write(fabs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM |
JKleinRot | 10:52b22bff409a | 97 | |
JKleinRot | 11:e9b906b5f572 | 98 | 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 | 11:e9b906b5f572 | 99 | state = KALIBRATIE_BICEPS; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel |
JKleinRot | 11:e9b906b5f572 | 100 | pc.printf("KALIBRATIE_ARM2 afgerond"); //Tekst voor controle Arm 2 naar thuispositie |
JKleinRot | 7:dd3cba61b34b | 101 | } |
JKleinRot | 7:dd3cba61b34b | 102 | } |
JKleinRot | 0:859c89785d3f | 103 | |
JKleinRot | 0:859c89785d3f | 104 | |
JKleinRot | 10:52b22bff409a | 105 | int main() |
JKleinRot | 10:52b22bff409a | 106 | { |
JKleinRot | 11:e9b906b5f572 | 107 | while(1) { //Oneindige while loop, anders wordt er na het uitvoeren van de eerste case niets meer gedaan |
JKleinRot | 10:52b22bff409a | 108 | //PC baud rate instellen |
JKleinRot | 10:52b22bff409a | 109 | pc.baud(38400); //PC baud rate is 38400 bits/seconde |
JKleinRot | 10:52b22bff409a | 110 | |
JKleinRot | 10:52b22bff409a | 111 | 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 | 10:52b22bff409a | 112 | |
JKleinRot | 11:e9b906b5f572 | 113 | case RUST: { //Aanzetten |
JKleinRot | 10:52b22bff409a | 114 | lcd_r1 = " BMT M9 GR. 4 "; //Tekst op eerste regel van LCD scherm |
JKleinRot | 10:52b22bff409a | 115 | lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst op tweede regel van LCD scherm |
JKleinRot | 10:52b22bff409a | 116 | wait(2); //Twee seconden wachten |
JKleinRot | 10:52b22bff409a | 117 | pc.printf("RUST afgerond"); //Tekst voor controle Aanzetten |
JKleinRot | 10:52b22bff409a | 118 | state = KALIBRATIE_ARM1; //State wordt kalibratie arm 1, zo door naar volgende onderdeel |
JKleinRot | 10:52b22bff409a | 119 | break; //Stopt acties in deze case |
JKleinRot | 10:52b22bff409a | 120 | } |
JKleinRot | 10:52b22bff409a | 121 | |
JKleinRot | 10:52b22bff409a | 122 | case KALIBRATIE_ARM1: { //Arm 1 naar thuispositie |
JKleinRot | 10:52b22bff409a | 123 | pc.printf("kalibratie-arm1"); |
JKleinRot | 10:52b22bff409a | 124 | wait(1); //Een seconde wachten |
JKleinRot | 10:52b22bff409a | 125 | ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken |
JKleinRot | 10:52b22bff409a | 126 | |
JKleinRot | 10:52b22bff409a | 127 | while(state == KALIBRATIE_ARM1) { |
JKleinRot | 10:52b22bff409a | 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 | 10:52b22bff409a | 129 | regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 10:52b22bff409a | 130 | |
JKleinRot | 10:52b22bff409a | 131 | arm1_naar_thuispositie(); //Voer acties uit om arm 1 naar thuispositie te krijgen |
JKleinRot | 10:52b22bff409a | 132 | } |
JKleinRot | 10:52b22bff409a | 133 | wait(1); //Een seconde wachten |
JKleinRot | 11:e9b906b5f572 | 134 | break; //Stopt acties in deze case |
JKleinRot | 10:52b22bff409a | 135 | } |
JKleinRot | 10:52b22bff409a | 136 | |
JKleinRot | 11:e9b906b5f572 | 137 | case KALIBRATIE_ARM2: { //Arm 2 naar thuispositie |
JKleinRot | 10:52b22bff409a | 138 | wait(1); //Een seconde wachten |
JKleinRot | 10:52b22bff409a | 139 | |
JKleinRot | 10:52b22bff409a | 140 | //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken |
JKleinRot | 10:52b22bff409a | 141 | |
JKleinRot | 10:52b22bff409a | 142 | while(state == KALIBRATIE_ARM2) { |
JKleinRot | 10:52b22bff409a | 143 | 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 | 10:52b22bff409a | 144 | regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 10:52b22bff409a | 145 | |
JKleinRot | 10:52b22bff409a | 146 | arm2_naar_thuispositie(); //Voer acties uit om arm 2 naar thuispositie te krijgen |
JKleinRot | 10:52b22bff409a | 147 | } |
JKleinRot | 11:e9b906b5f572 | 148 | wait(1); //Een seconde wachten |
JKleinRot | 11:e9b906b5f572 | 149 | ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer |
JKleinRot | 10:52b22bff409a | 150 | break; //Stopt acties in deze case |
JKleinRot | 10:52b22bff409a | 151 | } |
JKleinRot | 10:52b22bff409a | 152 | |
JKleinRot | 10:52b22bff409a | 153 | case KALIBRATIE_BICEPS: { //Kalibratie EMG signaal biceps |
JKleinRot | 10:52b22bff409a | 154 | wait(1); //Een seconde wachten |
JKleinRot | 10:52b22bff409a | 155 | |
JKleinRot | 10:52b22bff409a | 156 | ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); //Ticker iedere zoveel seconden de flag laten opsteken |
JKleinRot | 10:52b22bff409a | 157 | |
JKleinRot | 10:52b22bff409a | 158 | pc.printf("Ticker voor kalibratie compleet"); //Tekst voor controle Ticker voor kalibratie |
JKleinRot | 10:52b22bff409a | 159 | |
JKleinRot | 10:52b22bff409a | 160 | //5 seconden EMG biceps meten |
JKleinRot | 10:52b22bff409a | 161 | |
JKleinRot | 10:52b22bff409a | 162 | wait(1); //Een seconde wachten |
JKleinRot | 10:52b22bff409a | 163 | lcd_r1 = " EMG kalibratie "; //Tekst op eerste regel van LCD scherm |
JKleinRot | 10:52b22bff409a | 164 | lcd_r2 = " Span biceps aan"; //Tekst op tweede regel van LCD scherm |
JKleinRot | 10:52b22bff409a | 165 | |
JKleinRot | 10:52b22bff409a | 166 | for (i=0; i<1000; i++) { |
JKleinRot | 10:52b22bff409a | 167 | 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 | 10:52b22bff409a | 168 | regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan |
JKleinRot | 10:52b22bff409a | 169 | |
JKleinRot | 10:52b22bff409a | 170 | xbk = EMG_bi.read(); //EMG signaal uitlezen |
JKleinRot | 10:52b22bff409a | 171 | } |
JKleinRot | 10:52b22bff409a | 172 | break; |
JKleinRot | 10:52b22bff409a | 173 | |
JKleinRot | 10:52b22bff409a | 174 | } |
JKleinRot | 11:e9b906b5f572 | 175 | default: { //Default state, mocht er iets misgaan en het programma kan niet naar een volgende case |
JKleinRot | 11:e9b906b5f572 | 176 | state = RUST; //Als dat gebeurt wordt de state rust en begint hij weer vooraan |
JKleinRot | 10:52b22bff409a | 177 | } |
JKleinRot | 9:454e7da8ab65 | 178 | } |
JKleinRot | 10:52b22bff409a | 179 | pc.printf("state: %u\n",state); |
JKleinRot | 3:adc3052039e7 | 180 | } |
JKleinRot | 0:859c89785d3f | 181 | } |