RP20240_Device_USB/main.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;
}