![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
V1 SANS PID
Diff: main.cpp
- Revision:
- 1:d784b3e1fb18
- Parent:
- 0:c25d26ef035b
diff -r c25d26ef035b -r d784b3e1fb18 main.cpp --- a/main.cpp Mon Jan 21 16:58:36 2019 +0000 +++ b/main.cpp Tue Jan 22 08:57:38 2019 +0000 @@ -2,8 +2,8 @@ #include <math.h> #define THRESHOLD 0.5 -#define K (2.0 + get_pot()) -#define SPEED 0.40 +#define K 2.33 //(2.0 + get_pot()) +#define SPEED get_pot() #define MAX_RATIO 2.5 // 100us @@ -15,8 +15,20 @@ STOP } RUN_STATE; +typedef enum { + CAL_DARK = 0, + CAL_LIGHT_W, + CAL_LIGHT, + CAL_DONE_W, + CAL_DONE +} CAL_STATE; + +double light_val[6] = {0}; +double dark_val[6] = {0}; + IHM ihm; RUN_STATE run_state = WAIT; +CAL_STATE cal_state = CAL_DARK; Timer timer; PwmOut mot1(PB_5); @@ -51,9 +63,19 @@ return ain.read(); } +double get_sens(int i) +{ + return 1.0 - get_an(i); +} + +double get_an_norm(int i) +{ + return ((get_sens(i) - dark_val[i]) / (light_val[i] - dark_val[i])); +} + double get_di(int i) { - return get_an(i) < THRESHOLD; + return get_an_norm(i) > 0.5; } double get_error() @@ -61,7 +83,7 @@ float n = 0; float facs[6] = {-3, -2, -1, 1, 2, 3}; for (int i = 0; i < 6; i++) - n += facs[i] * get_an(i); + n += facs[i] * get_an_norm(i); return (n); } @@ -90,13 +112,52 @@ int main() { ihm.BAR_set(0); - ihm.LCD_clear(); - ihm.LCD_gotoxy(0, 0); - ihm.LCD_printf("EL TACOS"); mot1.period_us(100); mot2.period_us(100); mot1.write(0); mot2.write(0); + while (cal_state != CAL_DONE) + { + switch (cal_state) + { + case CAL_DARK: + ihm.LCD_clear(); + ihm.LCD_gotoxy(0, 0); + ihm.LCD_printf("DARK"); + if (pb) + { + for (int i = 0; i < 6; i++) + dark_val[i] = get_sens(i); + cal_state = CAL_LIGHT_W; + } + break; + case CAL_LIGHT_W: + if (!pb) + cal_state = CAL_LIGHT; + break; + case CAL_LIGHT: + ihm.LCD_clear(); + ihm.LCD_gotoxy(0, 0); + ihm.LCD_printf("LIGHT"); + if (pb) + { + for (int i = 0; i < 6; i++) + light_val[i] = get_sens(i); + cal_state = CAL_DONE_W; + } + break; + case CAL_DONE_W: + if (!pb) + cal_state = CAL_DONE; + break; + default: + break; + } + wait_ms(100); + } + ihm.LCD_clear(); + ihm.LCD_gotoxy(0, 0); + ihm.LCD_printf("DURUM-DURUM"); timer.start(); while (1){ // wait DT @@ -112,7 +173,7 @@ if (run_state == RUN) { if (sens_count() <= 2) - set_motors(SPEED, borned(K * get_error() / 3.0, -1.0, 1.0)); + set_motors(SPEED, borned(-K * get_error() / 3.0, -1.0, 1.0)); } else { mot1.write(0); mot2.write(0);