Opgeschoonde code voor verslag
Dependencies: Encoder HIDScope MODSERIAL mbed
Fork of TotalCodegr13V2 by
Diff: main.cpp
- Revision:
- 3:5f59cbe53d7d
- Parent:
- 2:866a8a9f2b93
- Child:
- 4:b4530fb376dd
diff -r 866a8a9f2b93 -r 5f59cbe53d7d main.cpp --- a/main.cpp Tue Oct 20 12:38:31 2015 +0000 +++ b/main.cpp Tue Oct 20 13:15:35 2015 +0000 @@ -11,6 +11,9 @@ DigitalIn button_2(PTA4); //clockwise Encoder motor1(D12,D13); HIDScope scope(1); +AnalogIn PotMeter1(A1); +Ticker controller; +Ticker ticker_regelaar; MODSERIAL pc(USBTX, USBRX); volatile bool regelaar_ticker_flag; @@ -21,12 +24,50 @@ } #define SAMPLETIME_REGELAAR 0.005 -float referentie_arm1 = 0; //define states -enum state {GOTOHOMEPOSITION, MOVE_MOTOR, BACKTOHOMEPOSITION}; +enum state {GOTOHOMEPOSITION, MOVE_MOTOR, BACKTOHOMEPOSITION, STOP}; uint8_t state = GOTOHOMEPOSITION; +// Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering +const double g = 360; // Aantal graden 1 rotatie +const double c = 4200; // Aantal counts 1 rotatie +const double q = c/(g); + +//PI-controller constante +const double motor1_Kp = 2.0; // Dit is de proportionele gain motor 1 +const double motor1_Ki = 0.002; // Integrating gain m1. +const double motor1_Ts = 0.01; // Time step m1 +double err_int_m1 = 0 ; // De integrating error op het beginstijdstip m1 + +// Reusable P controller +double Pc (double error, const double Kp) +{ + return motor1_Kp * error; +} + +// Measure the error and apply output to the plant +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) +{ + 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. + +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); +} + const int pressed = 0; double H; @@ -35,7 +76,6 @@ void sethome(){ - motor1.setPosition(0); H = motor1.getPosition(); } @@ -52,15 +92,17 @@ void movetohome(){ P = motor1.getPosition(); D = (P - H); - - if (D > -5 && D < 5){ + + if (D >= -36 && D <= 36){ motor1_speed = 0; } - else if (D > 0){ - move_motor1_cw(); + else if (D > 24){ + motor1_direction = 1; + motor1_speed = 0.1; } - else if (D < 0){ - move_motor1_ccw(); + else if (D < -24){ + motor1_direction = 0; + motor1_speed = 0.1; } } @@ -121,18 +163,35 @@ ticker_regelaar.attach(setregelaar_ticker_flag, SAMPLETIME_REGELAAR); while(state == BACKTOHOMEPOSITION){ + movetohome(); while(regelaar_ticker_flag != true); regelaar_ticker_flag = false; pc.printf("pulsmotorposition1 %d", motor1.getPosition()); - pc.printf("referentie %f\n\r", referentie_arm1); + pc.printf("referentie %d\n\r", H); if (motor1.getPosition() <=24 && motor1.getPosition() >= -24){ - referentie_arm1 = 0; + motor1.setPosition(0); + H = motor1.getPosition(); + state = STOP; + break; } - + } + } + case STOP: + { + const int einde = 1; + while(state == STOP){ + motor1_speed = 0; + if (einde == 1){ + pc.printf("homepositie %d\n\r", H); + pc.printf("huidige positie %d\n\r", P); + pc.printf("einde\n\r"); + + } + } break; - } - } + } +} } } \ No newline at end of file