![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
V1 SANS PID
main.cpp@1:d784b3e1fb18, 2019-01-22 (annotated)
- Committer:
- thomaspotier
- Date:
- Tue Jan 22 08:57:38 2019 +0000
- Revision:
- 1:d784b3e1fb18
- Parent:
- 0:c25d26ef035b
Sensor calibration
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
thomaspotier | 0:c25d26ef035b | 1 | #include "IHM.h" |
thomaspotier | 0:c25d26ef035b | 2 | #include <math.h> |
thomaspotier | 0:c25d26ef035b | 3 | |
thomaspotier | 0:c25d26ef035b | 4 | #define THRESHOLD 0.5 |
thomaspotier | 1:d784b3e1fb18 | 5 | #define K 2.33 //(2.0 + get_pot()) |
thomaspotier | 1:d784b3e1fb18 | 6 | #define SPEED get_pot() |
thomaspotier | 0:c25d26ef035b | 7 | #define MAX_RATIO 2.5 |
thomaspotier | 0:c25d26ef035b | 8 | |
thomaspotier | 0:c25d26ef035b | 9 | // 100us |
thomaspotier | 0:c25d26ef035b | 10 | #define DT 0.0001 |
thomaspotier | 0:c25d26ef035b | 11 | |
thomaspotier | 0:c25d26ef035b | 12 | typedef enum { |
thomaspotier | 0:c25d26ef035b | 13 | WAIT = 0, |
thomaspotier | 0:c25d26ef035b | 14 | RUN, |
thomaspotier | 0:c25d26ef035b | 15 | STOP |
thomaspotier | 0:c25d26ef035b | 16 | } RUN_STATE; |
thomaspotier | 0:c25d26ef035b | 17 | |
thomaspotier | 1:d784b3e1fb18 | 18 | typedef enum { |
thomaspotier | 1:d784b3e1fb18 | 19 | CAL_DARK = 0, |
thomaspotier | 1:d784b3e1fb18 | 20 | CAL_LIGHT_W, |
thomaspotier | 1:d784b3e1fb18 | 21 | CAL_LIGHT, |
thomaspotier | 1:d784b3e1fb18 | 22 | CAL_DONE_W, |
thomaspotier | 1:d784b3e1fb18 | 23 | CAL_DONE |
thomaspotier | 1:d784b3e1fb18 | 24 | } CAL_STATE; |
thomaspotier | 1:d784b3e1fb18 | 25 | |
thomaspotier | 1:d784b3e1fb18 | 26 | double light_val[6] = {0}; |
thomaspotier | 1:d784b3e1fb18 | 27 | double dark_val[6] = {0}; |
thomaspotier | 1:d784b3e1fb18 | 28 | |
thomaspotier | 0:c25d26ef035b | 29 | IHM ihm; |
thomaspotier | 0:c25d26ef035b | 30 | RUN_STATE run_state = WAIT; |
thomaspotier | 1:d784b3e1fb18 | 31 | CAL_STATE cal_state = CAL_DARK; |
thomaspotier | 0:c25d26ef035b | 32 | Timer timer; |
thomaspotier | 0:c25d26ef035b | 33 | |
thomaspotier | 0:c25d26ef035b | 34 | PwmOut mot1(PB_5); |
thomaspotier | 0:c25d26ef035b | 35 | PwmOut mot2(PB_4); |
thomaspotier | 0:c25d26ef035b | 36 | |
thomaspotier | 0:c25d26ef035b | 37 | // AnaIn |
thomaspotier | 0:c25d26ef035b | 38 | AnalogIn ain(PB_1); |
thomaspotier | 0:c25d26ef035b | 39 | // AnaIn Select |
thomaspotier | 0:c25d26ef035b | 40 | BusOut select_analog(PA_8, PF_1, PF_0); |
thomaspotier | 0:c25d26ef035b | 41 | DigitalIn jack(PB_6); |
thomaspotier | 0:c25d26ef035b | 42 | DigitalIn pb(PB_7); |
thomaspotier | 0:c25d26ef035b | 43 | |
thomaspotier | 0:c25d26ef035b | 44 | double min(double a, double b) |
thomaspotier | 0:c25d26ef035b | 45 | { |
thomaspotier | 0:c25d26ef035b | 46 | return a < b ? a : b; |
thomaspotier | 0:c25d26ef035b | 47 | } |
thomaspotier | 0:c25d26ef035b | 48 | |
thomaspotier | 0:c25d26ef035b | 49 | double max(double a, double b) |
thomaspotier | 0:c25d26ef035b | 50 | { |
thomaspotier | 0:c25d26ef035b | 51 | return a > b ? a : b; |
thomaspotier | 0:c25d26ef035b | 52 | } |
thomaspotier | 0:c25d26ef035b | 53 | |
thomaspotier | 0:c25d26ef035b | 54 | double borned(double n, double mi, double ma) |
thomaspotier | 0:c25d26ef035b | 55 | { |
thomaspotier | 0:c25d26ef035b | 56 | return max(mi, min(ma, n)); |
thomaspotier | 0:c25d26ef035b | 57 | } |
thomaspotier | 0:c25d26ef035b | 58 | |
thomaspotier | 0:c25d26ef035b | 59 | double get_an(int i) |
thomaspotier | 0:c25d26ef035b | 60 | { |
thomaspotier | 0:c25d26ef035b | 61 | select_analog = i; |
thomaspotier | 0:c25d26ef035b | 62 | wait_us(1); |
thomaspotier | 0:c25d26ef035b | 63 | return ain.read(); |
thomaspotier | 0:c25d26ef035b | 64 | } |
thomaspotier | 0:c25d26ef035b | 65 | |
thomaspotier | 1:d784b3e1fb18 | 66 | double get_sens(int i) |
thomaspotier | 1:d784b3e1fb18 | 67 | { |
thomaspotier | 1:d784b3e1fb18 | 68 | return 1.0 - get_an(i); |
thomaspotier | 1:d784b3e1fb18 | 69 | } |
thomaspotier | 1:d784b3e1fb18 | 70 | |
thomaspotier | 1:d784b3e1fb18 | 71 | double get_an_norm(int i) |
thomaspotier | 1:d784b3e1fb18 | 72 | { |
thomaspotier | 1:d784b3e1fb18 | 73 | return ((get_sens(i) - dark_val[i]) / (light_val[i] - dark_val[i])); |
thomaspotier | 1:d784b3e1fb18 | 74 | } |
thomaspotier | 1:d784b3e1fb18 | 75 | |
thomaspotier | 0:c25d26ef035b | 76 | double get_di(int i) |
thomaspotier | 0:c25d26ef035b | 77 | { |
thomaspotier | 1:d784b3e1fb18 | 78 | return get_an_norm(i) > 0.5; |
thomaspotier | 0:c25d26ef035b | 79 | } |
thomaspotier | 0:c25d26ef035b | 80 | |
thomaspotier | 0:c25d26ef035b | 81 | double get_error() |
thomaspotier | 0:c25d26ef035b | 82 | { |
thomaspotier | 0:c25d26ef035b | 83 | float n = 0; |
thomaspotier | 0:c25d26ef035b | 84 | float facs[6] = {-3, -2, -1, 1, 2, 3}; |
thomaspotier | 0:c25d26ef035b | 85 | for (int i = 0; i < 6; i++) |
thomaspotier | 1:d784b3e1fb18 | 86 | n += facs[i] * get_an_norm(i); |
thomaspotier | 0:c25d26ef035b | 87 | return (n); |
thomaspotier | 0:c25d26ef035b | 88 | } |
thomaspotier | 0:c25d26ef035b | 89 | |
thomaspotier | 0:c25d26ef035b | 90 | double get_pot() |
thomaspotier | 0:c25d26ef035b | 91 | { |
thomaspotier | 0:c25d26ef035b | 92 | return get_an(7); |
thomaspotier | 0:c25d26ef035b | 93 | } |
thomaspotier | 0:c25d26ef035b | 94 | |
thomaspotier | 0:c25d26ef035b | 95 | void set_motors(double speed, double dir) |
thomaspotier | 0:c25d26ef035b | 96 | { |
thomaspotier | 0:c25d26ef035b | 97 | double ratio = (MAX_RATIO - 1.0) / (MAX_RATIO + 1.0); |
thomaspotier | 0:c25d26ef035b | 98 | double v1 = speed - dir * speed * ratio; |
thomaspotier | 0:c25d26ef035b | 99 | double v2 = speed + dir * speed * ratio; |
thomaspotier | 0:c25d26ef035b | 100 | mot1.write(v1); |
thomaspotier | 0:c25d26ef035b | 101 | mot2.write(v2); |
thomaspotier | 0:c25d26ef035b | 102 | } |
thomaspotier | 0:c25d26ef035b | 103 | |
thomaspotier | 0:c25d26ef035b | 104 | int sens_count() |
thomaspotier | 0:c25d26ef035b | 105 | { |
thomaspotier | 0:c25d26ef035b | 106 | int n = 0; |
thomaspotier | 0:c25d26ef035b | 107 | for (int i = 0; i < 6; i++) |
thomaspotier | 0:c25d26ef035b | 108 | if (get_di(i)) |
thomaspotier | 0:c25d26ef035b | 109 | n++; |
thomaspotier | 0:c25d26ef035b | 110 | return (n); |
thomaspotier | 0:c25d26ef035b | 111 | } |
thomaspotier | 0:c25d26ef035b | 112 | |
thomaspotier | 0:c25d26ef035b | 113 | int main() { |
thomaspotier | 0:c25d26ef035b | 114 | ihm.BAR_set(0); |
thomaspotier | 0:c25d26ef035b | 115 | mot1.period_us(100); |
thomaspotier | 0:c25d26ef035b | 116 | mot2.period_us(100); |
thomaspotier | 0:c25d26ef035b | 117 | mot1.write(0); |
thomaspotier | 0:c25d26ef035b | 118 | mot2.write(0); |
thomaspotier | 1:d784b3e1fb18 | 119 | while (cal_state != CAL_DONE) |
thomaspotier | 1:d784b3e1fb18 | 120 | { |
thomaspotier | 1:d784b3e1fb18 | 121 | switch (cal_state) |
thomaspotier | 1:d784b3e1fb18 | 122 | { |
thomaspotier | 1:d784b3e1fb18 | 123 | case CAL_DARK: |
thomaspotier | 1:d784b3e1fb18 | 124 | ihm.LCD_clear(); |
thomaspotier | 1:d784b3e1fb18 | 125 | ihm.LCD_gotoxy(0, 0); |
thomaspotier | 1:d784b3e1fb18 | 126 | ihm.LCD_printf("DARK"); |
thomaspotier | 1:d784b3e1fb18 | 127 | if (pb) |
thomaspotier | 1:d784b3e1fb18 | 128 | { |
thomaspotier | 1:d784b3e1fb18 | 129 | for (int i = 0; i < 6; i++) |
thomaspotier | 1:d784b3e1fb18 | 130 | dark_val[i] = get_sens(i); |
thomaspotier | 1:d784b3e1fb18 | 131 | cal_state = CAL_LIGHT_W; |
thomaspotier | 1:d784b3e1fb18 | 132 | } |
thomaspotier | 1:d784b3e1fb18 | 133 | break; |
thomaspotier | 1:d784b3e1fb18 | 134 | case CAL_LIGHT_W: |
thomaspotier | 1:d784b3e1fb18 | 135 | if (!pb) |
thomaspotier | 1:d784b3e1fb18 | 136 | cal_state = CAL_LIGHT; |
thomaspotier | 1:d784b3e1fb18 | 137 | break; |
thomaspotier | 1:d784b3e1fb18 | 138 | case CAL_LIGHT: |
thomaspotier | 1:d784b3e1fb18 | 139 | ihm.LCD_clear(); |
thomaspotier | 1:d784b3e1fb18 | 140 | ihm.LCD_gotoxy(0, 0); |
thomaspotier | 1:d784b3e1fb18 | 141 | ihm.LCD_printf("LIGHT"); |
thomaspotier | 1:d784b3e1fb18 | 142 | if (pb) |
thomaspotier | 1:d784b3e1fb18 | 143 | { |
thomaspotier | 1:d784b3e1fb18 | 144 | for (int i = 0; i < 6; i++) |
thomaspotier | 1:d784b3e1fb18 | 145 | light_val[i] = get_sens(i); |
thomaspotier | 1:d784b3e1fb18 | 146 | cal_state = CAL_DONE_W; |
thomaspotier | 1:d784b3e1fb18 | 147 | } |
thomaspotier | 1:d784b3e1fb18 | 148 | break; |
thomaspotier | 1:d784b3e1fb18 | 149 | case CAL_DONE_W: |
thomaspotier | 1:d784b3e1fb18 | 150 | if (!pb) |
thomaspotier | 1:d784b3e1fb18 | 151 | cal_state = CAL_DONE; |
thomaspotier | 1:d784b3e1fb18 | 152 | break; |
thomaspotier | 1:d784b3e1fb18 | 153 | default: |
thomaspotier | 1:d784b3e1fb18 | 154 | break; |
thomaspotier | 1:d784b3e1fb18 | 155 | } |
thomaspotier | 1:d784b3e1fb18 | 156 | wait_ms(100); |
thomaspotier | 1:d784b3e1fb18 | 157 | } |
thomaspotier | 1:d784b3e1fb18 | 158 | ihm.LCD_clear(); |
thomaspotier | 1:d784b3e1fb18 | 159 | ihm.LCD_gotoxy(0, 0); |
thomaspotier | 1:d784b3e1fb18 | 160 | ihm.LCD_printf("DURUM-DURUM"); |
thomaspotier | 0:c25d26ef035b | 161 | timer.start(); |
thomaspotier | 0:c25d26ef035b | 162 | while (1){ |
thomaspotier | 0:c25d26ef035b | 163 | // wait DT |
thomaspotier | 0:c25d26ef035b | 164 | while (timer.read_us() < 1000); |
thomaspotier | 0:c25d26ef035b | 165 | timer.reset(); |
thomaspotier | 0:c25d26ef035b | 166 | // RUN_STATE automata |
thomaspotier | 0:c25d26ef035b | 167 | if (run_state == WAIT && jack) |
thomaspotier | 0:c25d26ef035b | 168 | run_state = RUN; |
thomaspotier | 0:c25d26ef035b | 169 | if (run_state == RUN && pb) |
thomaspotier | 0:c25d26ef035b | 170 | run_state = STOP; |
thomaspotier | 0:c25d26ef035b | 171 | if (run_state == STOP && !jack) |
thomaspotier | 0:c25d26ef035b | 172 | run_state = WAIT; |
thomaspotier | 0:c25d26ef035b | 173 | if (run_state == RUN) |
thomaspotier | 0:c25d26ef035b | 174 | { |
thomaspotier | 0:c25d26ef035b | 175 | if (sens_count() <= 2) |
thomaspotier | 1:d784b3e1fb18 | 176 | set_motors(SPEED, borned(-K * get_error() / 3.0, -1.0, 1.0)); |
thomaspotier | 0:c25d26ef035b | 177 | } else { |
thomaspotier | 0:c25d26ef035b | 178 | mot1.write(0); |
thomaspotier | 0:c25d26ef035b | 179 | mot2.write(0); |
thomaspotier | 0:c25d26ef035b | 180 | } |
thomaspotier | 0:c25d26ef035b | 181 | } |
thomaspotier | 0:c25d26ef035b | 182 | } |
thomaspotier | 0:c25d26ef035b | 183 | |
thomaspotier | 0:c25d26ef035b | 184 |