Polybot / Mbed 2 deprecated M3PI_SuiviLigne

The program

Dependencies:   FileSystem_POPS Sound m3pi_test2 mbed

Fork of M3PI_SuiviLigne by Maxime Chevreau

main3.cpp@4:22275847875d, 2018-05-03 (annotated)

Committer:
Maximousse
Date:
Thu May 03 12:08:03 2018 +0000
Revision:
4:22275847875d
Parent:
0:398afdd73d9e 
test3;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bouaziz 0:398afdd73d9e 1 #include "mbed.h"
bouaziz 0:398afdd73d9e 2 #include "m3pi.h"
bouaziz 0:398afdd73d9e 3 #include "MSCFileSystem.h"
bouaziz 0:398afdd73d9e 4
bouaziz 0:398afdd73d9e 5 m3pi m3pi; // Initialise the m3pi
bouaziz 0:398afdd73d9e 6
bouaziz 0:398afdd73d9e 7 DigitalOut resetxbee(p26);
Maximousse 4:22275847875d 8 DigitalOut led3(LED3);
Maximousse 4:22275847875d 9 DigitalOut led1(LED1);
Maximousse 4:22275847875d 10 DigitalIn push(p21);
Maximousse 4:22275847875d 11
Maximousse 4:22275847875d 12 Serial pc(USBTX, USBRX); // For debugging and pc messages, uses commented out to prevent hangings
Maximousse 4:22275847875d 13 Serial bt(p28,p27);
Maximousse 4:22275847875d 14
bouaziz 0:398afdd73d9e 15 MSCFileSystem fs("fs");
bouaziz 0:398afdd73d9e 16
bouaziz 0:398afdd73d9e 17 BusOut myleds(LED1, LED2, LED3, LED4);
bouaziz 0:398afdd73d9e 18
Maximousse 4:22275847875d 19 const int THRESH = 200;
Maximousse 4:22275847875d 20
Maximousse 4:22275847875d 21 unsigned short sensors[5], mems[5], nbCapt, memsNb;
Maximousse 4:22275847875d 22 char tab[4];
Maximousse 4:22275847875d 23 float position, proportional, last_proportional, integral;
Maximousse 4:22275847875d 24
Maximousse 4:22275847875d 25 unsigned char dir;
Maximousse 4:22275847875d 26 float speed, duration, s1, s2;
Maximousse 4:22275847875d 27 char src='x';
Maximousse 4:22275847875d 28
Maximousse 4:22275847875d 29 Timer timer;
Maximousse 4:22275847875d 30
Maximousse 4:22275847875d 31
Maximousse 4:22275847875d 32
Maximousse 4:22275847875d 33
Maximousse 4:22275847875d 34
Maximousse 4:22275847875d 35
Maximousse 4:22275847875d 36
Maximousse 4:22275847875d 37
Maximousse 4:22275847875d 38
Maximousse 4:22275847875d 39 Serial& getCurrentSerial()
Maximousse 4:22275847875d 40 {
Maximousse 4:22275847875d 41 return (src == 'u' ? pc : bt);
Maximousse 4:22275847875d 42 }
Maximousse 4:22275847875d 43
Maximousse 4:22275847875d 44
Maximousse 4:22275847875d 45
Maximousse 4:22275847875d 46
Maximousse 4:22275847875d 47 void set_motors(float mr, float ml)
Maximousse 4:22275847875d 48 {
Maximousse 4:22275847875d 49 m3pi.left_motor(ml);
Maximousse 4:22275847875d 50 m3pi.right_motor(mr);
Maximousse 4:22275847875d 51 }
Maximousse 4:22275847875d 52
Maximousse 4:22275847875d 53 //------------------------------------------------------------------------------
Maximousse 4:22275847875d 54
Maximousse 4:22275847875d 55 void getCapt(bool mem = false)
Maximousse 4:22275847875d 56 {
Maximousse 4:22275847875d 57 int v = 0;
Maximousse 4:22275847875d 58 nbCapt = 0;
bouaziz 0:398afdd73d9e 59
Maximousse 4:22275847875d 60 m3pi.calibrated_sensors(sensors);
Maximousse 4:22275847875d 61
Maximousse 4:22275847875d 62 for(int i=0 ; i<5 ;i++)
Maximousse 4:22275847875d 63 {
Maximousse 4:22275847875d 64 sensors[i] = (sensors[i] > THRESH ? true : false);
Maximousse 4:22275847875d 65 if(sensors[i])
Maximousse 4:22275847875d 66 {
Maximousse 4:22275847875d 67 v += 0x1<<i;
Maximousse 4:22275847875d 68 nbCapt++;
Maximousse 4:22275847875d 69 }
Maximousse 4:22275847875d 70 if(mem)
Maximousse 4:22275847875d 71 {
Maximousse 4:22275847875d 72 mems[i] = sensors[i];
Maximousse 4:22275847875d 73 memsNb = nbCapt;
Maximousse 4:22275847875d 74 }
Maximousse 4:22275847875d 75 }
Maximousse 4:22275847875d 76 m3pi.leds(v);
Maximousse 4:22275847875d 77 }
Maximousse 4:22275847875d 78
Maximousse 4:22275847875d 79 //------------------------------------------------------------------------------
Maximousse 4:22275847875d 80
Maximousse 4:22275847875d 81 void suiviIntersect(float intervalTour = 0.0f)
Maximousse 4:22275847875d 82 {
Maximousse 4:22275847875d 83 bool done = false;
Maximousse 4:22275847875d 84 //forward = true;
Maximousse 4:22275847875d 85 unsigned int idx=0;
Maximousse 4:22275847875d 86 float vl, vr, omega, l_k=0, l_k1=0, sum=0, vmax=1.0f;
Maximousse 4:22275847875d 87
Maximousse 4:22275847875d 88 #if 1 // Low speed
Maximousse 4:22275847875d 89 const float Ki=0.007f, Kp=0.7f, Kd=1.5f, v=0.2;
Maximousse 4:22275847875d 90 #else
Maximousse 4:22275847875d 91 const float Ki=0.0007f, Kp=0.07f, Kd=1.5f, v=0.80f;
Maximousse 4:22275847875d 92 #endif
bouaziz 0:398afdd73d9e 93 m3pi.sensor_auto_calibrate();
Maximousse 4:22275847875d 94 wait(1.0);
bouaziz 0:398afdd73d9e 95
Maximousse 4:22275847875d 96 Timer t; t.start();
Maximousse 4:22275847875d 97 while(!done)
Maximousse 4:22275847875d 98 {
Maximousse 4:22275847875d 99 idx++;
Maximousse 4:22275847875d 100 getCapt();
Maximousse 4:22275847875d 101
Maximousse 4:22275847875d 102 // PID
Maximousse 4:22275847875d 103 l_k1 = l_k;
Maximousse 4:22275847875d 104 l_k = m3pi.line_position();
Maximousse 4:22275847875d 105 //v = sp;
Maximousse 4:22275847875d 106 omega = l_k*Kp; // Proportionnal
Maximousse 4:22275847875d 107 sum += l_k;
Maximousse 4:22275847875d 108 omega += sum*Ki; // Integral
Maximousse 4:22275847875d 109 omega += (l_k-l_k1)*Kd;
bouaziz 0:398afdd73d9e 110
bouaziz 0:398afdd73d9e 111
Maximousse 4:22275847875d 112 vl = v+omega < vmax ? (v+omega > -vmax ? v+omega : vmax) : vmax;
Maximousse 4:22275847875d 113 vr = v-omega < vmax ? (v-omega > -vmax ? v-omega : -vmax): vmax;
Maximousse 4:22275847875d 114
Maximousse 4:22275847875d 115 vr *= 1.2;
Maximousse 4:22275847875d 116 vl *= 1.2;
Maximousse 4:22275847875d 117 set_motors(vr,vl);
Maximousse 4:22275847875d 118
Maximousse 4:22275847875d 119 //---------------------INTERSECTION
Maximousse 4:22275847875d 120 if(sensors[0] || sensors[4]) {
Maximousse 4:22275847875d 121 char c = 'a';
Maximousse 4:22275847875d 122 //if ((c<='5') & (c>='1')) {
Maximousse 4:22275847875d 123 // leds[c - '1'] = !leds[c - '1'];
Maximousse 4:22275847875d 124 //}
Maximousse 4:22275847875d 125 bt.printf("Voici un %c \n", c);
Maximousse 4:22275847875d 126 }
Maximousse 4:22275847875d 127
Maximousse 4:22275847875d 128 /*
Maximousse 4:22275847875d 129 if(forward) set_motors(vr,vl);
Maximousse 4:22275847875d 130 else set_motors(-vl,-vr);
Maximousse 4:22275847875d 131 */
Maximousse 4:22275847875d 132
Maximousse 4:22275847875d 133 m3pi.cls();
Maximousse 4:22275847875d 134 //const char* C [1]= {'0' + idx};
Maximousse 4:22275847875d 135 //m3pi.printf(C);
bouaziz 0:398afdd73d9e 136 }
Maximousse 4:22275847875d 137
Maximousse 4:22275847875d 138 return;
bouaziz 0:398afdd73d9e 139 }
Maximousse 4:22275847875d 140
Maximousse 4:22275847875d 141
Maximousse 4:22275847875d 142
Maximousse 4:22275847875d 143
Maximousse 4:22275847875d 144
Maximousse 4:22275847875d 145
Maximousse 4:22275847875d 146
Maximousse 4:22275847875d 147
Maximousse 4:22275847875d 148
Maximousse 4:22275847875d 149
Maximousse 4:22275847875d 150 int main() {
Maximousse 4:22275847875d 151 bt.baud(115200);
Maximousse 4:22275847875d 152 m3pi.cls();
Maximousse 4:22275847875d 153 wait(0.5);
Maximousse 4:22275847875d 154
Maximousse 4:22275847875d 155 m3pi.sensor_auto_calibrate();
Maximousse 4:22275847875d 156
Maximousse 4:22275847875d 157 set_motors(0.f,0.f);
Maximousse 4:22275847875d 158 char c = 'a';
Maximousse 4:22275847875d 159 bt.printf("Hello World!\n");
Maximousse 4:22275847875d 160
Maximousse 4:22275847875d 161 while(1)
Maximousse 4:22275847875d 162 {
Maximousse 4:22275847875d 163 getCapt();
Maximousse 4:22275847875d 164 suiviIntersect();
Maximousse 4:22275847875d 165 }
Maximousse 4:22275847875d 166 }