Robot_tennis / Mbed 2 deprecated cerv0

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

fonction.cpp@0:c2a70ac44fd7, 2022-06-06 (annotated)

Committer:
terrier_julien
Date:
Mon Jun 06 07:20:16 2022 +0000
Revision:
0:c2a70ac44fd7
Child:
1:cfb7e4122e21
Fonction de calcule d'angle et de reception des camera lateral, renvoie un angle sur 360 avec comme origine la droite (repere trigo)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
terrier_julien 0:c2a70ac44fd7 1 #include "mbed.h"
terrier_julien 0:c2a70ac44fd7 2 #include <string>
terrier_julien 0:c2a70ac44fd7 3 #include <iostream>
terrier_julien 0:c2a70ac44fd7 4 #include "header.h"
terrier_julien 0:c2a70ac44fd7 5
terrier_julien 0:c2a70ac44fd7 6 Serial pc (PA_2, PA_3);
terrier_julien 0:c2a70ac44fd7 7
terrier_julien 0:c2a70ac44fd7 8 Serial husky (PA_0,PA_1);
terrier_julien 0:c2a70ac44fd7 9
terrier_julien 0:c2a70ac44fd7 10 unsigned char reception_bidule[7]={0,0,0,0,0,0,'\n'};
terrier_julien 0:c2a70ac44fd7 11
terrier_julien 0:c2a70ac44fd7 12 unsigned char data_recept[50];
terrier_julien 0:c2a70ac44fd7 13 unsigned char cpt=0,flag_printf=0,flag_tram=0;
terrier_julien 0:c2a70ac44fd7 14 unsigned char data_taille=0,cpt_taille=0,chksum=0;
terrier_julien 0:c2a70ac44fd7 15 unsigned char tableau_COMMAND_REQUEST[16];
terrier_julien 0:c2a70ac44fd7 16
terrier_julien 0:c2a70ac44fd7 17 unsigned short pos_x, pos_y;
terrier_julien 0:c2a70ac44fd7 18
terrier_julien 0:c2a70ac44fd7 19 bool flag_renvoi_pos = false;
terrier_julien 0:c2a70ac44fd7 20
terrier_julien 0:c2a70ac44fd7 21 unsigned char COMMAND_REQUEST_KNOCK[6]={0x55,0xAA,0x11,0x00,0x2C,0x3C};
terrier_julien 0:c2a70ac44fd7 22 unsigned char COMMAND_REQUEST_BLOCKS[6]={0x55,0xAA,0x11,0x00,0x21,0x31};
terrier_julien 0:c2a70ac44fd7 23 unsigned char COMMAND_REQUEST[6]={0x55,0xAA,0x11,0x00,0x20,0x30};
terrier_julien 0:c2a70ac44fd7 24 unsigned char COMMAND_REQUEST_ALGORITHM_BLOCK[8]={0x55,0xAA,0x11,0x02,0x2D,0x01,0x00,0x40};
terrier_julien 0:c2a70ac44fd7 25 unsigned char COMMAND_REQUEST_ALGORITHM_COLOR[8]={0x55,0xAA,0x11,0x02,0x2D,0x04,0x00,0x43};
terrier_julien 0:c2a70ac44fd7 26
terrier_julien 0:c2a70ac44fd7 27 typedef enum
terrier_julien 0:c2a70ac44fd7 28 {
terrier_julien 0:c2a70ac44fd7 29 HEADER1,
terrier_julien 0:c2a70ac44fd7 30 HEADER2,
terrier_julien 0:c2a70ac44fd7 31 ADRESSE,
terrier_julien 0:c2a70ac44fd7 32 TAILLE,
terrier_julien 0:c2a70ac44fd7 33 CHECKSUM,
terrier_julien 0:c2a70ac44fd7 34 DATA,
terrier_julien 0:c2a70ac44fd7 35 CMD
terrier_julien 0:c2a70ac44fd7 36 }T_etat;
terrier_julien 0:c2a70ac44fd7 37
terrier_julien 0:c2a70ac44fd7 38 static T_etat etat = HEADER1;
terrier_julien 0:c2a70ac44fd7 39
terrier_julien 0:c2a70ac44fd7 40 short xg_c,xd_c;
terrier_julien 0:c2a70ac44fd7 41 short xg_f,xd_f,xg_trig, xd_trig;
terrier_julien 0:c2a70ac44fd7 42 short angl;
terrier_julien 0:c2a70ac44fd7 43
terrier_julien 0:c2a70ac44fd7 44 float calcul_angle( void )
terrier_julien 0:c2a70ac44fd7 45 {
terrier_julien 0:c2a70ac44fd7 46 xg_c = main_recept_droit();
terrier_julien 0:c2a70ac44fd7 47 xd_c = 150; //main_recept_gauche();
terrier_julien 0:c2a70ac44fd7 48 xg_f = xg_c;
terrier_julien 0:c2a70ac44fd7 49 xd_f = xd_c;
terrier_julien 0:c2a70ac44fd7 50
terrier_julien 0:c2a70ac44fd7 51 xg_trig = 0.185 * xg_f;
terrier_julien 0:c2a70ac44fd7 52 xd_trig = 60 - 0.185 * xd_f;
terrier_julien 0:c2a70ac44fd7 53
terrier_julien 0:c2a70ac44fd7 54 if ( xd_trig > xd_trig )
terrier_julien 0:c2a70ac44fd7 55 {
terrier_julien 0:c2a70ac44fd7 56 angl = (xd_trig - 30)%360;
terrier_julien 0:c2a70ac44fd7 57 }
terrier_julien 0:c2a70ac44fd7 58 else
terrier_julien 0:c2a70ac44fd7 59 {
terrier_julien 0:c2a70ac44fd7 60 angl = 210 - 0.185* xd_f;
terrier_julien 0:c2a70ac44fd7 61 }
terrier_julien 0:c2a70ac44fd7 62 return angl;
terrier_julien 0:c2a70ac44fd7 63 }
terrier_julien 0:c2a70ac44fd7 64
terrier_julien 0:c2a70ac44fd7 65 short main_recept_droit()
terrier_julien 0:c2a70ac44fd7 66
terrier_julien 0:c2a70ac44fd7 67 {
terrier_julien 0:c2a70ac44fd7 68 husky.attach(&reception,Serial::RxIrq); //La fct réception est donc activé a chaque fois qu'une donnée passe dans le Rx
terrier_julien 0:c2a70ac44fd7 69 husky.baud(9600);
terrier_julien 0:c2a70ac44fd7 70 pc.printf("Debut du programme\n");
terrier_julien 0:c2a70ac44fd7 71
terrier_julien 0:c2a70ac44fd7 72 pc.printf("Envoie command_request_knock\n");
terrier_julien 0:c2a70ac44fd7 73
terrier_julien 0:c2a70ac44fd7 74 envoi_commande(6,COMMAND_REQUEST_KNOCK); //Test d'aqcuisiton avec commande_bidule
terrier_julien 0:c2a70ac44fd7 75
terrier_julien 0:c2a70ac44fd7 76 bool flag_end = false;
terrier_julien 0:c2a70ac44fd7 77
terrier_julien 0:c2a70ac44fd7 78 while(flag_end==false)
terrier_julien 0:c2a70ac44fd7 79 {
terrier_julien 0:c2a70ac44fd7 80 pc.printf("Check:%02X\n",chksum);
terrier_julien 0:c2a70ac44fd7 81 if(flag_printf)
terrier_julien 0:c2a70ac44fd7 82 {
terrier_julien 0:c2a70ac44fd7 83 affichage_buffer_reception(); //renvoie de données
terrier_julien 0:c2a70ac44fd7 84 flag_printf=0;
terrier_julien 0:c2a70ac44fd7 85 flag_end=true;
terrier_julien 0:c2a70ac44fd7 86 }
terrier_julien 0:c2a70ac44fd7 87 }
terrier_julien 0:c2a70ac44fd7 88
terrier_julien 0:c2a70ac44fd7 89 flag_end=false;
terrier_julien 0:c2a70ac44fd7 90
terrier_julien 0:c2a70ac44fd7 91 pc.printf("Reception de data, envoie Command_request_blocks\n");
terrier_julien 0:c2a70ac44fd7 92
terrier_julien 0:c2a70ac44fd7 93 flag_renvoi_pos = false;
terrier_julien 0:c2a70ac44fd7 94 while(1)
terrier_julien 0:c2a70ac44fd7 95 {
terrier_julien 0:c2a70ac44fd7 96
terrier_julien 0:c2a70ac44fd7 97 envoi_commande(6,COMMAND_REQUEST_BLOCKS);
terrier_julien 0:c2a70ac44fd7 98
terrier_julien 0:c2a70ac44fd7 99 for(int i=0;i<data_taille+6;i++) //header + header2 + adress+ datataille + comande + data + checksum
terrier_julien 0:c2a70ac44fd7 100 {
terrier_julien 0:c2a70ac44fd7 101 tableau_COMMAND_REQUEST[i] = data_recept[i];
terrier_julien 0:c2a70ac44fd7 102 }
terrier_julien 0:c2a70ac44fd7 103
terrier_julien 0:c2a70ac44fd7 104 flag_end=false;
terrier_julien 0:c2a70ac44fd7 105
terrier_julien 0:c2a70ac44fd7 106
terrier_julien 0:c2a70ac44fd7 107 for(int j=0;j<16;j++) //header + header2 + adress+ datataille + comande + data + checksum
terrier_julien 0:c2a70ac44fd7 108 {
terrier_julien 0:c2a70ac44fd7 109 pc.printf("%02x:",tableau_COMMAND_REQUEST[j]);
terrier_julien 0:c2a70ac44fd7 110
terrier_julien 0:c2a70ac44fd7 111 }
terrier_julien 0:c2a70ac44fd7 112 pc.printf("\n");
terrier_julien 0:c2a70ac44fd7 113 reception_position();
terrier_julien 0:c2a70ac44fd7 114
terrier_julien 0:c2a70ac44fd7 115 if(flag_renvoi_pos)
terrier_julien 0:c2a70ac44fd7 116 {
terrier_julien 0:c2a70ac44fd7 117 return pos_x;
terrier_julien 0:c2a70ac44fd7 118 }
terrier_julien 0:c2a70ac44fd7 119 flag_renvoi_pos = true;
terrier_julien 0:c2a70ac44fd7 120 }
terrier_julien 0:c2a70ac44fd7 121 }
terrier_julien 0:c2a70ac44fd7 122 void reception_position(void)
terrier_julien 0:c2a70ac44fd7 123 {
terrier_julien 0:c2a70ac44fd7 124 if(tableau_COMMAND_REQUEST[4] == 0x2A) //0x2A de [4] pour vérifier qu'ils s'agisse d'un block return_blocks
terrier_julien 0:c2a70ac44fd7 125 {
terrier_julien 0:c2a70ac44fd7 126 unsigned short high_x, low_x, high_y, low_y;
terrier_julien 0:c2a70ac44fd7 127
terrier_julien 0:c2a70ac44fd7 128 high_x = tableau_COMMAND_REQUEST[6];
terrier_julien 0:c2a70ac44fd7 129 low_x = tableau_COMMAND_REQUEST[5];
terrier_julien 0:c2a70ac44fd7 130
terrier_julien 0:c2a70ac44fd7 131 high_y = tableau_COMMAND_REQUEST[8];
terrier_julien 0:c2a70ac44fd7 132 low_y = tableau_COMMAND_REQUEST[7];
terrier_julien 0:c2a70ac44fd7 133
terrier_julien 0:c2a70ac44fd7 134 pos_x = (high_x<<8) + low_x;
terrier_julien 0:c2a70ac44fd7 135 pos_y = (high_y<<8) + low_y;
terrier_julien 0:c2a70ac44fd7 136 }
terrier_julien 0:c2a70ac44fd7 137 else
terrier_julien 0:c2a70ac44fd7 138 {
terrier_julien 0:c2a70ac44fd7 139 pos_x = 0x0000;
terrier_julien 0:c2a70ac44fd7 140 pos_y = 0x0000;
terrier_julien 0:c2a70ac44fd7 141 }
terrier_julien 0:c2a70ac44fd7 142 pc.printf("%04d:",pos_x);
terrier_julien 0:c2a70ac44fd7 143 pc.printf("%04d:",pos_y);
terrier_julien 0:c2a70ac44fd7 144 pc.printf("\n");
terrier_julien 0:c2a70ac44fd7 145 }
terrier_julien 0:c2a70ac44fd7 146
terrier_julien 0:c2a70ac44fd7 147 void envoi_commande(unsigned char taille,unsigned char buffer[]) //Envoie la commande a la carte
terrier_julien 0:c2a70ac44fd7 148 {
terrier_julien 0:c2a70ac44fd7 149 for(int i=0;i<taille;i++)
terrier_julien 0:c2a70ac44fd7 150 {
terrier_julien 0:c2a70ac44fd7 151 husky.putc(buffer[i]);
terrier_julien 0:c2a70ac44fd7 152 }
terrier_julien 0:c2a70ac44fd7 153 }
terrier_julien 0:c2a70ac44fd7 154
terrier_julien 0:c2a70ac44fd7 155 void affichage_buffer_reception() //Acquiere la donnée revoyé par la carte ex : [0x55]
terrier_julien 0:c2a70ac44fd7 156 {
terrier_julien 0:c2a70ac44fd7 157 for(int i=0;i<data_taille+6;i++) //header + header2 + adress+ datataille + comande + data + checksum
terrier_julien 0:c2a70ac44fd7 158 {
terrier_julien 0:c2a70ac44fd7 159 pc.printf("%02x:",data_recept[i]);
terrier_julien 0:c2a70ac44fd7 160 }
terrier_julien 0:c2a70ac44fd7 161 pc.printf("\n");
terrier_julien 0:c2a70ac44fd7 162 }
terrier_julien 0:c2a70ac44fd7 163
terrier_julien 0:c2a70ac44fd7 164 void reception(void) //Automate de réception des données
terrier_julien 0:c2a70ac44fd7 165 {
terrier_julien 0:c2a70ac44fd7 166 data_recept[cpt] = husky.getc();
terrier_julien 0:c2a70ac44fd7 167 //flag_recept=1;
terrier_julien 0:c2a70ac44fd7 168 switch(etat)
terrier_julien 0:c2a70ac44fd7 169 {
terrier_julien 0:c2a70ac44fd7 170 case HEADER1:chksum=0;
terrier_julien 0:c2a70ac44fd7 171 cpt=0;
terrier_julien 0:c2a70ac44fd7 172 if(data_recept[cpt]==0x55){
terrier_julien 0:c2a70ac44fd7 173 etat=HEADER2;
terrier_julien 0:c2a70ac44fd7 174 }
terrier_julien 0:c2a70ac44fd7 175 break;
terrier_julien 0:c2a70ac44fd7 176
terrier_julien 0:c2a70ac44fd7 177 case HEADER2 : if(data_recept[cpt]==0xAA)
terrier_julien 0:c2a70ac44fd7 178 etat=ADRESSE;
terrier_julien 0:c2a70ac44fd7 179 else
terrier_julien 0:c2a70ac44fd7 180 etat=HEADER1;
terrier_julien 0:c2a70ac44fd7 181 break;
terrier_julien 0:c2a70ac44fd7 182
terrier_julien 0:c2a70ac44fd7 183 case ADRESSE : if(data_recept[cpt]==0x11)
terrier_julien 0:c2a70ac44fd7 184 etat=TAILLE;
terrier_julien 0:c2a70ac44fd7 185 else
terrier_julien 0:c2a70ac44fd7 186 etat=HEADER1;
terrier_julien 0:c2a70ac44fd7 187 break;
terrier_julien 0:c2a70ac44fd7 188 case TAILLE : data_taille=data_recept[cpt];
terrier_julien 0:c2a70ac44fd7 189 etat=CMD;
terrier_julien 0:c2a70ac44fd7 190 break;
terrier_julien 0:c2a70ac44fd7 191
terrier_julien 0:c2a70ac44fd7 192 case CMD: // utilisation de cmd???
terrier_julien 0:c2a70ac44fd7 193 etat=DATA;
terrier_julien 0:c2a70ac44fd7 194 if(data_taille==0)
terrier_julien 0:c2a70ac44fd7 195 etat=CHECKSUM;
terrier_julien 0:c2a70ac44fd7 196 break;
terrier_julien 0:c2a70ac44fd7 197
terrier_julien 0:c2a70ac44fd7 198 case DATA: cpt_taille++;
terrier_julien 0:c2a70ac44fd7 199 if(cpt_taille==data_taille)
terrier_julien 0:c2a70ac44fd7 200 etat=CHECKSUM;
terrier_julien 0:c2a70ac44fd7 201 break;
terrier_julien 0:c2a70ac44fd7 202
terrier_julien 0:c2a70ac44fd7 203 case CHECKSUM : etat=HEADER1;
terrier_julien 0:c2a70ac44fd7 204
terrier_julien 0:c2a70ac44fd7 205 if(data_recept[cpt]==chksum)
terrier_julien 0:c2a70ac44fd7 206 {
terrier_julien 0:c2a70ac44fd7 207 flag_tram=flag_tram+1;
terrier_julien 0:c2a70ac44fd7 208 flag_printf=1;
terrier_julien 0:c2a70ac44fd7 209 cpt_taille=0;
terrier_julien 0:c2a70ac44fd7 210 cpt=0;
terrier_julien 0:c2a70ac44fd7 211 }
terrier_julien 0:c2a70ac44fd7 212 break;
terrier_julien 0:c2a70ac44fd7 213 }
terrier_julien 0:c2a70ac44fd7 214 chksum=chksum+data_recept[cpt];
terrier_julien 0:c2a70ac44fd7 215 cpt++;
terrier_julien 0:c2a70ac44fd7 216
terrier_julien 0:c2a70ac44fd7 217 }