141 lines
3.1 KiB
C
141 lines
3.1 KiB
C
/*****
|
|
* 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 "Localisation.h"
|
|
#include "Moteurs.h"
|
|
#include "Temps.h"
|
|
#include "Trajectoire.h"
|
|
#include "Trajet.h"
|
|
#include <stdio.h>
|
|
#include "QEI.h"
|
|
|
|
#define LED1PIN 20
|
|
|
|
void affichage(void);
|
|
void tension_batterie_init(void);
|
|
uint16_t tension_batterie_lire(void);
|
|
|
|
void identifiant_init(void);
|
|
uint identifiant_lire(void);
|
|
|
|
uint32_t step_ms=1;
|
|
float distance1_mm=0, distance2_mm=0;
|
|
|
|
// DEBUG
|
|
extern float abscisse;
|
|
|
|
void main(void)
|
|
{
|
|
int ledpower = 500;
|
|
|
|
|
|
|
|
stdio_init_all();
|
|
Temps_init();
|
|
//tension_batterie_init();
|
|
identifiant_init();
|
|
Localisation_init();
|
|
Trajet_init();
|
|
|
|
uint32_t temps_ms = Temps_get_temps_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(affichage);
|
|
Localisation_set(1122, 2000-63, M_PI);
|
|
|
|
struct trajectoire_t trajectoire;
|
|
Trajectoire_bezier(&trajectoire, 1122, 2000-63, 905, 2000-63,
|
|
606, 2000-590, 225, 2000-225, M_PI, M_PI);
|
|
Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
|
|
Trajet_debut_trajectoire(trajectoire);
|
|
|
|
|
|
|
|
enum etat_trajet_t etat_trajet=TRAJET_EN_COURS;
|
|
|
|
sleep_ms(8000);
|
|
|
|
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){
|
|
AsserMoteur_Gestion(step_ms);
|
|
etat_trajet = Trajet_avance((float)step_ms/1000.);
|
|
}else{
|
|
Moteur_Stop();
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
}
|
|
}
|
|
|
|
void affichage(void){
|
|
while(1){
|
|
printf(">c1_mm:%f\n>c2_mm:%f\n", QEI_get_mm(QEI_A_NAME), QEI_get_mm(QEI_B_NAME) );
|
|
|
|
/*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(">abscisse:%f\n",abscisse);
|
|
printf(">id:%d\n", identifiant_lire());
|
|
sleep_ms(100);
|
|
}
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
|
|
/// @brief !! Arg la GPIO 26 ne répond pas !
|
|
/// @return
|
|
uint identifiant_lire(){
|
|
return (gpio_get(21) << 2)+ (gpio_get(22) << 1) + gpio_get(26);
|
|
} |