2014-10-15 Arm 1 naar thuispositie. Eerste poging, fout in keep_in_range
Dependencies: Encoder MODSERIAL TextLCD mbed mbed-dsp
main.cpp
- Committer:
- JKleinRot
- Date:
- 2014-10-16
- Revision:
- 4:69540b9c0646
- Parent:
- 3:adc3052039e7
- Child:
- 5:a1dcd605dd3d
File content as of revision 4:69540b9c0646:
#include "mbed.h" //Mbed bibliotheek inladen, standaard functies #include "MODSERIAL.h" //MODSERIAL bibliotheek inladen, communicatie met PC #include "encoder.h" //Encoder bibliotheek inladen, communicatie met encoder #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 #define SAMPLETIME_EMG 0.005 //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 PwmOut pwm_motor_arm2(PTA12); DigitalOut dir_motor_arm2(PTD3); Encoder puls_motor_arm2(PTD4, PTC8); AnalogIn EMG_bi(PTB1); Ticker ticker_regelaar; Ticker ticker_EMG; //Gedefinieerde datatypen en naamgeving bool rust = false; //Bool voor controleren volgorde in programma bool kalibratie_positie_arm1 = false; //Bool voor controleren volgorde in programma bool kalibratie_positie_arm2 = false; bool kalibratie_EMG_bi = false; //Bool voor controleren volgorde in programma bool kalibratie_EMG_tri = false; 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; } volatile bool regelaar_EMG_flag; void setregelaar_EMG_flag(){ regelaar_EMG_flag = true; } void keep_in_range(float * in, float min, float max){ if (*in < min){ *in = min; } if (*in > max){ *in = max; } else { *in = *in; } } int puls_arm1_home = 363; int puls_arm2_home = 787; float pwm_to_motor_arm1; float pwm_to_motor_arm2; //Beginwaarden voor variabelen int main() { //PC baud rate instellen pc.baud(38400); //PC baud rate is 38400 bits/seconde //Aanzetten if (rust == false && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){ lcd_r1 = " BMT M9 GR. 4 "; //Tekst op eerste regel van LCD scherm lcd_r2 = "Hoi! Ik ben PIPO"; //Tekst op tweede regel van LCD scherm wait(2); //Twee seconden wachten 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_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){ wait(1); //Een seconde wachten ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken while(rust == true && kalibratie_positie_arm1 == false && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false) { 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)); if (pwm_to_motor_arm1 == 0) { kalibratie_positie_arm1 = true; pc.printf("Arm 1 naar thuispositie compleet"); } wait(1); } } //Arm 2 naar thuispositie if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){ wait(1); //Een seconde wachten //ticker_regelaar.attach(setregelaar_ticker_flag,SAMPLETIME_REGELAAR); //Ticker iedere zoveel seconde de flag op laten steken while(rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == false && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false) { while(regelaar_ticker_flag != true) ; regelaar_ticker_flag = false; pwm_to_motor_arm2 = (puls_arm2_home - puls_motor_arm2.getPosition())*KP; keep_in_range(&pwm_to_motor_arm2, -1, 1); if (pwm_to_motor_arm2 > 0){ dir_motor_arm2.write(1); } else { dir_motor_arm2.write(0); } pwm_motor_arm2.write(abs(pwm_to_motor_arm2)); if (pwm_to_motor_arm2 == 0) { kalibratie_positie_arm2 = true; pc.printf("Arm 2 naar thuispositie compleet"); } wait(1); } } //Ticker voor kalibratie if (rust == true && kalibratie_positie_arm1 == true && kalibratie_positie_arm2 == true && kalibratie_EMG_bi == false && kalibratie_EMG_tri == false){ ticker_EMG.attach(setregelaar_EMG_flag,SAMPLETIME_EMG); pc.printf("Ticker voor kalibratie compleet"); } }