102 lines
2.8 KiB
C
102 lines
2.8 KiB
C
#include "pico/stdlib.h"
|
|
#include <stdio.h>
|
|
#include "Monitoring.h"
|
|
#include "Localisation.h"
|
|
#include "Trajet.h"
|
|
#include "Asser_Moteurs.h"
|
|
#include "i2c_annexe.h"
|
|
|
|
#define LED_PIN_ROUGE 28
|
|
|
|
uint32_t temps_cycle_min = UINT32_MAX;
|
|
uint32_t temps_cycle_max=0;
|
|
int erreur_critique = 0;
|
|
int lock=0;
|
|
uint32_t debug_var=0;
|
|
float debug_varf=0;
|
|
|
|
void temps_cycle_check(){
|
|
static uint32_t temps_old;
|
|
uint32_t temps, temps_cycle;
|
|
|
|
temps = time_us_32();
|
|
temps_cycle = temps - temps_old;
|
|
|
|
if(temps_cycle < temps_cycle_min){
|
|
temps_cycle_min = temps_cycle;
|
|
}
|
|
|
|
if(temps_cycle > temps_cycle_max){
|
|
temps_cycle_max = temps_cycle;
|
|
}
|
|
temps_old=time_us_32();
|
|
}
|
|
|
|
void temps_cycle_reset(){
|
|
temps_cycle_min = UINT32_MAX;
|
|
temps_cycle_max=0;
|
|
}
|
|
|
|
void Monitoring_display(){
|
|
while(1){
|
|
uint32_t temps;
|
|
temps = time_us_32()/1000;
|
|
temps_cycle_display();
|
|
printf(">V_bat:%ld:%2.1f\n", temps, (float) (i2c_annexe_get_tension_batterie() / 10.));
|
|
printf(">DebugVar:%ld:%d\n", temps, debug_var);
|
|
printf(">Distance_Obstacle:%ld:%f\n", temps, Trajet_get_obstacle_mm());
|
|
printf(">DebugVarf(abscisse):%ld:%f\n", temps, debug_varf);
|
|
struct position_t position = Localisation_get();
|
|
printf(">pos_x:%ld:%f\n", temps, position.x_mm);
|
|
printf(">P_con_x:%ld:%f\n", temps, Trajet_get_consigne().x_mm);
|
|
printf(">pos_y:%ld:%f\n", temps, position.y_mm);
|
|
printf(">P_con_y:%ld:%f\n", temps, Trajet_get_consigne().y_mm);
|
|
printf(">pos_angle:%ld:%f\n", temps, position.angle_radian);
|
|
printf(">P_con_angle:%ld:%f\n", temps, Trajet_get_consigne().angle_radian);
|
|
printf(">V_a:%ld:%f\n", temps, AsserMoteur_getVitesse_mm_s(MOTEUR_A, 1));
|
|
printf(">V_b:%ld:%f\n", temps, AsserMoteur_getVitesse_mm_s(MOTEUR_B, 1));
|
|
printf(">V_c:%ld:%f\n", temps, AsserMoteur_getVitesse_mm_s(MOTEUR_C, 1));
|
|
printf(">V_con_a:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_A));
|
|
printf(">V_con_b:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_B));
|
|
printf(">V_con_c:%ld:%f\n", temps, AsserMoteur_getConsigne_mm_s(MOTEUR_C));
|
|
}
|
|
}
|
|
|
|
void temps_cycle_display(){
|
|
uint32_t temps;
|
|
temps = time_us_32()/1000;
|
|
printf(">T_cycle_min(us):%ld:%d\n", temps, temps_cycle_min);
|
|
printf(">T_cycle_max(us):%ld:%d\n", temps, temps_cycle_max);
|
|
temps_cycle_reset();
|
|
}
|
|
|
|
uint32_t temps_cycle_get_min(){
|
|
return temps_cycle_min;
|
|
}
|
|
|
|
uint32_t temps_cycle_get_max(){
|
|
return temps_cycle_max;
|
|
}
|
|
|
|
void set_debug_var(uint32_t variable){
|
|
debug_var = variable;
|
|
}
|
|
|
|
void set_debug_varf(float variable){
|
|
debug_varf = variable;
|
|
}
|
|
|
|
void Monitoring_Error(char * msg){
|
|
gpio_put(LED_PIN_ROUGE, 1);
|
|
|
|
}
|
|
|
|
void Monitoring_set_erreur_critique(){
|
|
Monitoring_Error("");
|
|
erreur_critique=1;
|
|
}
|
|
|
|
int Monitoring_get_erreur_critique(){
|
|
return erreur_critique;
|
|
}
|