Polybot / Mbed 2 deprecated mbed_BT_Control

Programme de controle du robot joueur

Dependencies:   m3piBluetooth mbed

Fork of Serial_HelloWorld_Mbed by mbed official

main.cpp@1:6d8c50dcae28, 2018-05-03 (annotated)

Committer:
Maximousse
Date:
Thu May 03 13:00:03 2018 +0000
Revision:
1:6d8c50dcae28
Parent:
0:879aa9d0247b 
BT_Control_commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Maximousse 1:6d8c50dcae28 1
Maximousse 1:6d8c50dcae28 2
Maximousse 1:6d8c50dcae28 3
Maximousse 1:6d8c50dcae28 4 //-------------------------BT CONTROL
mbed_official 0:879aa9d0247b 5 #include "mbed.h"
Maximousse 1:6d8c50dcae28 6 #include "m3pi.h"
Maximousse 1:6d8c50dcae28 7 //#include "MSCFileSystem.h"
Maximousse 1:6d8c50dcae28 8
Maximousse 1:6d8c50dcae28 9 Serial pc(USBTX, USBRX); //tx, rx
Maximousse 1:6d8c50dcae28 10 Serial bt(p28,p27); //bluetooth
Maximousse 1:6d8c50dcae28 11
Maximousse 1:6d8c50dcae28 12
Maximousse 1:6d8c50dcae28 13 DigitalOut myled1(LED1);
Maximousse 1:6d8c50dcae28 14 DigitalOut myled2(LED2);
Maximousse 1:6d8c50dcae28 15 DigitalOut myled3(LED3);
Maximousse 1:6d8c50dcae28 16 DigitalOut myled4(LED4);
Maximousse 1:6d8c50dcae28 17 DigitalOut leds [4] = {LED1, LED2, LED3, LED4};
Maximousse 1:6d8c50dcae28 18
Maximousse 1:6d8c50dcae28 19 m3pi m3pi;
Maximousse 1:6d8c50dcae28 20
Maximousse 1:6d8c50dcae28 21 //MSCFileSystem fs("fs");
Maximousse 1:6d8c50dcae28 22
Maximousse 1:6d8c50dcae28 23 BusOut myleds(LED1, LED2, LED3, LED4);
Maximousse 1:6d8c50dcae28 24
Maximousse 1:6d8c50dcae28 25 const int THRESH = 300;
Maximousse 1:6d8c50dcae28 26
Maximousse 1:6d8c50dcae28 27 unsigned short sensors[5], mems[5], nbCapt, memsNb;
Maximousse 1:6d8c50dcae28 28 char tab[4];
Maximousse 1:6d8c50dcae28 29 float position, proportional, last_proportional, integral;
Maximousse 1:6d8c50dcae28 30
Maximousse 1:6d8c50dcae28 31 unsigned char dir;
Maximousse 1:6d8c50dcae28 32 float speed, duration, s1, s2;
Maximousse 1:6d8c50dcae28 33
Maximousse 1:6d8c50dcae28 34
Maximousse 1:6d8c50dcae28 35 void getCapt(bool mem = false)
Maximousse 1:6d8c50dcae28 36 {
Maximousse 1:6d8c50dcae28 37 int v = 0;
Maximousse 1:6d8c50dcae28 38 nbCapt = 0;
Maximousse 1:6d8c50dcae28 39
Maximousse 1:6d8c50dcae28 40 m3pi.calibrated_sensors(sensors);
Maximousse 1:6d8c50dcae28 41
Maximousse 1:6d8c50dcae28 42 for(int i=0 ; i<5 ;i++)
Maximousse 1:6d8c50dcae28 43 {
Maximousse 1:6d8c50dcae28 44 sensors[i] = (sensors[i] > THRESH ? true : false);
Maximousse 1:6d8c50dcae28 45 if(sensors[i])
Maximousse 1:6d8c50dcae28 46 {
Maximousse 1:6d8c50dcae28 47 v += 0x1<<i;
Maximousse 1:6d8c50dcae28 48 nbCapt++;
Maximousse 1:6d8c50dcae28 49 }
Maximousse 1:6d8c50dcae28 50 if(mem)
Maximousse 1:6d8c50dcae28 51 {
Maximousse 1:6d8c50dcae28 52 mems[i] = sensors[i];
Maximousse 1:6d8c50dcae28 53 memsNb = nbCapt;
Maximousse 1:6d8c50dcae28 54 }
Maximousse 1:6d8c50dcae28 55 }
Maximousse 1:6d8c50dcae28 56 m3pi.leds(v);
Maximousse 1:6d8c50dcae28 57 }
Maximousse 1:6d8c50dcae28 58
Maximousse 1:6d8c50dcae28 59
Maximousse 1:6d8c50dcae28 60 void set_motors(float mr, float ml)
Maximousse 1:6d8c50dcae28 61 {
Maximousse 1:6d8c50dcae28 62 m3pi.left_motor(ml);
Maximousse 1:6d8c50dcae28 63 m3pi.right_motor(mr);
Maximousse 1:6d8c50dcae28 64 }
Maximousse 1:6d8c50dcae28 65
Maximousse 1:6d8c50dcae28 66 void suiviIntersect(float intervalTour = 0.0f)
Maximousse 1:6d8c50dcae28 67 {
Maximousse 1:6d8c50dcae28 68 bt.printf("startSuivi_");
Maximousse 1:6d8c50dcae28 69 bool done = false;
Maximousse 1:6d8c50dcae28 70 unsigned int idx=0;
Maximousse 1:6d8c50dcae28 71 float vl, vr, omega, l_k=0, l_k1=0, sum=0, vmax=0.50f;
Maximousse 1:6d8c50dcae28 72
Maximousse 1:6d8c50dcae28 73 #if 1 // Low speed
Maximousse 1:6d8c50dcae28 74 const float Ki=0.005f, Kp=0.5f, Kd=1.5f, v=0.2;
Maximousse 1:6d8c50dcae28 75 #else
Maximousse 1:6d8c50dcae28 76 const float Ki=0.0005f, Kp=0.05f, Kd=1.5f, v=0.80f;
Maximousse 1:6d8c50dcae28 77 #endif
Maximousse 1:6d8c50dcae28 78 m3pi.sensor_auto_calibrate();
Maximousse 1:6d8c50dcae28 79 wait(1.0);
Maximousse 1:6d8c50dcae28 80 bt.printf("---> While in<--- ");
Maximousse 1:6d8c50dcae28 81 Timer t; t.start();
Maximousse 1:6d8c50dcae28 82 while(!done)
Maximousse 1:6d8c50dcae28 83 {
Maximousse 1:6d8c50dcae28 84 idx++;
Maximousse 1:6d8c50dcae28 85 getCapt();
Maximousse 1:6d8c50dcae28 86 // PID
Maximousse 1:6d8c50dcae28 87 l_k1 = l_k;
Maximousse 1:6d8c50dcae28 88 l_k = m3pi.line_position();
Maximousse 1:6d8c50dcae28 89 //v = sp;
Maximousse 1:6d8c50dcae28 90 omega = l_k*Kp; // Proportionnal
Maximousse 1:6d8c50dcae28 91 sum += l_k;
Maximousse 1:6d8c50dcae28 92 omega += sum*Ki; // Integral
Maximousse 1:6d8c50dcae28 93 omega += (l_k-l_k1)*Kd;
Maximousse 1:6d8c50dcae28 94 //omega *= v;
Maximousse 1:6d8c50dcae28 95
Maximousse 1:6d8c50dcae28 96
Maximousse 1:6d8c50dcae28 97 vl = v+omega < vmax ? (v+omega > -vmax ? v+omega : vmax) : vmax;
Maximousse 1:6d8c50dcae28 98 vr = v-omega < vmax ? (v-omega > -vmax ? v-omega : -vmax): vmax;
Maximousse 1:6d8c50dcae28 99
Maximousse 1:6d8c50dcae28 100
Maximousse 1:6d8c50dcae28 101 //------------------------------------- Pour les zigzags
Maximousse 1:6d8c50dcae28 102 //getCurrentSerial().printf("? hola que pasa ?");
Maximousse 1:6d8c50dcae28 103
Maximousse 1:6d8c50dcae28 104 //-------------------------------------
Maximousse 1:6d8c50dcae28 105
Maximousse 1:6d8c50dcae28 106 //-----------------Intersection
Maximousse 1:6d8c50dcae28 107 if(sensors[0] || sensors[4])
Maximousse 1:6d8c50dcae28 108 {
Maximousse 1:6d8c50dcae28 109 done = true;
Maximousse 1:6d8c50dcae28 110 }
Maximousse 1:6d8c50dcae28 111 set_motors(vr,vl);
Maximousse 1:6d8c50dcae28 112 }
Maximousse 1:6d8c50dcae28 113 bt.printf("-->While out<--");
Maximousse 1:6d8c50dcae28 114
Maximousse 1:6d8c50dcae28 115 return;
Maximousse 1:6d8c50dcae28 116 }
Maximousse 1:6d8c50dcae28 117
Maximousse 1:6d8c50dcae28 118
Maximousse 1:6d8c50dcae28 119
Maximousse 1:6d8c50dcae28 120
Maximousse 1:6d8c50dcae28 121
Maximousse 1:6d8c50dcae28 122
Maximousse 1:6d8c50dcae28 123
Maximousse 1:6d8c50dcae28 124
Maximousse 1:6d8c50dcae28 125
Maximousse 1:6d8c50dcae28 126 void assignDirection(char c, float s){
Maximousse 1:6d8c50dcae28 127 if (c == 'a') m3pi.left(s);
Maximousse 1:6d8c50dcae28 128 if (c == 'c') m3pi.right(s);
Maximousse 1:6d8c50dcae28 129 if (c == 'd') {
Maximousse 1:6d8c50dcae28 130 m3pi.right(s);
Maximousse 1:6d8c50dcae28 131 wait(0.355);
Maximousse 1:6d8c50dcae28 132 }
Maximousse 1:6d8c50dcae28 133 wait(0.355);
Maximousse 1:6d8c50dcae28 134 }
Maximousse 1:6d8c50dcae28 135
Maximousse 1:6d8c50dcae28 136
Maximousse 1:6d8c50dcae28 137
Maximousse 1:6d8c50dcae28 138
mbed_official 0:879aa9d0247b 139 int main() {
Maximousse 1:6d8c50dcae28 140 bt.baud(115200);
Maximousse 1:6d8c50dcae28 141 char c = 'a';
Maximousse 1:6d8c50dcae28 142 bt.printf("Hello World!\n");
mbed_official 0:879aa9d0247b 143 while(1) {
Maximousse 1:6d8c50dcae28 144 m3pi.stop();
Maximousse 1:6d8c50dcae28 145 c = bt.getc();
Maximousse 1:6d8c50dcae28 146 bt.printf("Je viens de recevoir : %c \n", c);
Maximousse 1:6d8c50dcae28 147 //move
Maximousse 1:6d8c50dcae28 148 assignDirection(c, 0.2);
Maximousse 1:6d8c50dcae28 149 getCapt();
Maximousse 1:6d8c50dcae28 150 suiviIntersect();
mbed_official 0:879aa9d0247b 151 }
Maximousse 1:6d8c50dcae28 152 }
Maximousse 1:6d8c50dcae28 153