60 lines
1.4 KiB
C
60 lines
1.4 KiB
C
/*****
|
|
* Copyright (c) 2024 - Poivron Robotique
|
|
*
|
|
* SPDX-License-Identifier: BSD-3-Clause
|
|
*/
|
|
#include "pico/stdlib.h"
|
|
#include "Moteurs.h"
|
|
#include "QEI.h"
|
|
#include "Teleplot.h"
|
|
#include "Temps.h"
|
|
#include <stdio.h>
|
|
#include <math.h>
|
|
|
|
void setup(void);
|
|
void loop(void);
|
|
|
|
int valeur = 1;
|
|
uint32_t m_temps_ms;
|
|
|
|
void main(void)
|
|
{
|
|
setup();
|
|
while(1){
|
|
loop();
|
|
}
|
|
}
|
|
|
|
void setup(void){
|
|
stdio_init_all();
|
|
Teleplot_init();
|
|
Temps_init();
|
|
Moteur_init();
|
|
QEI_init();
|
|
}
|
|
|
|
void loop(void){
|
|
static float cA_distance = 0, cB_distance = 0;
|
|
float t = (float) Temps_get_temps_ms() / 1000. ;
|
|
Moteur_set_commande(MOTEUR_A, sin(t * 3.14) * MOTEUR_COMMANDE_MAX) ;
|
|
Moteur_set_commande(MOTEUR_B, sin(t * 3.14) * MOTEUR_COMMANDE_MAX) ;
|
|
if(m_temps_ms != Temps_get_temps_ms()){
|
|
m_temps_ms = Temps_get_temps_ms();
|
|
|
|
if(m_temps_ms % 1 == 0){ // Fréquence d'actualisation des codeurs (1 ms)
|
|
QEI_update();
|
|
cA_distance += QEI_get_mm(QEI_A_NAME);
|
|
cB_distance += QEI_get_mm(QEI_B_NAME);
|
|
}
|
|
|
|
if(m_temps_ms % 20 == 0){ // Fréquence d'affichage (20 ms)
|
|
Teleplot_fige_temps();
|
|
Teleplot_add_variable_float_2decimal("codeur_A_vitesse", QEI_get_mm(QEI_A_NAME));
|
|
Teleplot_add_variable_float_2decimal("codeur_B_vitesse", QEI_get_mm(QEI_B_NAME));
|
|
Teleplot_add_variable_float_2decimal("codeur_A_distance", cA_distance);
|
|
Teleplot_add_variable_float_2decimal("codeur_B_distance", cA_distance);
|
|
Teleplot_envoie_tampon();
|
|
}
|
|
}
|
|
|
|
} |