2014-10-15 Arm 1 naar thuispositie. Eerste poging, fout in keep_in_range
Dependencies: Encoder MODSERIAL TextLCD mbed mbed-dsp
Diff: main.cpp
- Revision:
- 1:e5e1eb9d0025
- Parent:
- 0:859c89785d3f
- Child:
- 2:0cb899f2800a
--- a/main.cpp Wed Oct 15 12:40:36 2014 +0000 +++ b/main.cpp Wed Oct 15 14:37:28 2014 +0000 @@ -4,12 +4,19 @@ #include "TextLCD.h" //LCD scherm bibliotheek inladen, communicatie met LCD scherm //Constanten definiëren en waarde geven - +#define SAMPLETIME_REGELAAR 0.005 +#define KP 1 //Pinverdeling en naamgeving variabelen TextLCD lcd(PTD2, PTB8, PTB9, PTB10, PTB11, PTE2); //LCD scherm MODSERIAL pc(USBTX, USBRX); //PC +PwmOut pwm_motor_arm1(PTA5); //PWM naar motor arm 1 +DigitalOut dir_motor_arm1(PTD1); //Richting van motor arm 1 +Encoder puls_motor_arm1(PTD0, PTC9); //Encoder pulsen tellen van motor arm 1 + +Ticker ticker_regelaar; + //Gedefinieerde datatypen en naamgeving bool rust = false; //Bool voor controleren volgorde in programma bool kalibratie_positie = false; //Bool voor controleren volgorde in programma @@ -18,6 +25,16 @@ char *lcd_r1 = new char[16]; //Char voor tekst op eerste regel LCD scherm char *lcd_r2 = new char[16]; //Char voor tekst op tweede regel LCD scherm +volatile bool regelaar_ticker_flag; +void setregelaar_ticker_flag(){ + regelaar_ticker_flag = true; +} + +void keep_in_range(float * in, float min, float max); + +int puls_arm1_home = 363; +float pwm_to_motor_arm1; + //Beginwaarden voor variabelen @@ -34,4 +51,32 @@ pc.printf("Aanzetten compleet"); //Tekst voor controle Aanzetten rust = true; //Rust wordt waar zodat door kan worden gegaan naar het volgende deel } + + + + //Arm 1 naar thuispositie + if (rust == true && kalibratie_positie == false && kalibratie_EMG == false){ + wait(1); //Een seconde wachten + + ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken + + while(1) { + while(regelaar_ticker_flag != true) ; + regelaar_ticker_flag = false; + + pwm_to_motor_arm1 = (puls_arm1_home - puls_motor_arm1.getPosition())*KP; + keep_in_range(&pwm_to_motor_arm1, -1, 1); + + if (pwm_to_motor_arm1 > 0){ + dir_motor_arm1.write(1); + } + else { + dir_motor_arm1.write(0); + } + pwm_motor_arm1.write(abs(pwm_to_motor_arm1)); + + wait(1); + pc.printf("Arm 1 naar thuispositie compleet"); + } + } } \ No newline at end of file