Keuronde/PAMI_2024
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Asservissement moteurs - fonctionnel ?

Browse Source
This commit is contained in:
Samuel 2024-04-27 21:21:41 +02:00
parent 901bdeffa1
commit 6162c6f412
9 changed files with 296 additions and 105 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
Asser_Moteurs.c Normal file
View File

@ -0,0 +1,102 @@
#include "QEI.h"
#include "Moteurs.h"
#include "Asser_Moteurs.h"
/*** C'est ici que se fait la conversion en mm
* ***/
// Roues 60 mm de diamètre, 188,5 mm de circonférence
// Réduction Moteur 30:1
// Réduction poulie 16:12
// Nombre d'impulsions par tour moteur : 200
// Nombre d'impulsions par tour réducteur : 6000
// Nombre d'impulsions par tour de roue : 8000
// Impulsion / mm : 42,44
#define IMPULSION_PAR_MM (1.f)
#define ASSERMOTEUR_GAIN_P 2.f
#define ASSERMOTEUR_GAIN_I 0.f
float consigne_mm_s[3]; // Consigne de vitesse (en mm/s)
float commande_I[3]; // Terme integral
void AsserMoteur_Init(){
QEI_init();
Moteur_Init();
for(unsigned int i =0; i< 2; i ++){
commande_I[i]=0;
consigne_mm_s[i]=0;
}
}
/// @brief Défini une consigne de vitesse pour le moteur indiqué.
/// @param moteur : Moteur à asservir
/// @param _consigne_mm_s : consigne de vitesse en mm/s
void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, float _consigne_mm_s){
consigne_mm_s[moteur] = _consigne_mm_s;
}
/// @brief Envoie la consigne du moteur
/// @param moteur : Moteur à asservir
float AsserMoteur_getConsigne_mm_s(enum t_moteur moteur){
return consigne_mm_s[moteur];
}
float AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms){
enum QEI_name_t qei;
float distance, temps;
switch (moteur)
{
case MOTEUR_A: qei = QEI_A_NAME; break;
case MOTEUR_B: qei = QEI_B_NAME; break;
default: break;
}
distance = QEI_get_mm(qei);
temps = step_ms / 1000.0f;
return distance / temps;
}
/// @brief Indique si le robot est à l'arrêt
/// @param step_ms : pas de temps (utilisé pour déterminer les vitesses)
/// @return 1 si le robot est immobile, 0 s'il est en mouvement.
uint32_t AsserMoteur_RobotImmobile(int step_ms){
const float seuil_vitesse_immobile_mm_s = 0.1;
if(AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms) < seuil_vitesse_immobile_mm_s &&
AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) < seuil_vitesse_immobile_mm_s ){
return 1;
}
return 0;
}
/// @brief Fonction d'asservissement des moteurs, à appeler périodiquement
/// @param step_ms
void AsserMoteur_Gestion(int step_ms){
// Pour chaque moteur
for(uint moteur=MOTEUR_A; moteur<MOTEUR_B+1; moteur++ ){
float erreur; // Erreur entre la consigne et la vitesse actuelle
float commande_P; // Terme proportionnel
float commande;
// Calcul de l'erreur
erreur = consigne_mm_s[moteur] - AsserMoteur_getVitesse_mm_s(moteur, step_ms);
// Calcul du terme propotionnel
commande_P = erreur * ASSERMOTEUR_GAIN_P;
// Calcul du terme integral
commande_I[moteur] = commande_I[moteur] + (erreur * ASSERMOTEUR_GAIN_I * step_ms);
commande = commande_P + commande_I[moteur];
//Saturation de la commande
if(commande > 32760) {commande = 32760;}
if(commande < -32760) {commande = -32760;}
Moteur_SetVitesse(moteur, commande);
}
}

8
Asser_Moteurs.h Normal file
View File

@ -0,0 +1,8 @@
#include "Moteurs.h"
uint32_t AsserMoteur_RobotImmobile(int step_ms);
void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, float consigne_mm_s);
float AsserMoteur_getConsigne_mm_s(enum t_moteur moteur);
float AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms);
void AsserMoteur_Gestion(int step_ms);
void AsserMoteur_Init();

3
CMakeLists.txt
View File

@ -11,7 +11,10 @@ set(PICO_EXAMPLES_PATH ${PROJECT_SOURCE_DIR})
pico_sdk_init()
add_executable(Mon_Projet
Asser_Moteurs.c
Moteurs.c
main.c
Temps.c
QEI.c
)

98
Moteurs.c Normal file
View File

@ -0,0 +1,98 @@
#include "hardware/pwm.h"
#include "Moteurs.h"
#define MOTEUR_A 0
#define MOTEUR_B 1
#define MOTEUR_C 2
#define MOTEUR_A_VITESSE 6
#define MOTEUR_B_VITESSE 8
#define MOTEUR_C_VITESSE 10
#define MOTEUR_A_SENS 5
#define MOTEUR_B_SENS 7
#define MOTEUR_C_SENS 9
#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
uint slice_moteur_A, slice_moteur_B, slice_moteur_C;
/// @brief Initialisation les entrées / sorties requises pour les moteurs
void Moteur_Init(){
gpio_init(M1_SENS1);
gpio_init(M1_SENS2);
gpio_init(M2_SENS1);
gpio_init(M2_SENS2);
gpio_set_dir(M1_SENS1, GPIO_OUT);
gpio_set_dir(M1_SENS2, GPIO_OUT);
gpio_set_dir(M2_SENS1, GPIO_OUT);
gpio_set_dir(M2_SENS2, GPIO_OUT);
gpio_put(M1_SENS1, 0);
gpio_put(M1_SENS2, 0);
gpio_put(M2_SENS1, 0);
gpio_put(M2_SENS2, 0);
gpio_set_function(M1_VITESSE, GPIO_FUNC_PWM);
gpio_set_function(M2_VITESSE, GPIO_FUNC_PWM);
pwm_set_wrap(5, (uint16_t)65535);
pwm_set_wrap(4, (uint16_t)65535);
pwm_set_chan_level(5, PWM_CHAN_B, 0);
pwm_set_chan_level(4, PWM_CHAN_B, 0);
pwm_set_enabled(5, true);
pwm_set_enabled(4, true);
Moteur_SetVitesse(MOTEUR_A, 0);
Moteur_SetVitesse(MOTEUR_B, 0);
}
/// @brief Configure le PWM et la broche de sens en fonction de la vitesse et du moteur
/// @param moteur : Moteur (voir enum t_moteur)
/// @param vitesse : Vitesse entre -32767 et 32767
void Moteur_SetVitesse(enum t_moteur moteur, int16_t vitesse ){
uint16_t u_vitesse;
// Le PWM accepte 16 bits de résolution, on se remet sur 16 bits (et non sur 15 + signe)
if (vitesse < 0){
u_vitesse = -vitesse;
}else{
u_vitesse = vitesse;
}
u_vitesse = u_vitesse * 2;
switch(moteur){
case MOTEUR_A:
pwm_set_chan_level(5, PWM_CHAN_B, u_vitesse);
if(vitesse > 0){
gpio_put(M1_SENS1, 0);
gpio_put(M1_SENS2, 1);
}else{
gpio_put(M1_SENS1, 1);
gpio_put(M1_SENS2, 0);
}
break;
case MOTEUR_B:
pwm_set_chan_level(4, PWM_CHAN_B, u_vitesse);
if(vitesse < 0){
gpio_put(M2_SENS1, 0);
gpio_put(M2_SENS2, 1);
}else{
gpio_put(M2_SENS1, 1);
gpio_put(M2_SENS2, 0);
}
break;
}
}
void Moteur_Stop(void){
Moteur_SetVitesse(MOTEUR_A, 0);
Moteur_SetVitesse(MOTEUR_B, 0);
}

14
Moteurs.h Normal file
View File

@ -0,0 +1,14 @@
#include "pico/stdlib.h"
#ifndef MOTEURS_H
#define MOTEURS_H
enum t_moteur {
MOTEUR_A=0,
MOTEUR_B=1,
MOTEUR_C=2
};
#endif
void Moteur_Init(void);
void Moteur_SetVitesse(enum t_moteur moteur, int16_t vitesse );
void Moteur_Stop(void);

12
QEI.c
View File

@ -17,9 +17,9 @@
// Nombre d'impulsions par tour de roue : 8000
// Impulsion / mm : 42,44
#define IMPULSION_PAR_MM (95.4929658551372f)
#define ASSERMOTEUR_GAIN_P 160
#define ASSERMOTEUR_GAIN_I .80f
#define IMPULSION_PAR_MM (1.f)
#define ASSERMOTEUR_GAIN_P 2
#define ASSERMOTEUR_GAIN_I .0f
struct QEI_t QEI_A, QEI_B;
@ -87,11 +87,11 @@ int QEI_get(enum QEI_name_t qei){
switch (qei)
{
case QEI_A_NAME:
return QEI_A.delta;
return -QEI_A.delta;
break;
case QEI_B_NAME:
return -QEI_B.delta;
return QEI_B.delta;
break;
default:
@ -103,5 +103,5 @@ int QEI_get(enum QEI_name_t qei){
/// @param qei : Nom du module à lire (QEI_A_NAME, QEI_B_NAME ou QEI_C_NAME)
/// @return la distance parcourue en mm calculée lors du dernier appel de la function QEI_Update()
float QEI_get_mm(enum QEI_name_t qei){
return (float) QEI_get(qei) / (float)IMPULSION_PAR_MM;
return ((float) QEI_get(qei)) / (float)IMPULSION_PAR_MM;
}

24
Temps.c Normal file
View File

@ -0,0 +1,24 @@
#include <stdio.h>
#include "pico/stdlib.h"
#include "Temps.h"
uint32_t temps_ms=0;
bool temps_est_init=false;
struct repeating_timer timer;
bool Temps_increment(struct repeating_timer *t){
temps_ms++;
return true;
}
void Temps_init(void){
if(!temps_est_init){
temps_ms=0;
add_repeating_timer_ms(-1, Temps_increment, NULL, &timer);
temps_est_init = true;
}
}
uint32_t Temps_get_temps_ms(void){
return temps_ms;
}

5
Temps.h Normal file
View File

@ -0,0 +1,5 @@
#include "pico/stdlib.h"
bool Temps_increment(struct repeating_timer *t);
void Temps_init(void);
uint32_t Temps_get_temps_ms(void);

135
main.c
View File

@ -4,7 +4,11 @@
* 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"
@ -16,119 +20,52 @@
#define M2_SENS2 5
#define M2_VITESSE 9 //4B
void Moteur_init(void);
void M1_forward(int speed);
void M1_backward(int speed);
void M1_speed(int speed);
void M2_forward(int speed);
void M2_backward(int speed);
void M2_speed(int speed);
void affichage(void);
void main(void)
{
int ledpower = 500;
stdio_init_all();
QEI_init();
Moteur_init();
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){
QEI_update();
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) );
}
}
printf(">c1:%d\n>c2:%d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME) );
M1_speed(1000);
M2_speed(1000);
}
}
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);
}
}
void Moteur_init(void){
gpio_init(M1_SENS1);
gpio_init(M1_SENS2);
gpio_init(M2_SENS1);
gpio_init(M2_SENS2);
gpio_set_dir(M1_SENS1, GPIO_OUT);
gpio_set_dir(M1_SENS2, GPIO_OUT);
gpio_set_dir(M2_SENS1, GPIO_OUT);
gpio_set_dir(M2_SENS2, GPIO_OUT);
gpio_put(M1_SENS1, 0);
gpio_put(M1_SENS2, 0);
gpio_put(M2_SENS1, 0);
gpio_put(M2_SENS2, 0);
gpio_set_function(M1_VITESSE, GPIO_FUNC_PWM);
gpio_set_function(M2_VITESSE, GPIO_FUNC_PWM);
pwm_set_wrap(5, 1000);
pwm_set_wrap(4, 1000);
pwm_set_chan_level(5, PWM_CHAN_B, 0);
pwm_set_chan_level(4, PWM_CHAN_B, 0);
pwm_set_enabled(5, true);
pwm_set_enabled(4, true);
}
void M1_forward(int speed)
{
pwm_set_chan_level(5, PWM_CHAN_B, speed);
gpio_put(M1_SENS1, 1);
gpio_put(M1_SENS2, 0);
}
// Set motor 1 speed backward
void M1_backward(int speed)
{
pwm_set_chan_level(5, PWM_CHAN_B, speed);
gpio_put(M1_SENS1, 0);
gpio_put(M1_SENS2, 1);
}
// Set motor 1 speed and direction (negative value : backward / positive value : forward)
void M1_speed(int speed)
{
if(speed < 0)
{
speed = -speed;
M1_backward(speed);
}
else
{
M1_forward(speed);
}
}
// Set motor 2 speed forward
void M2_forward(int speed)
{
pwm_set_chan_level(4, PWM_CHAN_B, speed);
gpio_put(M2_SENS1, 0);
gpio_put(M2_SENS2, 1);
}
// Set motor 2 speed backward
void M2_backward(int speed)
{
pwm_set_chan_level(4, PWM_CHAN_B, speed);
gpio_put(M2_SENS1, 1);
gpio_put(M2_SENS2, 0);
}
// Set motor 2 speed and direction (negative value : backward / positive value : forward)
void M2_speed(int speed)
{
if(speed < 0)
{
speed = -speed;
M2_backward(speed);
}
else
{
M2_forward(speed);
}
}
}