Keuronde/PAMI_2024
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
7538fdd311
PAMI_2024/main.c

339 lines
8.3 KiB
C
Raw Blame History

/*****
* Copyright (c) 2023 - Poivron Robotique
*
* SPDX-License-Identifier: BSD-3-Clause
*/
#include "pico/stdlib.h"
#include "pico/multicore.h"
#include "hardware/adc.h"
#include "hardware/pwm.h"
#include "Asser_Position.h"
#include "Asser_Moteurs.h"
#include "Commande_vitesse.h"
#include "i2c_maitre.h"
#include "Localisation.h"
#include "Moteurs.h"
#include "Temps.h"
#include "Trajectoire.h"
#include "Trajet.h"
#include "VL53L8_2024.h"
#include "vl53l8cx_api.h"
#include <stdio.h>
#include <math.h>
#include "QEI.h"
#define LED1PIN 20
#define TIRETTE_PIN 6
#define COULEUR_PIN 4
#define COULEUR_BLEU 1
#define COULEUR_JAUNE 0
void affichage(void);
void gestion_affichage(void);
void tension_batterie_init(void);
uint16_t tension_batterie_lire(void);
void identifiant_init(void);
uint identifiant_lire(void);
int get_tirette(int);
int get_couleur(void);
void configure_trajet(int identifiant, int couleur);
void gestion_VL53L8CX(void);
const uint32_t step_ms=1;
float distance1_mm=0, distance2_mm=0;
// DEBUG
extern float abscisse;
extern struct point_xyo_t point;
float vitesse;
VL53L8CX_Configuration Dev;
void main(void)
{
int ledpower = 500;
VL53L8CX_ResultsData Results;
bool fin_match = false;
stdio_init_all();
Temps_init();
//tension_batterie_init();
identifiant_init();
Localisation_init(identifiant_lire());
Trajet_init(identifiant_lire());
i2c_maitre_init();
uint32_t temps_ms = Temps_get_temps_ms();
uint32_t temps_depart_ms;
struct position_t position_robot={.x_mm=0, .y_mm=0, .angle_radian=0};
float vitesse_mm_s=100;
gpio_init(LED1PIN);
gpio_set_dir(LED1PIN, GPIO_OUT );
gpio_put(LED1PIN, 1);
//multicore_launch_core1(gestion_affichage);
multicore_launch_core1(gestion_VL53L8CX);
sleep_ms(5000);
printf("Demarrage...\n");
configure_trajet(identifiant_lire(), get_couleur());
float vitesse_init =300;
vitesse = vitesse_init;
enum etat_trajet_t etat_trajet=TRAJET_EN_COURS;
while(get_tirette(identifiant_lire()));
gpio_put(LED1PIN, 0);
// Seul le premier PAMI doit attendre 90s, les autres démarrent lorsque celui de devant part
if(identifiant_lire() == 3){
sleep_ms(90000);
}
temps_depart_ms = Temps_get_temps_ms();
while(1){
// Fin du match
if((Temps_get_temps_ms() -temps_depart_ms) >10000 || (fin_match == 1)){
Moteur_Stop();
while(1);
}
if(temps_ms != Temps_get_temps_ms()){
temps_ms = Temps_get_temps_ms();
if(temps_ms % step_ms == 0){
QEI_update();
Localisation_gestion();
if(etat_trajet != TRAJET_TERMINE){
etat_trajet = Trajet_avance((float)step_ms/1000.);
}else{
Asser_Position_maintien();
if(Asser_Position_panic_angle()){
fin_match=1;
}
}
AsserMoteur_Gestion(step_ms);
}
}
}
}
/// @brief Obtient la distance de l'obstacle le plus proche.
/// @param
void gestion_VL53L8CX(void){
VL53L8CX_ResultsData Results;
float distance_obstacle;
VL53L8_init(&Dev);
sleep_ms(100);
VL53L8_lecture( &Dev, &Results); // une première lecture
uint8_t status, isReady;
while(1){
status = vl53l8cx_check_data_ready(&Dev, &isReady);
if(isReady){
VL53L8_lecture( &Dev, &Results);
VL53L8_min_distance(Results, &distance_obstacle);
Trajet_set_obstacle_mm(distance_obstacle);
}
affichage();
}
}
extern float delta_x_mm, delta_y_mm, delta_orientation_radian;
void gestion_affichage(void){
while(1){
affichage();
}
}
void affichage(void){
/*printf(">m1:%f\n>m2:%f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) );
printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );*/
printf(">pos_x:%.1f\n>pos_y:%.1f\n>pos_angle:%.1f\n", Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian);
printf(">distance_obstacle:%f\n",Trajet_get_obstacle_mm());
printf(">abscisse:%f\n",abscisse);
struct position_t position_actuelle;
position_actuelle = Localisation_get();
printf(">delta_orientation_radian:%.2f\n>angle_delta:%.2f\n",delta_orientation_radian, atan2f(delta_y_mm, delta_x_mm));
printf(">pos_x:%.2f\n>pos_y:%.2f\n", position_actuelle.x_mm, position_actuelle.y_mm);
printf(">con_x:%.2f\n>con_y:%.2f\n", point.point_xy.x, point.point_xy.y);
printf(">couleur:%d\n>id:%d\n>Tirette:%d\n", get_couleur(), identifiant_lire(), get_tirette(identifiant_lire()));
}
void tension_batterie_init(void){
adc_init();
adc_gpio_init(28); // Analog_2
adc_select_input(2);
adc_run(1); // Free running mode
}
uint16_t tension_batterie_lire(){
uint16_t result = (uint16_t) adc_hw->result;
const float conversion_factor = 3.3f / (1 << 12);
float v_bat = result * conversion_factor * 11.;
return result;
}
void identifiant_init(){
gpio_init(21);
gpio_init(22);
gpio_init(26);
gpio_pull_up(21);
gpio_pull_up(22);
gpio_pull_up(26);
gpio_set_dir(21, GPIO_IN);
gpio_set_dir(22, GPIO_IN);
gpio_set_dir(26, GPIO_IN);
// Tirette
gpio_init(TIRETTE_PIN);
gpio_pull_up(TIRETTE_PIN);
gpio_set_dir(TIRETTE_PIN, GPIO_IN);
// Couleur
gpio_init(COULEUR_PIN);
gpio_pull_up(COULEUR_PIN);
gpio_set_dir(COULEUR_PIN, GPIO_IN);
}
int get_tirette(int id){
if(id == 3){
return !gpio_get(TIRETTE_PIN);
}
return (VL53L8_get_old_min_distance() <50);
}
int get_couleur(void){
return gpio_get(COULEUR_PIN);
}
/// @brief !! Arg la GPIO 26 ne répond pas ! => Réglé ADC_VREF était à la masse
/// @return identifiant du robot (PDI switch)
uint identifiant_lire(){
return (gpio_get(21) << 2)+ (gpio_get(22) << 1) + gpio_get(26);
}
void configure_trajet(int identifiant, int couleur){
struct trajectoire_t trajectoire;
Trajet_config(TRAJECT_CONFIG_RAPIDE);
switch(couleur){
case COULEUR_JAUNE:
switch (identifiant)
{
case 0:
Localisation_set(3000-1249, 2000-63, 0);
Trajectoire_bezier(&trajectoire, 3000-1250, 2000-63, 3000-1050, 2000-63,
3000-750, 1400, 3000-750, 2100, 0, 0);
break;
case 1:
Localisation_set(3000-1249, 2000-63, 0);
Trajectoire_bezier(&trajectoire, 3000-1250, 2000-63, 3000-1050, 2000-63,
3000-750, 1400, 3000-750, 2100, 0, 0);
break;
case 2:
Localisation_set(3000-1245, 2000-63, 0);
Trajectoire_bezier(&trajectoire, 3000-1244, 2000-63, 3000-950, 2000-63,
3000-540, 1400, 3100, 1400, M_PI, M_PI);
break;
case 3:
Localisation_set(3000-1130, 2000-63, 0);
Trajectoire_bezier(&trajectoire, 3000-1122, 2000-63, 3000-905, 2000-63,
3000-606, 2000-590, 3000-225, 2000-225, -M_PI, -M_PI);
break;
case 4:
Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
Localisation_set(3000-1364, 2000-63, 0);
Trajectoire_bezier(&trajectoire, 3000-1363, 2000-63, 3000-550, 2000-63,
2700-900, 600, 2700-0, 0, 0, 0);
break;
case 5:
Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
Localisation_set(3000-1450, 2000-63, 0);
Trajectoire_bezier(&trajectoire, 3000-1449, 2000-63, 3000-675, 2000-63,
3000-930, 970, 0, 1200, -M_PI / 2., M_PI);
break;
case 6:
break;
case 7:
break;
default:
break;
}
break;
case COULEUR_BLEU:
switch (identifiant)
{
case 0:
Localisation_set(1249, 2000-63, M_PI);
Trajectoire_bezier(&trajectoire, 1250, 2000-63, 1050, 2000-63,
750, 1400, 750, 2100, M_PI, M_PI);
break;
case 1:
Localisation_set(1249, 2000-63, M_PI);
Trajectoire_bezier(&trajectoire, 1250, 2000-63, 1050, 2000-63,
750, 1400, 750, 2100, M_PI, M_PI);
break;
case 2:
Localisation_set(1245, 2000-63, M_PI);
Trajectoire_bezier(&trajectoire, 1244, 2000-63, 950, 2000-63,
540, 1400, -100, 1400, M_PI, M_PI);
break;
case 3:
Localisation_set(1121, 2000-63, M_PI);
Trajectoire_bezier(&trajectoire, 1122, 2000-63, 905, 2000-63,
606, 2000-590, 225, 2000-225, M_PI, M_PI);
break;
case 4:
Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
Localisation_set(1364, 2000-63, M_PI);
Trajectoire_bezier(&trajectoire, 1363, 2000-63, 550, 2000-63,
900, 600, 0, 0, -M_PI / 2., M_PI);
break;
case 5:
Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
Localisation_set(1450, 2000-63, M_PI);
Trajectoire_bezier(&trajectoire, 1449, 2000-63, 675, 2000-63,
930, 970, 3000, 1200, -M_PI / 2., M_PI);
break;
case 6:
break;
case 7:
break;
default:
break;
}
break;
}
Trajet_debut_trajectoire(trajectoire);
}
Reference in New Issue View Git Blame Copy Permalink