Thomas Potier / Mbed 2 deprecated GAMEL_SIMPLE_V0_1

V1 SANS PID

Dependencies:   mbed IHM

main.cpp@0:c25d26ef035b, 2019-01-21 (annotated)

Committer:
thomaspotier
Date:
Mon Jan 21 16:58:36 2019 +0000
Revision:
0:c25d26ef035b
Child:
1:d784b3e1fb18
FONCTIONNE V1

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 0:c25d26ef035b 5 #define K (2.0 + get_pot())
thomaspotier 0:c25d26ef035b 6 #define SPEED 0.40
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 0:c25d26ef035b 18 IHM ihm;
thomaspotier 0:c25d26ef035b 19 RUN_STATE run_state = WAIT;
thomaspotier 0:c25d26ef035b 20 Timer timer;
thomaspotier 0:c25d26ef035b 21
thomaspotier 0:c25d26ef035b 22 PwmOut mot1(PB_5);
thomaspotier 0:c25d26ef035b 23 PwmOut mot2(PB_4);
thomaspotier 0:c25d26ef035b 24
thomaspotier 0:c25d26ef035b 25 // AnaIn
thomaspotier 0:c25d26ef035b 26 AnalogIn ain(PB_1);
thomaspotier 0:c25d26ef035b 27 // AnaIn Select
thomaspotier 0:c25d26ef035b 28 BusOut select_analog(PA_8, PF_1, PF_0);
thomaspotier 0:c25d26ef035b 29 DigitalIn jack(PB_6);
thomaspotier 0:c25d26ef035b 30 DigitalIn pb(PB_7);
thomaspotier 0:c25d26ef035b 31
thomaspotier 0:c25d26ef035b 32 double min(double a, double b)
thomaspotier 0:c25d26ef035b 33 {
thomaspotier 0:c25d26ef035b 34 return a < b ? a : b;
thomaspotier 0:c25d26ef035b 35 }
thomaspotier 0:c25d26ef035b 36
thomaspotier 0:c25d26ef035b 37 double max(double a, double b)
thomaspotier 0:c25d26ef035b 38 {
thomaspotier 0:c25d26ef035b 39 return a > b ? a : b;
thomaspotier 0:c25d26ef035b 40 }
thomaspotier 0:c25d26ef035b 41
thomaspotier 0:c25d26ef035b 42 double borned(double n, double mi, double ma)
thomaspotier 0:c25d26ef035b 43 {
thomaspotier 0:c25d26ef035b 44 return max(mi, min(ma, n));
thomaspotier 0:c25d26ef035b 45 }
thomaspotier 0:c25d26ef035b 46
thomaspotier 0:c25d26ef035b 47 double get_an(int i)
thomaspotier 0:c25d26ef035b 48 {
thomaspotier 0:c25d26ef035b 49 select_analog = i;
thomaspotier 0:c25d26ef035b 50 wait_us(1);
thomaspotier 0:c25d26ef035b 51 return ain.read();
thomaspotier 0:c25d26ef035b 52 }
thomaspotier 0:c25d26ef035b 53
thomaspotier 0:c25d26ef035b 54 double get_di(int i)
thomaspotier 0:c25d26ef035b 55 {
thomaspotier 0:c25d26ef035b 56 return get_an(i) < THRESHOLD;
thomaspotier 0:c25d26ef035b 57 }
thomaspotier 0:c25d26ef035b 58
thomaspotier 0:c25d26ef035b 59 double get_error()
thomaspotier 0:c25d26ef035b 60 {
thomaspotier 0:c25d26ef035b 61 float n = 0;
thomaspotier 0:c25d26ef035b 62 float facs[6] = {-3, -2, -1, 1, 2, 3};
thomaspotier 0:c25d26ef035b 63 for (int i = 0; i < 6; i++)
thomaspotier 0:c25d26ef035b 64 n += facs[i] * get_an(i);
thomaspotier 0:c25d26ef035b 65 return (n);
thomaspotier 0:c25d26ef035b 66 }
thomaspotier 0:c25d26ef035b 67
thomaspotier 0:c25d26ef035b 68 double get_pot()
thomaspotier 0:c25d26ef035b 69 {
thomaspotier 0:c25d26ef035b 70 return get_an(7);
thomaspotier 0:c25d26ef035b 71 }
thomaspotier 0:c25d26ef035b 72
thomaspotier 0:c25d26ef035b 73 void set_motors(double speed, double dir)
thomaspotier 0:c25d26ef035b 74 {
thomaspotier 0:c25d26ef035b 75 double ratio = (MAX_RATIO - 1.0) / (MAX_RATIO + 1.0);
thomaspotier 0:c25d26ef035b 76 double v1 = speed - dir * speed * ratio;
thomaspotier 0:c25d26ef035b 77 double v2 = speed + dir * speed * ratio;
thomaspotier 0:c25d26ef035b 78 mot1.write(v1);
thomaspotier 0:c25d26ef035b 79 mot2.write(v2);
thomaspotier 0:c25d26ef035b 80 }
thomaspotier 0:c25d26ef035b 81
thomaspotier 0:c25d26ef035b 82 int sens_count()
thomaspotier 0:c25d26ef035b 83 {
thomaspotier 0:c25d26ef035b 84 int n = 0;
thomaspotier 0:c25d26ef035b 85 for (int i = 0; i < 6; i++)
thomaspotier 0:c25d26ef035b 86 if (get_di(i))
thomaspotier 0:c25d26ef035b 87 n++;
thomaspotier 0:c25d26ef035b 88 return (n);
thomaspotier 0:c25d26ef035b 89 }
thomaspotier 0:c25d26ef035b 90
thomaspotier 0:c25d26ef035b 91 int main() {
thomaspotier 0:c25d26ef035b 92 ihm.BAR_set(0);
thomaspotier 0:c25d26ef035b 93 ihm.LCD_clear();
thomaspotier 0:c25d26ef035b 94 ihm.LCD_gotoxy(0, 0);
thomaspotier 0:c25d26ef035b 95 ihm.LCD_printf("EL TACOS");
thomaspotier 0:c25d26ef035b 96 mot1.period_us(100);
thomaspotier 0:c25d26ef035b 97 mot2.period_us(100);
thomaspotier 0:c25d26ef035b 98 mot1.write(0);
thomaspotier 0:c25d26ef035b 99 mot2.write(0);
thomaspotier 0:c25d26ef035b 100 timer.start();
thomaspotier 0:c25d26ef035b 101 while (1){
thomaspotier 0:c25d26ef035b 102 // wait DT
thomaspotier 0:c25d26ef035b 103 while (timer.read_us() < 1000);
thomaspotier 0:c25d26ef035b 104 timer.reset();
thomaspotier 0:c25d26ef035b 105 // RUN_STATE automata
thomaspotier 0:c25d26ef035b 106 if (run_state == WAIT && jack)
thomaspotier 0:c25d26ef035b 107 run_state = RUN;
thomaspotier 0:c25d26ef035b 108 if (run_state == RUN && pb)
thomaspotier 0:c25d26ef035b 109 run_state = STOP;
thomaspotier 0:c25d26ef035b 110 if (run_state == STOP && !jack)
thomaspotier 0:c25d26ef035b 111 run_state = WAIT;
thomaspotier 0:c25d26ef035b 112 if (run_state == RUN)
thomaspotier 0:c25d26ef035b 113 {
thomaspotier 0:c25d26ef035b 114 if (sens_count() <= 2)
thomaspotier 0:c25d26ef035b 115 set_motors(SPEED, borned(K * get_error() / 3.0, -1.0, 1.0));
thomaspotier 0:c25d26ef035b 116 } else {
thomaspotier 0:c25d26ef035b 117 mot1.write(0);
thomaspotier 0:c25d26ef035b 118 mot2.write(0);
thomaspotier 0:c25d26ef035b 119 }
thomaspotier 0:c25d26ef035b 120 }
thomaspotier 0:c25d26ef035b 121 }
thomaspotier 0:c25d26ef035b 122
thomaspotier 0:c25d26ef035b 123