Polybot / Mbed 2 deprecated polulu_pacman

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?

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