71 lines
1.6 KiB
C
71 lines
1.6 KiB
C
/*****
|
|
* Copyright (c) 2023 - Poivron Robotique
|
|
*
|
|
* SPDX-License-Identifier: BSD-3-Clause
|
|
*/
|
|
#include "pico/stdlib.h"
|
|
#include "pico/multicore.h"
|
|
#include "hardware/pwm.h"
|
|
#include "Asser_Moteurs.h"
|
|
#include "Moteurs.h"
|
|
#include "Temps.h"
|
|
#include <stdio.h>
|
|
#include "QEI.h"
|
|
|
|
#define LED1PIN 20
|
|
#define M1_SENS1 7
|
|
#define M1_SENS2 13
|
|
#define M1_VITESSE 27 //5B
|
|
#define M2_SENS1 10
|
|
#define M2_SENS2 5
|
|
#define M2_VITESSE 9 //4B
|
|
|
|
void affichage(void);
|
|
|
|
void main(void)
|
|
{
|
|
int ledpower = 500;
|
|
|
|
|
|
stdio_init_all();
|
|
AsserMoteur_Init();
|
|
Temps_init();
|
|
uint32_t temps_ms = Temps_get_temps_ms();
|
|
uint32_t step_ms=1;
|
|
|
|
gpio_init(LED1PIN);
|
|
gpio_set_dir(LED1PIN, GPIO_OUT );
|
|
gpio_put(LED1PIN, 1);
|
|
AsserMoteur_setConsigne_mm_s(MOTEUR_A, 6000);
|
|
AsserMoteur_setConsigne_mm_s(MOTEUR_B, 6000);
|
|
|
|
multicore_launch_core1(affichage);
|
|
Moteur_SetVitesse(MOTEUR_A, 16000);
|
|
Moteur_SetVitesse(MOTEUR_B, 16000);
|
|
|
|
while(1){
|
|
|
|
if(temps_ms != Temps_get_temps_ms()){
|
|
temps_ms = Temps_get_temps_ms();
|
|
QEI_update();
|
|
AsserMoteur_Gestion(step_ms);
|
|
if(temps_ms % 100 == 0){
|
|
//printf(">c1:%d\n>c2:%d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME) );
|
|
}
|
|
}
|
|
|
|
|
|
|
|
}
|
|
}
|
|
|
|
void affichage(void){
|
|
while(1){
|
|
|
|
printf(">c1:%d\n>c1_mm:%f\n", QEI_get(QEI_A_NAME), QEI_get_mm(QEI_A_NAME) );
|
|
//printf(">c2:%d\n>c2_mm:%f\n", QEI_get(QEI_B_NAME), QEI_get_mm(QEI_B_NAME) );
|
|
printf(">m1:%f\n>m2:%f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, 1), AsserMoteur_getVitesse_mm_s(MOTEUR_B, 1) );
|
|
printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );
|
|
sleep_ms(100);
|
|
}
|
|
} |