115 lines
2.4 KiB
C
115 lines
2.4 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_Moteurs.h"
|
|
#include "Localisation.h"
|
|
#include "Moteurs.h"
|
|
#include "Temps.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;
|
|
|
|
void main(void)
|
|
{
|
|
int ledpower = 500;
|
|
|
|
|
|
|
|
stdio_init_all();
|
|
AsserMoteur_Init();
|
|
Temps_init();
|
|
tension_batterie_init();
|
|
identifiant_init();
|
|
Localisation_init();
|
|
uint32_t temps_ms = Temps_get_temps_ms();
|
|
|
|
|
|
gpio_init(LED1PIN);
|
|
gpio_set_dir(LED1PIN, GPIO_OUT );
|
|
gpio_put(LED1PIN, 1);
|
|
|
|
|
|
multicore_launch_core1(affichage);
|
|
|
|
|
|
while(1){
|
|
|
|
if(temps_ms != Temps_get_temps_ms()){
|
|
temps_ms = Temps_get_temps_ms();
|
|
if(temps_ms % step_ms == 0){
|
|
QEI_update();
|
|
AsserMoteur_Gestion(step_ms);
|
|
Localisation_gestion();
|
|
}
|
|
if(temps_ms % 100 == 0){
|
|
identifiant_lire();
|
|
//printf(">c1:%d\n>c2:%d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME) );
|
|
}
|
|
}
|
|
|
|
|
|
|
|
}
|
|
}
|
|
|
|
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);
|
|
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);
|
|
} |