Freescale_Cachan / Mbed 2 deprecated Programme_course_JourJ1

Programme course fonctionnel Jour J Croisement non pris en charge (tourne en rond)

Dependencies:   MMA8451Q mbed

Fork of Programme_course_30Tr by Freescale_Cachan

Mlib.cpp@5:7714a216c91d, 2017-03-24 (annotated)

Committer:
leandre
Date:
Fri Mar 24 09:29:12 2017 +0000
Revision:
5:7714a216c91d
Parent:
4:8b0215ec788f 
Prog fonctionnel Jour J; Croisement ? r?gler

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Freescale_cup 0:3ec7fc598e48 1 #include "Mlib.h"
Freescale_cup 0:3ec7fc598e48 2
Freescale_cup 0:3ec7fc598e48 3 /***********AUTRES_FCT***************/
Freescale_cup 0:3ec7fc598e48 4
Freescale_cup 0:3ec7fc598e48 5 char middle_3(char *tab)
Freescale_cup 0:3ec7fc598e48 6 {
Freescale_cup 0:3ec7fc598e48 7 tri_a_bulle(tab, 3);
Freescale_cup 0:3ec7fc598e48 8 return tab[1];
Freescale_cup 0:3ec7fc598e48 9 }
Freescale_cup 0:3ec7fc598e48 10
Freescale_cup 0:3ec7fc598e48 11 int middle_3U(int *tab)
Freescale_cup 0:3ec7fc598e48 12 {
Freescale_cup 0:3ec7fc598e48 13 tri_a_bulleU(tab, 3);
Freescale_cup 0:3ec7fc598e48 14 return tab[1];
Freescale_cup 0:3ec7fc598e48 15 }
Freescale_cup 0:3ec7fc598e48 16
Freescale_cup 0:3ec7fc598e48 17 float middle_3F(float *tab)
Freescale_cup 0:3ec7fc598e48 18 {
Freescale_cup 0:3ec7fc598e48 19
Freescale_cup 0:3ec7fc598e48 20 return tri_a_bulleF(tab, 3);
Freescale_cup 0:3ec7fc598e48 21 }
Freescale_cup 0:3ec7fc598e48 22
Freescale_cup 0:3ec7fc598e48 23
Freescale_cup 0:3ec7fc598e48 24 void tri_a_bulle(char* t, int const size)
Freescale_cup 0:3ec7fc598e48 25 {
Freescale_cup 0:3ec7fc598e48 26 int en_desordre = 1;
Freescale_cup 0:3ec7fc598e48 27 int i,j;
Freescale_cup 0:3ec7fc598e48 28
Freescale_cup 0:3ec7fc598e48 29 for (i = 0; (i < size) && en_desordre; ++i)
Freescale_cup 0:3ec7fc598e48 30 {
Freescale_cup 0:3ec7fc598e48 31 en_desordre = 0;
Freescale_cup 0:3ec7fc598e48 32 for (j = 1; j < (size - i); ++j)
Freescale_cup 0:3ec7fc598e48 33 {
Freescale_cup 0:3ec7fc598e48 34 if (t[j-1] > t[j])
Freescale_cup 0:3ec7fc598e48 35 {
Freescale_cup 0:3ec7fc598e48 36 int temp = t[j-1];
Freescale_cup 0:3ec7fc598e48 37 t[j-1] = t[j];
Freescale_cup 0:3ec7fc598e48 38 t[j] = temp;
Freescale_cup 0:3ec7fc598e48 39 en_desordre = 1;
Freescale_cup 0:3ec7fc598e48 40 }
Freescale_cup 0:3ec7fc598e48 41 }
Freescale_cup 0:3ec7fc598e48 42 }
Freescale_cup 0:3ec7fc598e48 43 }
Freescale_cup 0:3ec7fc598e48 44 void tri_a_bulleU(int* t, int const size)
Freescale_cup 0:3ec7fc598e48 45 {
Freescale_cup 0:3ec7fc598e48 46 int en_desordre = 1;
Freescale_cup 0:3ec7fc598e48 47 int i,j;
Freescale_cup 0:3ec7fc598e48 48
Freescale_cup 0:3ec7fc598e48 49 for (i = 0; (i < size) && en_desordre; ++i)
Freescale_cup 0:3ec7fc598e48 50 {
Freescale_cup 0:3ec7fc598e48 51 en_desordre = 0;
Freescale_cup 0:3ec7fc598e48 52 for (j = 1; j < (size - i); ++j)
Freescale_cup 0:3ec7fc598e48 53 {
Freescale_cup 0:3ec7fc598e48 54 if (t[j-1] > t[j])
Freescale_cup 0:3ec7fc598e48 55 {
Freescale_cup 0:3ec7fc598e48 56 int temp = t[j-1];
Freescale_cup 0:3ec7fc598e48 57 t[j-1] = t[j];
Freescale_cup 0:3ec7fc598e48 58 t[j] = temp;
Freescale_cup 0:3ec7fc598e48 59 en_desordre = 1;
Freescale_cup 0:3ec7fc598e48 60 }
Freescale_cup 0:3ec7fc598e48 61 }
Freescale_cup 0:3ec7fc598e48 62 }
Freescale_cup 0:3ec7fc598e48 63 }
Freescale_cup 0:3ec7fc598e48 64 float tri_a_bulleF(float* t2, int const size)
Freescale_cup 0:3ec7fc598e48 65 {
Freescale_cup 0:3ec7fc598e48 66 float t[3];
Freescale_cup 0:3ec7fc598e48 67 t[0] = t2[0];
Freescale_cup 0:3ec7fc598e48 68 t[1] = t2[1];
Freescale_cup 0:3ec7fc598e48 69 t[2] = t2[2];
Freescale_cup 0:3ec7fc598e48 70
Freescale_cup 0:3ec7fc598e48 71
Freescale_cup 0:3ec7fc598e48 72 int en_desordre = 1;
Freescale_cup 0:3ec7fc598e48 73 int i,j;
Freescale_cup 0:3ec7fc598e48 74
Freescale_cup 0:3ec7fc598e48 75 for (i = 0; (i < size) && en_desordre; ++i)
Freescale_cup 0:3ec7fc598e48 76 {
Freescale_cup 0:3ec7fc598e48 77 en_desordre = 0;
Freescale_cup 0:3ec7fc598e48 78 for (j = 1; j < (size - i); ++j)
Freescale_cup 0:3ec7fc598e48 79 {
Freescale_cup 0:3ec7fc598e48 80 if (t[j-1] > t[j])
Freescale_cup 0:3ec7fc598e48 81 {
Freescale_cup 0:3ec7fc598e48 82 float temp = t[j-1];
Freescale_cup 0:3ec7fc598e48 83 t[j-1] = t[j];
Freescale_cup 0:3ec7fc598e48 84 t[j] = temp;
Freescale_cup 0:3ec7fc598e48 85 en_desordre = 1;
Freescale_cup 0:3ec7fc598e48 86 }
Freescale_cup 0:3ec7fc598e48 87 }
Freescale_cup 0:3ec7fc598e48 88 }
Freescale_cup 0:3ec7fc598e48 89 return t[1];
Freescale_cup 0:3ec7fc598e48 90 }
Freescale_cup 0:3ec7fc598e48 91
Freescale_cup 0:3ec7fc598e48 92 unsigned char Moyenne(unsigned char *tab)
Freescale_cup 0:3ec7fc598e48 93 {
Freescale_cup 0:3ec7fc598e48 94 int i = 0;
Freescale_cup 0:3ec7fc598e48 95 int somme = 0;
Freescale_cup 0:3ec7fc598e48 96 for(i = 0; i < 128;i++)
Freescale_cup 0:3ec7fc598e48 97 {
Freescale_cup 0:3ec7fc598e48 98 somme+=tab[i];
Freescale_cup 0:3ec7fc598e48 99 }
Freescale_cup 0:3ec7fc598e48 100 return somme/128.0;
Freescale_cup 0:3ec7fc598e48 101 }
Freescale_cup 0:3ec7fc598e48 102 char Moyenne2(char *tab, int nb)
Freescale_cup 0:3ec7fc598e48 103 {
Freescale_cup 0:3ec7fc598e48 104 int i = 0;
Freescale_cup 0:3ec7fc598e48 105 int somme = 0;
Freescale_cup 0:3ec7fc598e48 106 for(i = 0; i < nb;i++)
Freescale_cup 0:3ec7fc598e48 107 {
Freescale_cup 0:3ec7fc598e48 108 somme+=tab[i];
Freescale_cup 0:3ec7fc598e48 109 }
Freescale_cup 0:3ec7fc598e48 110 return float(somme/nb);
Freescale_cup 0:3ec7fc598e48 111 }
Freescale_cup 0:3ec7fc598e48 112
Freescale_cup 0:3ec7fc598e48 113 void menu()
Freescale_cup 0:3ec7fc598e48 114 {
Freescale_cup 0:3ec7fc598e48 115 static int p = 0;
Freescale_cup 0:3ec7fc598e48 116 p++;
Freescale_cup 0:3ec7fc598e48 117 wait(0.1);
Freescale_cup 0:3ec7fc598e48 118 if(BP2)
Freescale_cup 0:3ec7fc598e48 119 {
Freescale_cup 0:3ec7fc598e48 120 seuil_der++;
Freescale_cup 0:3ec7fc598e48 121 if(seuil_der == 2) seuil_der = -1;
Freescale_cup 0:3ec7fc598e48 122 led1 = 0;
Freescale_cup 0:3ec7fc598e48 123 wait(0.5);
Freescale_cup 0:3ec7fc598e48 124 }
Freescale_cup 0:3ec7fc598e48 125 if(seuil_der == 0)
Freescale_cup 0:3ec7fc598e48 126 {
Freescale_cup 0:3ec7fc598e48 127 led1 = p%2;
Freescale_cup 0:3ec7fc598e48 128 }
Freescale_cup 0:3ec7fc598e48 129 else if(seuil_der == 1)
Freescale_cup 0:3ec7fc598e48 130 {
Freescale_cup 0:3ec7fc598e48 131 led1 = 1;
Freescale_cup 0:3ec7fc598e48 132 }
Freescale_cup 0:3ec7fc598e48 133 else
Freescale_cup 0:3ec7fc598e48 134 {
Freescale_cup 0:3ec7fc598e48 135 led1 = 0;
Freescale_cup 0:3ec7fc598e48 136 }
Freescale_cup 0:3ec7fc598e48 137 if(S1 == 1)
Freescale_cup 0:3ec7fc598e48 138 {
Freescale_cup 0:3ec7fc598e48 139 led2 = 1;
Freescale_cup 0:3ec7fc598e48 140 }
Freescale_cup 0:3ec7fc598e48 141 else led2 = 0;
Freescale_cup 0:3ec7fc598e48 142 if(S2 == 1)
Freescale_cup 0:3ec7fc598e48 143 {
Freescale_cup 0:3ec7fc598e48 144 led3 = 1;
Freescale_cup 0:3ec7fc598e48 145 }
Freescale_cup 0:3ec7fc598e48 146 else led3= 0;
Freescale_cup 0:3ec7fc598e48 147 }
Freescale_cup 0:3ec7fc598e48 148
Freescale_cup 0:3ec7fc598e48 149 void fin_course()
Freescale_cup 0:3ec7fc598e48 150 {
Freescale_cup 0:3ec7fc598e48 151 static int arr = 0;
leandre 4:8b0215ec788f 152 static char moy_inter, moy_mem_a,moy_mem_b;
leandre 4:8b0215ec788f 153 static char tab_mem_b[TAILLE_TAB_INT]={0,0,0};
leandre 4:8b0215ec788f 154 static char tab_mem_a[TAILLE_TAB_INT]={0,0,0};
leandre 4:8b0215ec788f 155
leandre 4:8b0215ec788f 156 if (t_debut.read_ms()>2000){
leandre 4:8b0215ec788f 157 moy_inter=Moyenne2(Interval, TAILLE_TAB_INT);
leandre 4:8b0215ec788f 158 moy_mem_a=Moyenne2(tab_mem_a, TAILLE_TAB_INT);
leandre 4:8b0215ec788f 159 moy_mem_b=Moyenne2(tab_mem_b, TAILLE_TAB_INT);
leandre 2:a37de5eb8bf3 160
leandre 2:a37de5eb8bf3 161 //pc.printf("%d\n",moy_inter);
leandre 5:7714a216c91d 162 if(S1&&(arr!=0)){//delai apres l'arret
leandre 5:7714a216c91d 163 if (t_debut.read_ms()-arr>250)VIT=0;
leandre 4:8b0215ec788f 164 }
leandre 4:8b0215ec788f 165 else if((moy_mem_a>20)&&(moy_mem_b>20)){ // si la voiture n'est pas trop sur l'exterieur de la piste
leandre 4:8b0215ec788f 166 if(S1 && (moy_inter<75) && (moy_inter>50)){ //si l'espace entre les deux lignes est plus petit que la normale (arret)
leandre 2:a37de5eb8bf3 167
leandre 2:a37de5eb8bf3 168 //for(int i=0;i<5;i++) pc.printf("%d ",Interval[i]);
leandre 2:a37de5eb8bf3 169 //pc.printf("\n");
leandre 4:8b0215ec788f 170 //VIT=0;
leandre 5:7714a216c91d 171 pc.printf("\narret ");
leandre 4:8b0215ec788f 172 //pc.printf("\n%d ",moy_inter);
leandre 4:8b0215ec788f 173 led1=1;
leandre 4:8b0215ec788f 174 led2=1;
leandre 4:8b0215ec788f 175 arr=t_debut.read_ms();
leandre 2:a37de5eb8bf3 176
leandre 2:a37de5eb8bf3 177 }
leandre 4:8b0215ec788f 178 }
leandre 4:8b0215ec788f 179 else {
leandre 4:8b0215ec788f 180 led1=0;
leandre 4:8b0215ec788f 181 led2=0;
leandre 4:8b0215ec788f 182
leandre 4:8b0215ec788f 183 }
leandre 4:8b0215ec788f 184
Freescale_cup 0:3ec7fc598e48 185 }
leandre 2:a37de5eb8bf3 186
leandre 2:a37de5eb8bf3 187 for(int j = TAILLE_TAB_INT-1; j > 0; j--)
leandre 2:a37de5eb8bf3 188 {
leandre 2:a37de5eb8bf3 189 Interval[j] = Interval[j-1];
leandre 4:8b0215ec788f 190 tab_mem_b[j]=tab_mem_b[j-1];
leandre 4:8b0215ec788f 191 tab_mem_a[j]=tab_mem_a[j-1];
leandre 4:8b0215ec788f 192 }
leandre 4:8b0215ec788f 193 tab_mem_b[0]=mem_b;
leandre 4:8b0215ec788f 194 tab_mem_a[0]=mem_a;
Freescale_cup 0:3ec7fc598e48 195 }
Freescale_cup 0:3ec7fc598e48 196
Freescale_cup 0:3ec7fc598e48 197
leandre 4:8b0215ec788f 198 void Gestion_bosse()
leandre 4:8b0215ec788f 199 {
leandre 4:8b0215ec788f 200 if(BOSSE_ACTIVE)//Si on active la détection de la bosse
leandre 4:8b0215ec788f 201 {
leandre 4:8b0215ec788f 202
leandre 4:8b0215ec788f 203 //pc.printf("bosse: %d , X: %1.2f , ajustement : %hi\n ",bosse,middle_3F(last_three),ajust_vit_bosse);
leandre 4:8b0215ec788f 204
leandre 4:8b0215ec788f 205
leandre 4:8b0215ec788f 206 last_three[2] = last_three[1]; // les 3 dernieres valeurs de X sont stockées dans un tableau
leandre 4:8b0215ec788f 207 last_three[1] = last_three[0];
leandre 4:8b0215ec788f 208 last_three[0] = acc.getAccX();
leandre 4:8b0215ec788f 209
leandre 4:8b0215ec788f 210 if(bosse == 1)
leandre 4:8b0215ec788f 211 {
leandre 4:8b0215ec788f 212 ajust_vit_bosse = 60;
leandre 4:8b0215ec788f 213 led3=1;
leandre 4:8b0215ec788f 214 led4=0;
leandre 4:8b0215ec788f 215
leandre 4:8b0215ec788f 216 }
leandre 4:8b0215ec788f 217 else if(bosse == 3){
leandre 4:8b0215ec788f 218 ajust_vit_bosse = -80;
leandre 4:8b0215ec788f 219 led3=0;
leandre 4:8b0215ec788f 220 led4=1;
leandre 4:8b0215ec788f 221
leandre 4:8b0215ec788f 222 }
leandre 4:8b0215ec788f 223 else
leandre 4:8b0215ec788f 224 {
leandre 4:8b0215ec788f 225 ajust_vit_bosse = 0;
leandre 4:8b0215ec788f 226 led3=0;
leandre 4:8b0215ec788f 227 led4=0;
leandre 4:8b0215ec788f 228
leandre 4:8b0215ec788f 229 }
leandre 4:8b0215ec788f 230
leandre 4:8b0215ec788f 231 if((abs(middle_3F(last_three)) > 0.13 ) && (bosse == 0)) // si montée détectée
leandre 4:8b0215ec788f 232 {
leandre 4:8b0215ec788f 233 bosse = 1;
leandre 4:8b0215ec788f 234 //tep = (int)t_debut.read_ms();
leandre 4:8b0215ec788f 235 }
leandre 4:8b0215ec788f 236 else if(bosse == 1 && (middle_3F(last_three) > -0.1))
leandre 4:8b0215ec788f 237 {
leandre 4:8b0215ec788f 238 bosse = 2;
leandre 4:8b0215ec788f 239 }
leandre 4:8b0215ec788f 240 else if(bosse == 2 && (middle_3F(last_three) > 0.19))
leandre 4:8b0215ec788f 241 {
leandre 4:8b0215ec788f 242 bosse = 3;
leandre 4:8b0215ec788f 243
leandre 4:8b0215ec788f 244 }
leandre 4:8b0215ec788f 245 else if(bosse==3 && (middle_3F(last_three) > 0.19))
leandre 4:8b0215ec788f 246 {
leandre 4:8b0215ec788f 247 ajust_vit_bosse = -50;
leandre 4:8b0215ec788f 248
leandre 4:8b0215ec788f 249 pc.printf("bosse: %d , X: %1.2f , ajustement : %hi\n",bosse,middle_3F(last_three),ajust_vit_bosse);
leandre 4:8b0215ec788f 250 }
leandre 4:8b0215ec788f 251
leandre 4:8b0215ec788f 252 else if (bosse==3 && (middle_3F(last_three) < 0.19))
leandre 4:8b0215ec788f 253 {
leandre 4:8b0215ec788f 254 bosse=0;
leandre 4:8b0215ec788f 255 ajust_vit_bosse=0;
leandre 4:8b0215ec788f 256 }
leandre 4:8b0215ec788f 257 }
leandre 4:8b0215ec788f 258 }
Freescale_cup 0:3ec7fc598e48 259
Freescale_cup 0:3ec7fc598e48 260
Freescale_cup 0:3ec7fc598e48 261 //void F_BalanceBlancs()
Freescale_cup 0:3ec7fc598e48 262 //{
Freescale_cup 0:3ec7fc598e48 263 // unsigned char moy = 0;
Freescale_cup 0:3ec7fc598e48 264 // moy = Moyenne(cam_data);
Freescale_cup 0:3ec7fc598e48 265 // if(moy > LUMI_MOYENNE && tempsInte >= MIN_INTE) tempsInte-=INCREM_BALANCE;
Freescale_cup 0:3ec7fc598e48 266 // else if(tempsInte <= MAX_INTE) tempsInte+=INCREM_BALANCE;
Freescale_cup 0:3ec7fc598e48 267 //}
Freescale_cup 0:3ec7fc598e48 268
Freescale_cup 0:3ec7fc598e48 269
Freescale_cup 0:3ec7fc598e48 270