2014-10-15 Arm 1 naar thuispositie. Eerste poging, fout in keep_in_range
Dependencies: Encoder MODSERIAL TextLCD mbed mbed-dsp
main.cpp@1:e5e1eb9d0025, 2014-10-15 (annotated)
- Committer:
- JKleinRot
- Date:
- Wed Oct 15 14:37:28 2014 +0000
- Revision:
- 1:e5e1eb9d0025
- Parent:
- 0:859c89785d3f
- Child:
- 2:0cb899f2800a
2014-10-15 Arm 1 naar thuispositie. Eerste poging, fout in keep_in_range
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 | 1:e5e1eb9d0025 | 7 | #define SAMPLETIME_REGELAAR 0.005 |
JKleinRot | 1:e5e1eb9d0025 | 8 | #define KP 1 |
JKleinRot | 0:859c89785d3f | 9 | |
JKleinRot | 0:859c89785d3f | 10 | //Pinverdeling en naamgeving variabelen |
JKleinRot | 0:859c89785d3f | 11 | TextLCD lcd(PTD2, PTB8, PTB9, PTB10, PTB11, PTE2); //LCD scherm |
JKleinRot | 0:859c89785d3f | 12 | MODSERIAL pc(USBTX, USBRX); //PC |
JKleinRot | 0:859c89785d3f | 13 | |
JKleinRot | 1:e5e1eb9d0025 | 14 | PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1 |
JKleinRot | 1:e5e1eb9d0025 | 15 | DigitalOut dir_motor_arm1(PTD1); //Richting van motor arm 1 |
JKleinRot | 1:e5e1eb9d0025 | 16 | Encoder puls_motor_arm1(PTD0, PTC9); //Encoder pulsen tellen van motor arm 1 |
JKleinRot | 1:e5e1eb9d0025 | 17 | |
JKleinRot | 1:e5e1eb9d0025 | 18 | Ticker ticker_regelaar; |
JKleinRot | 1:e5e1eb9d0025 | 19 | |
JKleinRot | 0:859c89785d3f | 20 | //Gedefinieerde datatypen en naamgeving |
JKleinRot | 0:859c89785d3f | 21 | bool rust = false; //Bool voor controleren volgorde in programma |
JKleinRot | 0:859c89785d3f | 22 | bool kalibratie_positie = false; //Bool voor controleren volgorde in programma |
JKleinRot | 0:859c89785d3f | 23 | bool kalibratie_EMG = false; //Bool voor controleren volgorde in programma |
JKleinRot | 0:859c89785d3f | 24 | |
JKleinRot | 0:859c89785d3f | 25 | char *lcd_r1 = new char[16]; //Char voor tekst op eerste regel LCD scherm |
JKleinRot | 0:859c89785d3f | 26 | char *lcd_r2 = new char[16]; //Char voor tekst op tweede regel LCD scherm |
JKleinRot | 0:859c89785d3f | 27 | |
JKleinRot | 1:e5e1eb9d0025 | 28 | volatile bool regelaar_ticker_flag; |
JKleinRot | 1:e5e1eb9d0025 | 29 | void setregelaar_ticker_flag(){ |
JKleinRot | 1:e5e1eb9d0025 | 30 | regelaar_ticker_flag = true; |
JKleinRot | 1:e5e1eb9d0025 | 31 | } |
JKleinRot | 1:e5e1eb9d0025 | 32 | |
JKleinRot | 1:e5e1eb9d0025 | 33 | void keep_in_range(float * in, float min, float max); |
JKleinRot | 1:e5e1eb9d0025 | 34 | |
JKleinRot | 1:e5e1eb9d0025 | 35 | int puls_arm1_home = 363; |
JKleinRot | 1:e5e1eb9d0025 | 36 | float pwm_to_motor_arm1; |
JKleinRot | 1:e5e1eb9d0025 | 37 | |
JKleinRot | 0:859c89785d3f | 38 | //Beginwaarden voor variabelen |
JKleinRot | 0:859c89785d3f | 39 | |
JKleinRot | 0:859c89785d3f | 40 | |
JKleinRot | 0:859c89785d3f | 41 | int main() { |
JKleinRot | 0:859c89785d3f | 42 | |
JKleinRot | 0:859c89785d3f | 43 | //PC baud rate instellen |
JKleinRot | 0:859c89785d3f | 44 | pc.baud(38400); //PC baud rate is 38400 bits/seconde |
JKleinRot | 0:859c89785d3f | 45 | |
JKleinRot | 0:859c89785d3f | 46 | //Aanzetten |
JKleinRot | 0:859c89785d3f | 47 | if (rust == false && kalibratie_positie == false && kalibratie_EMG == false){ |
JKleinRot | 0:859c89785d3f | 48 | lcd_r1 = " BMT M9 GR. 4 "; //Tekst op eerste regel van LCD scherm |
JKleinRot | 0:859c89785d3f | 49 | lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst op tweede regel van LCD scherm |
JKleinRot | 0:859c89785d3f | 50 | wait(2); //Twee seconden wachten |
JKleinRot | 0:859c89785d3f | 51 | pc.printf("Aanzetten compleet"); //Tekst voor controle Aanzetten |
JKleinRot | 0:859c89785d3f | 52 | rust = true; //Rust wordt waar zodat door kan worden gegaan naar het volgende deel |
JKleinRot | 0:859c89785d3f | 53 | } |
JKleinRot | 1:e5e1eb9d0025 | 54 | |
JKleinRot | 1:e5e1eb9d0025 | 55 | |
JKleinRot | 1:e5e1eb9d0025 | 56 | |
JKleinRot | 1:e5e1eb9d0025 | 57 | //Arm 1 naar thuispositie |
JKleinRot | 1:e5e1eb9d0025 | 58 | if (rust == true && kalibratie_positie == false && kalibratie_EMG == false){ |
JKleinRot | 1:e5e1eb9d0025 | 59 | wait(1); //Een seconde wachten |
JKleinRot | 1:e5e1eb9d0025 | 60 | |
JKleinRot | 1:e5e1eb9d0025 | 61 | ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken |
JKleinRot | 1:e5e1eb9d0025 | 62 | |
JKleinRot | 1:e5e1eb9d0025 | 63 | while(1) { |
JKleinRot | 1:e5e1eb9d0025 | 64 | while(regelaar_ticker_flag != true) ; |
JKleinRot | 1:e5e1eb9d0025 | 65 | regelaar_ticker_flag = false; |
JKleinRot | 1:e5e1eb9d0025 | 66 | |
JKleinRot | 1:e5e1eb9d0025 | 67 | pwm_to_motor_arm1 = (puls_arm1_home - puls_motor_arm1.getPosition())*KP; |
JKleinRot | 1:e5e1eb9d0025 | 68 | keep_in_range(&pwm_to_motor_arm1, -1, 1); |
JKleinRot | 1:e5e1eb9d0025 | 69 | |
JKleinRot | 1:e5e1eb9d0025 | 70 | if (pwm_to_motor_arm1 > 0){ |
JKleinRot | 1:e5e1eb9d0025 | 71 | dir_motor_arm1.write(1); |
JKleinRot | 1:e5e1eb9d0025 | 72 | } |
JKleinRot | 1:e5e1eb9d0025 | 73 | else { |
JKleinRot | 1:e5e1eb9d0025 | 74 | dir_motor_arm1.write(0); |
JKleinRot | 1:e5e1eb9d0025 | 75 | } |
JKleinRot | 1:e5e1eb9d0025 | 76 | pwm_motor_arm1.write(abs(pwm_to_motor_arm1)); |
JKleinRot | 1:e5e1eb9d0025 | 77 | |
JKleinRot | 1:e5e1eb9d0025 | 78 | wait(1); |
JKleinRot | 1:e5e1eb9d0025 | 79 | pc.printf("Arm 1 naar thuispositie compleet"); |
JKleinRot | 1:e5e1eb9d0025 | 80 | } |
JKleinRot | 1:e5e1eb9d0025 | 81 | } |
JKleinRot | 0:859c89785d3f | 82 | } |