Opgeschoonde code voor verslag
Dependencies: Encoder HIDScope MODSERIAL mbed
Fork of TotalCodegr13V2 by
Diff: main.cpp
- Revision:
- 8:b219ca30967f
- Parent:
- 7:22126f285d69
- Child:
- 9:888ed3c72795
diff -r 22126f285d69 -r b219ca30967f main.cpp --- a/main.cpp Thu Oct 22 13:55:28 2015 +0000 +++ b/main.cpp Thu Oct 22 15:07:20 2015 +0000 @@ -3,40 +3,36 @@ #include "HIDScope.h" #include "MODSERIAL.h" + +// pins DigitalOut motor1_direction(D4); PwmOut motor1_speed(D5); -PwmOut led(D9); DigitalIn button_1(PTC6); //counterclockwise DigitalIn button_2(PTA4); //clockwise -AnalogIn PotMeter1(A5); +AnalogIn PotMeter1(A4); AnalogIn EMG(A0); -//AnalogIn EMG_bicepsright (A1); -//AnalogIn EMG_legleft (A2); -//AnalogIn EMG_legright (A3); Ticker controller; Ticker ticker_regelaar; Ticker EMG_Filter; Ticker Scope; -//Ticker Encoder_Scope; -//Timer Timer_Calibration; Encoder motor1(D12,D13); HIDScope scope(3); +MODSERIAL pc(USBTX, USBRX); -MODSERIAL pc(USBTX, USBRX); + + +// Regelaar homeposition +#define SAMPLETIME_REGELAAR 0.005 volatile bool regelaar_ticker_flag; - -void setregelaar_ticker_flag() -{ +void setregelaar_ticker_flag(){ regelaar_ticker_flag = true; } -#define SAMPLETIME_REGELAAR 0.005 - //define states -enum state {HOME, MOVE_MOTOR, BACKTOHOMEPOSITION, STOP}; +enum state {HOME, MOVE_MOTOR_1, BACKTOHOMEPOSITION, STOP}; uint8_t state = HOME; -// Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering +// Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering (PI-controller) const double g = 360; // Aantal graden 1 rotatie const double c = 4200; // Aantal counts 1 rotatie const double q = c/(g); @@ -48,37 +44,34 @@ double err_int_m1 = 0 ; // De integrating error op het beginstijdstip m1 // Reusable P controller -double Pc (double error, const double Kp) -{ +double Pc (double error, const double Kp){ return motor1_Kp * error; } // Measure the error and apply output to the plant -void motor1_controlP() -{ +void motor1_controlP(){ double referenceP1 = PotMeter1.read(); double positionP1 = q*motor1.getPosition(); double motorP1 = Pc(referenceP1 - positionP1, motor1_Kp); } // Reusable PI controller -double PI (double error, const double Kp, const double Ki, const double Ts, double &err_int) -{ +double PI (double error, const double Kp, const double Ki, const double Ts, double &err_int){ err_int = err_int * Ts*error; // Dit is de fout die er door de integrator uit wordt gehaald. Deze wordt elke meting aangepast door het &-teken return motor1_Kp*error + motor1_Ki*err_int; } // De totale fout die wordt hersteld met behulp van PI control. -//bool Cali = false; -//double TimeCali = 5; // Filter1 = High pass filter tot 20 Hz double fh1_v1=0, fh1_v2=0, fh2_v1=0, fh2_v2=0; const double fh1_a1=-0.84909054461, fh1_a2=0.00000000000, fh1_b0= 1, fh1_b1=-1, fh1_b2=0; const double fh2_a1=-1.82553264091, fh2_a2=0.85001416809, fh2_b0= 1, fh2_b1=-2, fh2_b2=1; + // Filter2 = Low pass filter na 60 Hz double fl1_v1=0, fl1_v2=0, fl2_v1=0, fl2_v2=0; const double fl1_a1=-0.66979455390, fl1_a2=0.00000000000, fl1_b0= 1, fl1_b1=1, fl1_b2=0; const double fl2_a1=-1.55376616139, fl2_a2=0.68023470431, fl2_b0= 1, fl2_b1=2, fl2_b2=1; + // Filter3 = Notch filter at 50 Hz double fno1_v1=0, fno1_v2=0, fno2_v1=0, fno2_v2=0, fno3_v1=0, fno3_v2=0; const double fno1_a1 = -1.87934916386, fno1_a2= 0.97731851355, fno1_b0= 1, fno1_b1= -1.90090686046, fno1_b2= 1; @@ -94,9 +87,7 @@ double final_filter1; // Standaard formule voor het biquad filter -double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2) - -{ +double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2){ double v = u - a1*v1 - a2*v2; double y = b0*v + b1*v1 + b2*v2; v2=v1; @@ -104,8 +95,8 @@ return y; } -void Filters() -{ +// script voor filters +void Filters(){ u1 = EMG.read(); //Highpass y1 = biquad (u1, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547); @@ -132,8 +123,7 @@ } -void motor1_controlPI() -{ +void motor1_controlPI(){ double referencePI1 = PotMeter1.read(); double positionPI1 = q*motor1.getPosition(); double motorPI1 = PI(referencePI1 - positionPI1, motor1_Kp, motor1_Ki, motor1_Ts, err_int_m1); @@ -141,23 +131,28 @@ const int pressed = 0; +// constantes voor berekening homepositie double H; double P; double D; -void gethome(){ - H = motor1.getPosition(); -} - -void move_motor1_ccw (){ - motor1_direction = 0; - motor1_speed = 0.4; -} - -void move_motor1_cw (){ - motor1_direction = 1; - motor1_speed = 0.4; +void move_motor1() +{ + if (final_filter1 > 0.03){ + pc.printf("Moving clockwise \n\r"); + motor1_direction = 1; //clockwise + motor1_speed = 0.4; + } + else if (button_2 == pressed){ + pc.printf("Moving counterclockwise \n\r"); + motor1_direction = 0; //counterclockwise + motor1_speed = 0.4; + } + else { + pc.printf("Not moving \n\r"); + motor1_speed = 0; + } } void movetohome(){ @@ -176,90 +171,38 @@ } } -void move_motor1() -{ - if (final_filter1 > 0.03){ - pc.printf("Moving clockwise \n\r"); - move_motor1_cw (); - } - else if (button_2 == pressed){ - pc.printf("Moving counterclockwise \n\r"); - move_motor1_ccw (); - } - else { - pc.printf("Not moving \n\r"); - motor1_speed = 0; - } -} - -//void read_encoder1 () // aflezen van encoder via hidscope?? -//{ -// scope.set(0,motor1.getPosition()); - //led.write(motor1.getPosition()/100.0); -// scope.send(); -// wait(0.2f); -//} - void HIDScope (){ - scope.set (0, y8); - // scope.set (1, y2); - // scope.set (2, y4); - // scope.set (3, y7); - scope.set (1, final_filter1); - //scope.set (2, final_filter1); - scope.set(2, motor1.getPosition()); + scope.set (0, final_filter1); + scope.set(1, motor1.getPosition()); scope.send (); } - - int main() { while (true) { - pc.baud(9600); //pc baud rate van de computer - EMG_Filter.attach_us(Filters, 1e3); - Scope.attach_us(HIDScope, 1e3); + pc.baud(9600); //pc baud rate van de computer + EMG_Filter.attach_us(Filters, 1e3); //filters uitvoeren + Scope.attach_us(HIDScope, 1e3); //EMG en encoder signaal naar de hidscope sturen - switch (state) { //zorgt voor het in goede volgorde uitvoeren van de cases + switch (state) { //zorgt voor het in goede volgorde uitvoeren van de cases case HOME: //positie op 0 zetten voor arm 1 { pc.printf("home\n\r"); - //read_encoder1(); - gethome(); + H = motor1.getPosition(); pc.printf("Home-position is %f\n\r", H); - state = MOVE_MOTOR; - wait(5); + state = MOVE_MOTOR_1; + wait(2); break; } - - //case CALIBRATIE: - //{ - //pc.printf("Aanspannen in 10 \n\r"); - //wait(10); - //EMG_Control.attach_us(MyController,1e3); - //Timer_Calibration.start(); - //if (Timer_Calibration < TimeCali) { - // Cali = true; - // pc.printf("Aanspannen \n\r"); - //} - //else { - // Cali = false; - //} - //pc.printf("Stoppen \n\r"); - //Timer_Calibration.stop(); - //Timer_Calibration.reset(); - //state = MOVE_MOTOR; - // break; - //} - - case MOVE_MOTOR: //motor kan cw en ccw bewegen a.d.h.v. buttons + + case MOVE_MOTOR_1: //motor kan cw en ccw bewegen a.d.h.v. buttons { pc.printf("move_motor\n\r"); - while (state == MOVE_MOTOR){ + while (state == MOVE_MOTOR_1){ move_motor1(); - //print_position(); if (button_1 == pressed && button_2 == pressed){ + motor1_speed = 0; state = BACKTOHOMEPOSITION; } } @@ -273,8 +216,6 @@ wait (1); ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR); - //EMG_Control.attach_us(Filters,1e3); - while(state == BACKTOHOMEPOSITION){ movetohome(); while(regelaar_ticker_flag != true);