![](/media/cache/group/Logo_v4_ZnplGO9.jpg.50x50_q85.jpg)
il est la, il est beau!!!!
Dependencies: m3piBluetooth mbed
polulu.cpp@0:b839dbffc636, 2018-05-03 (annotated)
- Committer:
- thomasstrub
- Date:
- Thu May 03 14:03:58 2018 +0000
- Revision:
- 0:b839dbffc636
- Child:
- 1:63050abd40e9
projet pacman
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
thomasstrub | 0:b839dbffc636 | 1 | #include "mbed.h" |
thomasstrub | 0:b839dbffc636 | 2 | #include "m3pi.h" |
thomasstrub | 0:b839dbffc636 | 3 | |
thomasstrub | 0:b839dbffc636 | 4 | m3pi m3pi; // Initialise the m3pi |
thomasstrub | 0:b839dbffc636 | 5 | Serial pc(USBTX, USBRX); //tx, rx |
thomasstrub | 0:b839dbffc636 | 6 | Serial bt(p28,p27); //bluetooth |
thomasstrub | 0:b839dbffc636 | 7 | |
thomasstrub | 0:b839dbffc636 | 8 | |
thomasstrub | 0:b839dbffc636 | 9 | |
thomasstrub | 0:b839dbffc636 | 10 | DigitalOut myled1(LED1); |
thomasstrub | 0:b839dbffc636 | 11 | DigitalOut myled2(LED2); |
thomasstrub | 0:b839dbffc636 | 12 | DigitalOut myled3(LED3); |
thomasstrub | 0:b839dbffc636 | 13 | DigitalOut myled4(LED4); |
thomasstrub | 0:b839dbffc636 | 14 | DigitalOut leds [4] = {LED1, LED2, LED3, LED4}; |
thomasstrub | 0:b839dbffc636 | 15 | // variables |
thomasstrub | 0:b839dbffc636 | 16 | |
thomasstrub | 0:b839dbffc636 | 17 | BusOut myleds(LED1, LED2, LED3, LED4); |
thomasstrub | 0:b839dbffc636 | 18 | |
thomasstrub | 0:b839dbffc636 | 19 | const int THRESH = 300; |
thomasstrub | 0:b839dbffc636 | 20 | |
thomasstrub | 0:b839dbffc636 | 21 | unsigned short sensors[5], mems[5], nbCapt, memsNb; |
thomasstrub | 0:b839dbffc636 | 22 | char tab[4]; |
thomasstrub | 0:b839dbffc636 | 23 | float position, proportional, last_proportional, integral; |
thomasstrub | 0:b839dbffc636 | 24 | |
thomasstrub | 0:b839dbffc636 | 25 | unsigned char dir; |
thomasstrub | 0:b839dbffc636 | 26 | float speed, duration, s1, s2; |
thomasstrub | 0:b839dbffc636 | 27 | |
thomasstrub | 0:b839dbffc636 | 28 | |
thomasstrub | 0:b839dbffc636 | 29 | //------------------ paramétrage moteurs --------------------------// |
thomasstrub | 0:b839dbffc636 | 30 | |
thomasstrub | 0:b839dbffc636 | 31 | |
thomasstrub | 0:b839dbffc636 | 32 | void set_motors(float mr, float ml) |
thomasstrub | 0:b839dbffc636 | 33 | { |
thomasstrub | 0:b839dbffc636 | 34 | m3pi.left_motor(ml); |
thomasstrub | 0:b839dbffc636 | 35 | m3pi.right_motor(mr); |
thomasstrub | 0:b839dbffc636 | 36 | } |
thomasstrub | 0:b839dbffc636 | 37 | |
thomasstrub | 0:b839dbffc636 | 38 | //---------------------- arret moteur -----------------------------// |
thomasstrub | 0:b839dbffc636 | 39 | |
thomasstrub | 0:b839dbffc636 | 40 | void stop_motors() |
thomasstrub | 0:b839dbffc636 | 41 | { |
thomasstrub | 0:b839dbffc636 | 42 | m3pi.stop(); |
thomasstrub | 0:b839dbffc636 | 43 | } |
thomasstrub | 0:b839dbffc636 | 44 | |
thomasstrub | 0:b839dbffc636 | 45 | |
thomasstrub | 0:b839dbffc636 | 46 | //--------------- utilisation capteur -------------------------------// |
thomasstrub | 0:b839dbffc636 | 47 | |
thomasstrub | 0:b839dbffc636 | 48 | void getCapt(bool mem = false) |
thomasstrub | 0:b839dbffc636 | 49 | { |
thomasstrub | 0:b839dbffc636 | 50 | int v = 0; |
thomasstrub | 0:b839dbffc636 | 51 | int nbCapt = 0; |
thomasstrub | 0:b839dbffc636 | 52 | |
thomasstrub | 0:b839dbffc636 | 53 | m3pi.calibrated_sensors(sensors); |
thomasstrub | 0:b839dbffc636 | 54 | |
thomasstrub | 0:b839dbffc636 | 55 | for(int i=0 ; i<5 ;i++) |
thomasstrub | 0:b839dbffc636 | 56 | { |
thomasstrub | 0:b839dbffc636 | 57 | sensors[i] = (sensors[i] > THRESH ? true : false); |
thomasstrub | 0:b839dbffc636 | 58 | if(sensors[i]) |
thomasstrub | 0:b839dbffc636 | 59 | { |
thomasstrub | 0:b839dbffc636 | 60 | v += 0x1<<i; |
thomasstrub | 0:b839dbffc636 | 61 | nbCapt++; |
thomasstrub | 0:b839dbffc636 | 62 | } |
thomasstrub | 0:b839dbffc636 | 63 | if(mem) |
thomasstrub | 0:b839dbffc636 | 64 | { |
thomasstrub | 0:b839dbffc636 | 65 | mems[i] = sensors[i]; |
thomasstrub | 0:b839dbffc636 | 66 | memsNb = nbCapt; |
thomasstrub | 0:b839dbffc636 | 67 | } |
thomasstrub | 0:b839dbffc636 | 68 | } |
thomasstrub | 0:b839dbffc636 | 69 | m3pi.leds(v); |
thomasstrub | 0:b839dbffc636 | 70 | } |
thomasstrub | 0:b839dbffc636 | 71 | |
thomasstrub | 0:b839dbffc636 | 72 | |
thomasstrub | 0:b839dbffc636 | 73 | //-------------------- suivi ligne00000000 -------------------------// |
thomasstrub | 0:b839dbffc636 | 74 | |
thomasstrub | 0:b839dbffc636 | 75 | |
thomasstrub | 0:b839dbffc636 | 76 | void suiviIntersect(float intervalTour = 0.0f) |
thomasstrub | 0:b839dbffc636 | 77 | { |
thomasstrub | 0:b839dbffc636 | 78 | bt.printf("startSuivi_"); |
thomasstrub | 0:b839dbffc636 | 79 | bool done = false; |
thomasstrub | 0:b839dbffc636 | 80 | unsigned int idx=0; |
thomasstrub | 0:b839dbffc636 | 81 | float vl, vr, omega, l_k=0, l_k1=0, sum=0, vmax=0.50f; |
thomasstrub | 0:b839dbffc636 | 82 | |
thomasstrub | 0:b839dbffc636 | 83 | #if 1 // Low speed |
thomasstrub | 0:b839dbffc636 | 84 | const float Ki=0.005f, Kp=0.5f, Kd=1.5f, v=0.2; |
thomasstrub | 0:b839dbffc636 | 85 | #else |
thomasstrub | 0:b839dbffc636 | 86 | const float Ki=0.0005f, Kp=0.05f, Kd=1.5f, v=0.80f; |
thomasstrub | 0:b839dbffc636 | 87 | #endif |
thomasstrub | 0:b839dbffc636 | 88 | m3pi.sensor_auto_calibrate(); |
thomasstrub | 0:b839dbffc636 | 89 | wait(1.0); |
thomasstrub | 0:b839dbffc636 | 90 | bt.printf("---> While in<--- "); |
thomasstrub | 0:b839dbffc636 | 91 | Timer t; t.start(); |
thomasstrub | 0:b839dbffc636 | 92 | while(!done) |
thomasstrub | 0:b839dbffc636 | 93 | { |
thomasstrub | 0:b839dbffc636 | 94 | idx++; |
thomasstrub | 0:b839dbffc636 | 95 | getCapt(); |
thomasstrub | 0:b839dbffc636 | 96 | // PID |
thomasstrub | 0:b839dbffc636 | 97 | l_k1 = l_k; |
thomasstrub | 0:b839dbffc636 | 98 | l_k = m3pi.line_position(); |
thomasstrub | 0:b839dbffc636 | 99 | //v = sp; |
thomasstrub | 0:b839dbffc636 | 100 | omega = l_k*Kp; // Proportionnal |
thomasstrub | 0:b839dbffc636 | 101 | sum += l_k; |
thomasstrub | 0:b839dbffc636 | 102 | omega += sum*Ki; // Integral |
thomasstrub | 0:b839dbffc636 | 103 | omega += (l_k-l_k1)*Kd; |
thomasstrub | 0:b839dbffc636 | 104 | //omega *= v; |
thomasstrub | 0:b839dbffc636 | 105 | |
thomasstrub | 0:b839dbffc636 | 106 | |
thomasstrub | 0:b839dbffc636 | 107 | vl = v+omega < vmax ? (v+omega > -vmax ? v+omega : vmax) : vmax; |
thomasstrub | 0:b839dbffc636 | 108 | vr = v-omega < vmax ? (v-omega > -vmax ? v-omega : -vmax): vmax; |
thomasstrub | 0:b839dbffc636 | 109 | |
thomasstrub | 0:b839dbffc636 | 110 | |
thomasstrub | 0:b839dbffc636 | 111 | //------------------------------------- Pour les zigzags |
thomasstrub | 0:b839dbffc636 | 112 | //getCurrentSerial().printf("? hola que pasa ?"); |
thomasstrub | 0:b839dbffc636 | 113 | |
thomasstrub | 0:b839dbffc636 | 114 | //------------------------------------- |
thomasstrub | 0:b839dbffc636 | 115 | |
thomasstrub | 0:b839dbffc636 | 116 | //-----------------Intersection |
thomasstrub | 0:b839dbffc636 | 117 | if(sensors[0] || sensors[4]) |
thomasstrub | 0:b839dbffc636 | 118 | { |
thomasstrub | 0:b839dbffc636 | 119 | done = true; |
thomasstrub | 0:b839dbffc636 | 120 | } |
thomasstrub | 0:b839dbffc636 | 121 | set_motors(vr,vl); |
thomasstrub | 0:b839dbffc636 | 122 | } |
thomasstrub | 0:b839dbffc636 | 123 | bt.printf("-->While out<--"); |
thomasstrub | 0:b839dbffc636 | 124 | |
thomasstrub | 0:b839dbffc636 | 125 | return; |
thomasstrub | 0:b839dbffc636 | 126 | } |
thomasstrub | 0:b839dbffc636 | 127 | |
thomasstrub | 0:b839dbffc636 | 128 | |
thomasstrub | 0:b839dbffc636 | 129 | //----------------- pilotage -> direction ---------------------// |
thomasstrub | 0:b839dbffc636 | 130 | |
thomasstrub | 0:b839dbffc636 | 131 | |
thomasstrub | 0:b839dbffc636 | 132 | void assignDirection(char c, float s) |
thomasstrub | 0:b839dbffc636 | 133 | { |
thomasstrub | 0:b839dbffc636 | 134 | if (c == 'a') m3pi.left(s); |
thomasstrub | 0:b839dbffc636 | 135 | if (c == 'c') m3pi.right(s); |
thomasstrub | 0:b839dbffc636 | 136 | if (c == 'd') { |
thomasstrub | 0:b839dbffc636 | 137 | m3pi.right(s); |
thomasstrub | 0:b839dbffc636 | 138 | wait(0.355); |
thomasstrub | 0:b839dbffc636 | 139 | } |
thomasstrub | 0:b839dbffc636 | 140 | wait(0.355); |
thomasstrub | 0:b839dbffc636 | 141 | } |
thomasstrub | 0:b839dbffc636 | 142 | |
thomasstrub | 0:b839dbffc636 | 143 | |
thomasstrub | 0:b839dbffc636 | 144 | //-------- paramétrage fréquence (pour bluetooth ) --------// |
thomasstrub | 0:b839dbffc636 | 145 | |
thomasstrub | 0:b839dbffc636 | 146 | void deffreq(int x) |
thomasstrub | 0:b839dbffc636 | 147 | { |
thomasstrub | 0:b839dbffc636 | 148 | bt.baud(x); |
thomasstrub | 0:b839dbffc636 | 149 | } |
thomasstrub | 0:b839dbffc636 | 150 | |
thomasstrub | 0:b839dbffc636 | 151 | |
thomasstrub | 0:b839dbffc636 | 152 | //---------- renvoi de message à l'utilisateur ------------// |
thomasstrub | 0:b839dbffc636 | 153 | |
thomasstrub | 0:b839dbffc636 | 154 | |
thomasstrub | 0:b839dbffc636 | 155 | void sendmsg(char* msg) |
thomasstrub | 0:b839dbffc636 | 156 | { |
thomasstrub | 0:b839dbffc636 | 157 | bt.printf("%s\n", msg); |
thomasstrub | 0:b839dbffc636 | 158 | } |
thomasstrub | 0:b839dbffc636 | 159 | |
thomasstrub | 0:b839dbffc636 | 160 | |
thomasstrub | 0:b839dbffc636 | 161 | //-------- recuperation de l info sur la direction ---------// |
thomasstrub | 0:b839dbffc636 | 162 | |
thomasstrub | 0:b839dbffc636 | 163 | |
thomasstrub | 0:b839dbffc636 | 164 | void getdir(char c) |
thomasstrub | 0:b839dbffc636 | 165 | { |
thomasstrub | 0:b839dbffc636 | 166 | c = bt.getc(); |
thomasstrub | 0:b839dbffc636 | 167 | } |
thomasstrub | 0:b839dbffc636 | 168 |