250 lines
7.6 KiB
C
250 lines
7.6 KiB
C
/*****
|
|
* Copyright (c) 2023 - Poivron Robotique
|
|
*
|
|
* SPDX-License-Identifier: BSD-3-Clause
|
|
*/
|
|
#include "pico/stdlib.h"
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <pico/stdio.h>
|
|
|
|
#include "communication.h"
|
|
#include "messagerie.h"
|
|
#include "messagerie_applicative.h"
|
|
|
|
|
|
void stdio_usb_out_chars(const char *buf, int length);
|
|
|
|
#define NB_MORCEAUX 100
|
|
|
|
absolute_time_t envoie_nb_char(int nb_char);
|
|
char chaine_envoie[10000];
|
|
|
|
|
|
|
|
|
|
|
|
void envoie_donnees(char * donnees, uint16_t length){
|
|
for (int i=0; i<length; i++){
|
|
putchar(donnees[i]);
|
|
}
|
|
}
|
|
|
|
/// @brief Fonction pour envoyer une structure
|
|
/// @param donnees données à envoyer
|
|
/// @param size taille des données
|
|
void envoie_structure(const void * donnees, size_t size){
|
|
const unsigned char *byte;
|
|
// Entête du message : debut
|
|
putchar(0xFF);
|
|
putchar(0xFF);
|
|
putchar(size + 1);
|
|
// Entête du message : fin
|
|
// Données
|
|
for ( byte = donnees; size--; ++byte )
|
|
{
|
|
putchar(*byte);
|
|
//printf("%2X ", *byte);
|
|
}
|
|
// Caractère de fin
|
|
putchar(0x00);
|
|
printf("\n");
|
|
}
|
|
|
|
void main(void)
|
|
{
|
|
absolute_time_t measured_time_us;
|
|
absolute_time_t temps_moyen, temps_max, temps_m[NB_MORCEAUX];
|
|
absolute_time_t current_time_us, start_time_us;
|
|
absolute_time_t temps_message_detection;
|
|
struct message_t message;
|
|
struct message_requete_t message_requete_detection;
|
|
unsigned char message_binaire[100];
|
|
|
|
message_requete_detection.commande = 'd';
|
|
message_requete_detection.id_carte = 'D';
|
|
message_requete_detection.adresse_registre = 0;
|
|
message_requete_detection.taille_donnees = 19;
|
|
|
|
|
|
message_binaire[0]='r';
|
|
message_binaire[1]='A';
|
|
message_binaire[2]= 0;
|
|
message_binaire[3]= 5;
|
|
|
|
message_binaire[4]= 'a';
|
|
message_binaire[5]= 'b';
|
|
message_binaire[6]= 'c';
|
|
message_binaire[7]= 'd';
|
|
message_binaire[8]= 'e';
|
|
|
|
message_binaire[9]= 0;
|
|
|
|
struct message_applicatif_t message_applicatif;
|
|
|
|
|
|
|
|
for(int i=0; i<10000; i++){
|
|
/*j++;
|
|
if(j>25){
|
|
j=0;
|
|
}
|
|
chaine_envoie[i]= j + 'a';*/
|
|
chaine_envoie[i]= i%26 + 'a';
|
|
}
|
|
int nb_ech = 10;
|
|
int taille_morceau = 60;
|
|
int nb_message=0;
|
|
|
|
stdio_init_all();
|
|
sleep_ms(5000);
|
|
|
|
gpio_init(PICO_DEFAULT_LED_PIN);
|
|
gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
|
|
gpio_put(PICO_DEFAULT_LED_PIN, 0);
|
|
|
|
temps_message_detection = get_absolute_time();
|
|
unsigned int temps_ms;
|
|
temps_ms = get_absolute_time() / 1000.;
|
|
|
|
communication_init();
|
|
|
|
|
|
while(1){
|
|
/*
|
|
if(get_absolute_time() - temps_message_detection > MESSAGE_TIMEOUT_US){
|
|
start_time_us = get_absolute_time();
|
|
envoie_structure(&message_requete_detection, sizeof(message_requete_detection) );
|
|
temps_message_detection = get_absolute_time();
|
|
current_time_us = get_absolute_time();
|
|
//printf(">temps_emission:%lld\n", current_time_us - start_time_us);
|
|
}
|
|
|
|
start_time_us = get_absolute_time();*/
|
|
communication_reception_message();
|
|
|
|
if(messagerie_message_disponible()){
|
|
uint8_t id_carte, registre;
|
|
while(messagerie_message_disponible()){
|
|
message = messagerie_get_message();
|
|
nb_message++;
|
|
|
|
if(message.type == 'b'){
|
|
switch(message.donnees[0]){
|
|
case 'r': // réception de données
|
|
// Reception de données
|
|
// données[0]: commande ('r' ici)
|
|
// données[1]: id de la carte
|
|
// données[2]: 1er registre
|
|
// données[3 à tailles_données -1]: valeurs
|
|
memcpy(&message_applicatif, message.donnees, message.taille_donnees);
|
|
printf("id_carte:%c %d\n", message_applicatif.id_carte, message_applicatif.id_carte);
|
|
|
|
|
|
if(message_applicatif.id_carte == 'P'){
|
|
|
|
mise_a_jour_position = false;
|
|
mise_a_jour_mode = false;
|
|
mise_a_jour_pwm = false;
|
|
mise_a_jour_vitesse_roues = false;
|
|
mise_a_jour_vitesse_robot = false;
|
|
mise_a_jour_trajectoire = false;
|
|
|
|
// Copie des données dans la mémoire d'échange
|
|
// Et actualisation des variables "mise_a_jour_..."
|
|
for (int num_octet=0; num_octet<message_applicatif.taille_donnees; num_octet++){
|
|
écriture_données(message_applicatif.adresse_registre + num_octet, message_applicatif.donnees_applicative.donnees[num_octet]);
|
|
}
|
|
|
|
if(mise_a_jour_position){
|
|
struct msg_propulsion_position_t msg_propulsion_position;
|
|
|
|
get_données_reçues((uint8_t *) &msg_propulsion_position, sizeof(msg_propulsion_position), REG_PROPULSION_POSITION);
|
|
printf("mise à jour de la position, x:%2.2f, y:%2.2f, orientation: %2.2f\n", msg_propulsion_position.position_x_mm,
|
|
msg_propulsion_position.position_y_mm, msg_propulsion_position.orientation_rad);
|
|
}
|
|
if(mise_a_jour_mode){
|
|
uint8_t mode;
|
|
get_données_reçues(&mode, sizeof(mode), REG_PROPULSION_MODE);
|
|
printf("mode:%d\n", mode);
|
|
}
|
|
if(mise_a_jour_pwm){
|
|
struct msg_propulsion_pwm_t msg_propulsion_pwm;
|
|
get_données_reçues((uint8_t *) &msg_propulsion_pwm, sizeof(msg_propulsion_pwm), REG_PROPULSION_PWM);
|
|
printf("pwm_gauche:%d, pwm_droit:%d\n", msg_propulsion_pwm.pwm_gauche, msg_propulsion_pwm.pwm_gauche);
|
|
}
|
|
if(mise_a_jour_vitesse_roues){
|
|
struct msg_propulsion_vitesse_roues_t msg_propulsion_vitesse_roues;
|
|
get_données_reçues((uint8_t *) &msg_propulsion_vitesse_roues, sizeof(msg_propulsion_vitesse_roues), REG_PROPULSION_VITESSE_ROUES);
|
|
printf("vitesse_roue_gauche:%f, vitesse_roue_gauche:%d\n", msg_propulsion_vitesse_roues.vitesse_gauche_mm_s, msg_propulsion_vitesse_roues.vitesse_droite_mm_s);
|
|
}
|
|
if(mise_a_jour_vitesse_robot){
|
|
struct msg_propulsion_vitesse_robot_t msg_propulsion_vitesse_robot;
|
|
get_données_reçues((uint8_t *) &msg_propulsion_vitesse_robot, sizeof(msg_propulsion_vitesse_robot), REG_PROPULSION_VITESSE_ROBOT);
|
|
printf("avance_robot (mm/s) : %f, rotation (rad/s): %f\n", msg_propulsion_vitesse_robot.avance_mm_s,
|
|
msg_propulsion_vitesse_robot.rotation_rad_s);
|
|
|
|
}
|
|
if(mise_a_jour_trajectoire){
|
|
// Oh la la !
|
|
}
|
|
|
|
}
|
|
|
|
|
|
/*
|
|
if(message_applicatif.id_carte == USB_ID_PROPULSION && message_applicatif.adresse_registre == REG_PROPULSION_POSITION){
|
|
printf("x:%x, y:%x, rot:%x\n", message_applicatif.donnees_applicative.msg_propulsion_position.position_x_mm,
|
|
message_applicatif.donnees_applicative.msg_propulsion_position.position_y_mm,
|
|
message_applicatif.donnees_applicative.msg_propulsion_position.orientation_rad);
|
|
}*/
|
|
|
|
id_carte = message.donnees[1];
|
|
registre = message.donnees[2];
|
|
if(id_carte == 'D'){
|
|
temps_message_detection = get_absolute_time();
|
|
if(message.donnees[4] > 0 && message.donnees[4] < 30){
|
|
printf(">LED ON\n");
|
|
gpio_put(PICO_DEFAULT_LED_PIN, 1);
|
|
}else{
|
|
printf(">LED OFF\n");
|
|
gpio_put(PICO_DEFAULT_LED_PIN, 0);
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
current_time_us = get_absolute_time();
|
|
//printf(">temps_reception:%lld\n", current_time_us - start_time_us);
|
|
//printf(">nb_message:%u\n",nb_message);
|
|
|
|
}
|
|
|
|
if(get_absolute_time() / 1000. - temps_ms > 100){
|
|
temps_ms = get_absolute_time() / 1000.;
|
|
printf(">nb_message:%u\n", nb_message);
|
|
}
|
|
}
|
|
}
|
|
|
|
absolute_time_t envoie_nb_char(int nb_char) {
|
|
absolute_time_t current_time_us, start_time_us;
|
|
int j=0;
|
|
|
|
if (nb_char == 0){
|
|
return 0;
|
|
}
|
|
chaine_envoie[nb_char - 1] = '\n';
|
|
chaine_envoie[nb_char] = '\0';
|
|
start_time_us = get_absolute_time();
|
|
printf("%s", chaine_envoie);
|
|
envoie_donnees(chaine_envoie, strlen(chaine_envoie));
|
|
|
|
current_time_us = get_absolute_time();
|
|
chaine_envoie[nb_char - 1] = 'a';
|
|
chaine_envoie[nb_char] = 'a';
|
|
return current_time_us - start_time_us;
|
|
}
|
|
|