On_est_là / Mbed 2 deprecated pololu_mpi3

Version 1

Dependencies:   mbed m3pi_pops

main.cpp@2:4fede28fffa3, 2020-01-29 (annotated)

Committer:
acab1312
Date:
Wed Jan 29 14:26:15 2020 +0000
Revision:
2:4fede28fffa3
Parent:
1:ef658023014b 
PID Inclus; /Test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
emilienb 0:286c78fcd9dd 1 #include "mbed.h"
emilienb 0:286c78fcd9dd 2 #include "m3pi.h"
emilienb 0:286c78fcd9dd 3 //#include "MSCFileSystem.h"
emilienb 0:286c78fcd9dd 4
acab1312 2:4fede28fffa3 5 Serial usb(USBTX,USBRX);
acab1312 2:4fede28fffa3 6 //Serial usb(p28,p27);
emilienb 0:286c78fcd9dd 7
emilienb 0:286c78fcd9dd 8 DigitalOut l1(LED1);
emilienb 0:286c78fcd9dd 9 DigitalOut l2(LED2);
emilienb 0:286c78fcd9dd 10 DigitalOut l3(LED3);
emilienb 0:286c78fcd9dd 11 DigitalOut l4(LED4);
emilienb 0:286c78fcd9dd 12
acab1312 1:ef658023014b 13 m3pi m3pi;
acab1312 1:ef658023014b 14
acab1312 1:ef658023014b 15 // Minimum and maximum motor speeds
acab1312 2:4fede28fffa3 16 #define MAX 0.4
acab1312 1:ef658023014b 17 #define MIN 0
acab1312 1:ef658023014b 18
acab1312 1:ef658023014b 19 // PID terms
acab1312 1:ef658023014b 20 #define P_TERM 1
acab1312 1:ef658023014b 21 #define I_TERM 0
acab1312 1:ef658023014b 22 #define D_TERM 20
acab1312 1:ef658023014b 23
acab1312 1:ef658023014b 24 #define THRESHOLD 700
acab1312 1:ef658023014b 25
acab1312 1:ef658023014b 26 void PID(const float &cur, const float& prev, float& I, float& right, float& left)
acab1312 1:ef658023014b 27 {
acab1312 1:ef658023014b 28 float K;
acab1312 1:ef658023014b 29 float V = MAX;
acab1312 1:ef658023014b 30 float P,D=0;
acab1312 1:ef658023014b 31
acab1312 1:ef658023014b 32 // On calcul le proportionnel
acab1312 1:ef658023014b 33 P = cur;
acab1312 1:ef658023014b 34
acab1312 1:ef658023014b 35 // Calcul de la derivee
acab1312 1:ef658023014b 36 D = cur - prev;
acab1312 1:ef658023014b 37
acab1312 1:ef658023014b 38 // Calcul de l'intégral
acab1312 1:ef658023014b 39 I += P;
acab1312 1:ef658023014b 40
acab1312 1:ef658023014b 41 // On calcul le terme de correction
acab1312 1:ef658023014b 42 K = (P*(P_TERM)) + (I*(I_TERM)) + (D*(D_TERM)) ;
acab1312 1:ef658023014b 43
acab1312 1:ef658023014b 44 // On calcule les nouvelles vitesses
acab1312 1:ef658023014b 45 float tmpr = V+K;
acab1312 1:ef658023014b 46 float tmpl = V-K;
acab1312 1:ef658023014b 47
acab1312 1:ef658023014b 48 // On verifie les bornes
acab1312 1:ef658023014b 49 if (tmpr < MIN) tmpr = MIN;
acab1312 1:ef658023014b 50 else if (tmpr > MAX) tmpr = MAX;
acab1312 1:ef658023014b 51
acab1312 1:ef658023014b 52 if (tmpl < MIN) tmpl = MIN;
acab1312 1:ef658023014b 53 else if (tmpl > MAX) tmpl = MAX;
acab1312 1:ef658023014b 54
acab1312 1:ef658023014b 55 right=tmpr; left=tmpl;
acab1312 1:ef658023014b 56 }
acab1312 1:ef658023014b 57
acab1312 1:ef658023014b 58 void seuillage(int *cap, char *caps)
acab1312 1:ef658023014b 59 {
acab1312 1:ef658023014b 60 for(int i=0; i<5; i++){
acab1312 1:ef658023014b 61 if(cap[i] >= THRESHOLD)
acab1312 1:ef658023014b 62 caps[i] = 1;
acab1312 1:ef658023014b 63 else
acab1312 1:ef658023014b 64 caps[i] = 0;
acab1312 1:ef658023014b 65 }
acab1312 1:ef658023014b 66 }
emilienb 0:286c78fcd9dd 67
acab1312 1:ef658023014b 68 void getState(char* caps, char *state, char prev_state)
acab1312 1:ef658023014b 69 {
acab1312 1:ef658023014b 70 if(prev_state==0) //Initialisation
acab1312 1:ef658023014b 71 *state=1;
acab1312 1:ef658023014b 72 else if(prev_state==1)
acab1312 1:ef658023014b 73 {
acab1312 1:ef658023014b 74 if(caps[4]==1)
acab1312 1:ef658023014b 75 *state=2; //virage a droite
acab1312 1:ef658023014b 76 else if(caps[0]==1)
acab1312 1:ef658023014b 77 *state=3; //virage a gauche
acab1312 1:ef658023014b 78 else
acab1312 1:ef658023014b 79 *state=1; //ligne droite
acab1312 1:ef658023014b 80 }
acab1312 1:ef658023014b 81 else if(prev_state==2) //Si on était sur un virage a droite
acab1312 1:ef658023014b 82 {
acab1312 1:ef658023014b 83 if(caps[0]==0 && caps[4]==0) //et qu'on revient sur une ligne droite
acab1312 1:ef658023014b 84 *state=1;
acab1312 1:ef658023014b 85 }
acab1312 1:ef658023014b 86 else if(prev_state==3) //Si on était sur un virage a droite
acab1312 1:ef658023014b 87 {
acab1312 1:ef658023014b 88 if(caps[0]==0 && caps[4]==0)
acab1312 1:ef658023014b 89 *state=1;
acab1312 1:ef658023014b 90 }
acab1312 1:ef658023014b 91 }
acab1312 1:ef658023014b 92
acab1312 1:ef658023014b 93 //Fonction de correction
acab1312 1:ef658023014b 94 float getCorr(char state)
acab1312 1:ef658023014b 95 {
acab1312 1:ef658023014b 96 if(state==1)
acab1312 1:ef658023014b 97 return 2.0;
acab1312 1:ef658023014b 98 else
acab1312 1:ef658023014b 99 {
acab1312 1:ef658023014b 100 if(state==2)
acab1312 1:ef658023014b 101 return 0.7;
acab1312 1:ef658023014b 102 else if(state==3)
acab1312 1:ef658023014b 103 return -0.7;
acab1312 1:ef658023014b 104 }
acab1312 1:ef658023014b 105 }
acab1312 1:ef658023014b 106
acab1312 1:ef658023014b 107 int main() {
acab1312 1:ef658023014b 108 wait(1.0);
acab1312 1:ef658023014b 109 m3pi.sensor_auto_calibrate();
acab1312 1:ef658023014b 110
acab1312 1:ef658023014b 111 float right;
acab1312 1:ef658023014b 112 float left;
acab1312 1:ef658023014b 113 float cur = 0.0;
acab1312 1:ef658023014b 114 float prev = 0.0;
acab1312 1:ef658023014b 115 float I=0;
acab1312 1:ef658023014b 116
acab1312 1:ef658023014b 117 int cap[5]={1,1,1,1,1};
acab1312 1:ef658023014b 118 char caps[5]={0,0,0,0,0};
acab1312 1:ef658023014b 119
acab1312 1:ef658023014b 120 char state, prev_state=0;
acab1312 1:ef658023014b 121
acab1312 1:ef658023014b 122 float corr = 0.0;
acab1312 1:ef658023014b 123
acab1312 1:ef658023014b 124 while (1) {
acab1312 1:ef658023014b 125 //lecture du sensor
acab1312 1:ef658023014b 126 m3pi.readsensor(cap);
acab1312 1:ef658023014b 127 seuillage(cap,caps);
acab1312 1:ef658023014b 128
acab1312 1:ef658023014b 129 //On recupere l'etat
acab1312 1:ef658023014b 130 prev_state=state;
acab1312 1:ef658023014b 131 getState(caps,&state,prev_state);
acab1312 1:ef658023014b 132 corr=getCorr(state);
acab1312 1:ef658023014b 133
acab1312 1:ef658023014b 134 if(corr==2.0) //Signifie qu'on est sur une ligne droite
acab1312 1:ef658023014b 135 cur=m3pi.line_position();
acab1312 1:ef658023014b 136 else //Signifie qu'on est dans un etat particulier
acab1312 1:ef658023014b 137 cur=corr;
acab1312 1:ef658023014b 138
acab1312 1:ef658023014b 139 /*
acab1312 1:ef658023014b 140 //Condition d'etat
acab1312 1:ef658023014b 141 cur = m3pi.line_position(); // On recupere la position de la ligne
acab1312 1:ef658023014b 142 */
acab1312 1:ef658023014b 143
acab1312 1:ef658023014b 144 PID(cur,prev,I,right,left); //On calcule le PID
acab1312 1:ef658023014b 145 prev = cur; // Memorisation de l'etat precedent
acab1312 1:ef658023014b 146
acab1312 1:ef658023014b 147 //Envoi de la commande
acab1312 1:ef658023014b 148 m3pi.left_motor(left);
acab1312 1:ef658023014b 149 m3pi.right_motor(right);
acab1312 1:ef658023014b 150 }
acab1312 1:ef658023014b 151 }
acab1312 1:ef658023014b 152
acab1312 1:ef658023014b 153
acab1312 1:ef658023014b 154 /*
acab1312 1:ef658023014b 155 float speed = 0.1;
acab1312 1:ef658023014b 156 int seuil = 700;
acab1312 1:ef658023014b 157
acab1312 1:ef658023014b 158 int main() {
acab1312 1:ef658023014b 159
acab1312 1:ef658023014b 160 if(usb.writable())
acab1312 1:ef658023014b 161 usb.printf("Hello World");
emilienb 0:286c78fcd9dd 162
emilienb 0:286c78fcd9dd 163 int capteurs[5]={1,1,1,1,1};
acab1312 1:ef658023014b 164 int cap_seuil[5]={0,0,0,0,0};
acab1312 1:ef658023014b 165 float line_detec=0;
emilienb 0:286c78fcd9dd 166 char buff[8];
emilienb 0:286c78fcd9dd 167 char c,cprev;
acab1312 1:ef658023014b 168 while(1)
acab1312 1:ef658023014b 169 {
emilienb 0:286c78fcd9dd 170 //Boucle switch de reception de données via serial
emilienb 0:286c78fcd9dd 171 if(usb.readable()){
emilienb 0:286c78fcd9dd 172 cprev=c;
emilienb 0:286c78fcd9dd 173 c = usb.getc();
emilienb 0:286c78fcd9dd 174 switch(c){
emilienb 0:286c78fcd9dd 175 case 'z':
emilienb 0:286c78fcd9dd 176 pololu.forward(speed);
emilienb 0:286c78fcd9dd 177 l1 = 1;
emilienb 0:286c78fcd9dd 178 l2 = 0;
emilienb 0:286c78fcd9dd 179 l3 = 0;
emilienb 0:286c78fcd9dd 180 l4 = 0;
emilienb 0:286c78fcd9dd 181 break;
emilienb 0:286c78fcd9dd 182 case 's':
emilienb 0:286c78fcd9dd 183 pololu.backward(speed);
emilienb 0:286c78fcd9dd 184 l1 = 0;
emilienb 0:286c78fcd9dd 185 l2 = 1;
emilienb 0:286c78fcd9dd 186 l3 = 0;
emilienb 0:286c78fcd9dd 187 l4 = 0;
emilienb 0:286c78fcd9dd 188 break;
emilienb 0:286c78fcd9dd 189 case 'q':
acab1312 1:ef658023014b 190 pololu.left(speed);
emilienb 0:286c78fcd9dd 191 l1 = 1;
emilienb 0:286c78fcd9dd 192 l2 = 0;
emilienb 0:286c78fcd9dd 193 l3 = 0;
emilienb 0:286c78fcd9dd 194 l4 = 0;
emilienb 0:286c78fcd9dd 195 break;
emilienb 0:286c78fcd9dd 196 case 'd':
acab1312 1:ef658023014b 197 pololu.right(speed);
emilienb 0:286c78fcd9dd 198 l1 = 1;
emilienb 0:286c78fcd9dd 199 l2 = 0;
emilienb 0:286c78fcd9dd 200 l3 = 0;
emilienb 0:286c78fcd9dd 201 l4 = 0;
emilienb 0:286c78fcd9dd 202 break;
emilienb 0:286c78fcd9dd 203 case ' ':
emilienb 0:286c78fcd9dd 204 pololu.stop();
emilienb 0:286c78fcd9dd 205 l1 = 1;
emilienb 0:286c78fcd9dd 206 l2 = 1;
emilienb 0:286c78fcd9dd 207 l3 = 1;
emilienb 0:286c78fcd9dd 208 l4 = 1;
emilienb 0:286c78fcd9dd 209 break;
acab1312 1:ef658023014b 210
emilienb 0:286c78fcd9dd 211 case 'c':
emilienb 0:286c78fcd9dd 212 pololu.readsensor(capteurs);
acab1312 1:ef658023014b 213 for(int i=0; i<5; i++)
acab1312 1:ef658023014b 214 {
acab1312 1:ef658023014b 215 if(capteurs[i] >= seuil)
acab1312 1:ef658023014b 216 {
acab1312 1:ef658023014b 217 cap_seuil[i] = 1;
acab1312 1:ef658023014b 218 }
acab1312 1:ef658023014b 219 else
acab1312 1:ef658023014b 220 {
acab1312 1:ef658023014b 221 cap_seuil[i] = 0;
acab1312 1:ef658023014b 222 }
acab1312 1:ef658023014b 223 }
acab1312 1:ef658023014b 224
acab1312 1:ef658023014b 225 if(usb.writable())
acab1312 1:ef658023014b 226 {
acab1312 1:ef658023014b 227 usb.printf("Valeur capteur : \r\n");
acab1312 1:ef658023014b 228 usb.printf("Valeurs brutes: %d, %d, %d, %d, %d \r\n", (int)capteurs[0],(int)capteurs[1],(int)capteurs[2],(int)capteurs[3],(int)capteurs[4]);
acab1312 1:ef658023014b 229 usb.printf("Valeurs seuillees: %d, %d, %d, %d, %d \r\n \r\n ",cap_seuil[0],cap_seuil[1],cap_seuil[2],cap_seuil[3],cap_seuil[4]);
acab1312 1:ef658023014b 230 }
acab1312 1:ef658023014b 231 break;
acab1312 1:ef658023014b 232
acab1312 1:ef658023014b 233 case '0':
acab1312 1:ef658023014b 234 pololu.sensor_auto_calibrate();
acab1312 1:ef658023014b 235 break;
acab1312 1:ef658023014b 236
emilienb 0:286c78fcd9dd 237 case 'b':
emilienb 0:286c78fcd9dd 238 sprintf(buff, "Bat:%3.f",pololu.battery());
emilienb 0:286c78fcd9dd 239 pololu.cls();
emilienb 0:286c78fcd9dd 240 pololu.locate(0,0);
emilienb 0:286c78fcd9dd 241 pololu.print(buff,8);
acab1312 1:ef658023014b 242
acab1312 1:ef658023014b 243 case 'l':
acab1312 1:ef658023014b 244 line_detec = pololu.line_position();
acab1312 1:ef658023014b 245 if(usb.writable())
acab1312 1:ef658023014b 246 usb.printf("Position ligne : %f\r\n",line_detec);
acab1312 1:ef658023014b 247
emilienb 0:286c78fcd9dd 248
emilienb 0:286c78fcd9dd 249 default : break;
emilienb 0:286c78fcd9dd 250 }
emilienb 0:286c78fcd9dd 251 }
emilienb 0:286c78fcd9dd 252 }
acab1312 1:ef658023014b 253 }
acab1312 1:ef658023014b 254 */