Thomas Potier / Mbed 2 deprecated GAMEL_SIMPLE_V0_1

V1 SANS PID

Dependencies:   mbed IHM

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?

UserRevisionLine numberNew 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