From bf0a036f1cdfe06f31cb0898ced82756fcb86fe9 Mon Sep 17 00:00:00 2001
From: sfaguet <sfaguet@centumadetel.com>
Date: Sat, 30 Jan 2021 15:48:31 -0500
Subject: [PATCH] Adptation + Configuration pour le STD de Meca
---
Inc/motors.h | 3 +-
Src/BoardA_handle.c | 10 +---
Src/main.c | 4 +-
Src/motors.c | 7 +--
Src/robot_configuration.c | 102 +++++++++++++++++++++++++++++++++++++-
Src/traitement.c | 2 +-
6 files changed, 111 insertions(+), 17 deletions(-)
diff --git a/Inc/motors.h b/Inc/motors.h
index d9e3a64..d1a2199 100644
--- a/Inc/motors.h
+++ b/Inc/motors.h
@@ -47,6 +47,7 @@ typedef struct
struct pid_controller pid; // Controler
float consigne;
float command;
+ float direction; //1 ou -1
float MAX_POSITION;
float MIN_POSITION;
uint32_t signOfLife_tick;
@@ -74,6 +75,6 @@ void PWM_init(void);
void PWM_SetAllDuty(TIM_HandleTypeDef *tim, float duty_ch1, float duty_ch2);
/* scales all PWM duty cycles between 0 and 1 */
-void PWM_ScaleAll(TIM_HandleTypeDef *tim);
+void PWM_ScaleAll(TIM_HandleTypeDef *tim, bool switchRotationalDirection);
#endif
diff --git a/Src/BoardA_handle.c b/Src/BoardA_handle.c
index 6da7189..949e773 100644
--- a/Src/BoardA_handle.c
+++ b/Src/BoardA_handle.c
@@ -95,7 +95,7 @@ Exemple : uart_debug_printf("\tAngle: %u (%x)\r\n", motors[TOURELLE_YAW].info.an
*/
void uart_debug(){
static uint32_t tickstart = 0;
- static char buff[1000] = {0};
+ //static char buff[1000] = {0};
static char buff2[1000] = {0};
if(tickstart == 0){
tickstart = HAL_GetTick();
@@ -104,13 +104,7 @@ void uart_debug(){
return;
}
tickstart = HAL_GetTick();
- snprintf(buff2, 1000, "%f,%f,%f,", motors[FRONT_LEFT].consigne, motors[FRONT_LEFT].command, motors[FRONT_LEFT].info.speed);
- snprintf(buff, 1000, "%f,%f,%f,", motors[FRONT_RIGHT].consigne, motors[FRONT_RIGHT].command, motors[FRONT_RIGHT].info.speed);
- strcat(buff2, buff);
- snprintf(buff, 1000, "%f,%f,%f,", motors[BACK_LEFT].consigne, motors[BACK_LEFT].command, motors[BACK_LEFT].info.speed);
- strcat(buff2, buff);
- snprintf(buff, 1000, "%f,%f,%f_", motors[BACK_RIGHT].consigne, motors[BACK_RIGHT].command, motors[BACK_RIGHT].info.speed);
- strcat(buff2, buff);
+ snprintf(buff2, 1000, " PITCH(%f/%f) YAW(%f/%f)\r\n", motors[TOURELLE_PITCH].info.angle_360, motors[TOURELLE_PITCH].consigne, motors[TOURELLE_YAW].info.angle_360, motors[TOURELLE_YAW].consigne);
HAL_UART_Transmit_DMA(&huart8, (uint8_t*)buff2, strlen(buff2));
/*
uart_debug_command("[2J"); //Clear entire screen
diff --git a/Src/main.c b/Src/main.c
index 97d7549..a474459 100644
--- a/Src/main.c
+++ b/Src/main.c
@@ -100,14 +100,14 @@ int main(void)
MX_TIM1_Init();
/* USER CODE BEGIN 2 */
PWM_init();
- //PWM_ScaleAll(&htim1); //etalonnage du PWM entre 0 et 1 (a faire uniquement lors de l'installation des moteurs la 1ere fois) info: motors.c
+ //PWM_ScaleAll(&htim1, false); //etalonnage du PWM entre 0 et 1 (a faire uniquement lors de l'installation des moteurs la 1ere fois) info: motors.c
HAL_GPIO_WritePin(GPIOH, BOARD_POWER1_CTRL_Pin|BOARD_POWER2_CTRL_Pin|BOARD_POWER3_CTRL_Pin|BOARD_POWER4_CTRL_Pin, GPIO_PIN_SET); // switch on 24v power
BOARD_LED_ALL_OFF
oled_init();
uart1_init();
uart6_init();
can1_init();
- robotInit(3); //Initialise le robot comme un standard, l'objectif c'est que l'initialisation de fasse par le referee system
+ robotInit(5); //Initialise le robot comme un standard, l'objectif c'est que l'initialisation de fasse par le referee system
extern motor_t motors[MAX_MOTORS];
/* USER CODE END 2 */
diff --git a/Src/motors.c b/Src/motors.c
index baff08b..342bb9b 100644
--- a/Src/motors.c
+++ b/Src/motors.c
@@ -76,7 +76,7 @@ void can_motors_callback_handler(int16_t rx_id, uint8_t* rx_buff){
/* Modifie la consigne tout en vérifiant les limites de postion */
void add_consigne_position(motor_t* motor, float value, float coeff){
- float consigne_position = motor->consigne + (value * coeff);
+ float consigne_position = motor->consigne + (motor->direction * value * coeff);
if(consigne_position > 360) consigne_position -= (float) 360.0;
if(consigne_position < 0) consigne_position += (float) 360.0;
if(motor->MAX_POSITION > 0 && consigne_position > motor->MAX_POSITION) consigne_position = motor->MAX_POSITION;
@@ -142,13 +142,14 @@ Pour utiliser cette fonction, il faut le faire moteur PWM par moteur PWM
d) Coupe tous le signal PWM
*/
-void PWM_ScaleAll(TIM_HandleTypeDef *tim){ //il faudrait jouer sur l'allumage des ports d'alimentation des snails (voir Nathan pour plus de détails)
+void PWM_ScaleAll(TIM_HandleTypeDef *tim, bool switchRotationalDirection){ //il faudrait jouer sur l'allumage des ports d'alimentation des snails (voir Nathan pour plus de détails)
HAL_GPIO_WritePin(GPIOH, BOARD_POWER1_CTRL_Pin, GPIO_PIN_RESET); // switch off 24v power
PWM_SetAllDuty(&htim1,1,1);
HAL_Delay(10);
HAL_GPIO_WritePin(GPIOH, BOARD_POWER1_CTRL_Pin, GPIO_PIN_SET); // switch on 24v power
- HAL_Delay(3500);
+ if(switchRotationalDirection) HAL_Delay(7500);
+ else HAL_Delay(3500);
PWM_SetAllDuty(&htim1,0,0);
HAL_Delay(500);
diff --git a/Src/robot_configuration.c b/Src/robot_configuration.c
index 1f8b2f7..b45f233 100644
--- a/Src/robot_configuration.c
+++ b/Src/robot_configuration.c
@@ -59,8 +59,104 @@ void robotInit(uint8_t robot_id){
switch(robot_id){
/* Standard */
case 3:
- case 4:
- case 5:
+ case 4: //Robot Meca STD
+ strcpy(motors[FRONT_LEFT].debug_name, "FRONT_LEFT");
+ motors[FRONT_LEFT].type = M3508;
+ motors[FRONT_LEFT].can_rx_id = 0x200+1; // ID = 1
+ motors[FRONT_LEFT].can_tx_frame = 0x200;
+ motors[FRONT_LEFT].can_tx_id = 1;
+ pid_create(&motors[FRONT_LEFT].pid,
+ &motors[FRONT_LEFT].info.speed, //input : le retour sur la quelle ont veut atteintre la consigne
+ &motors[FRONT_LEFT].command, //output: la commande que l'on envoie au moteur
+ &motors[FRONT_LEFT].consigne, //consigne: On veut que le moteur soit à cette position ou tourne a cette vitesse
+ pid_chassis_p, pid_chassis_i, pid_chassis_d); //k, i, d : les coefficient de régulation : http://www.ferdinandpiette.com/blog/2011/08/implementer-un-pid-sans-faire-de-calculs/
+ pid_limits(&motors[FRONT_LEFT].pid, -16384, 16384); //Minimum et maximum de la commande envoyable au moteur
+
+
+ strcpy(motors[FRONT_RIGHT].debug_name, "FRONT RIGHT");
+ motors[FRONT_RIGHT].type = M3508;
+ motors[FRONT_RIGHT].can_rx_id = 0x200+2; // ID = 2
+ motors[FRONT_RIGHT].can_tx_frame = 0x200;
+ motors[FRONT_RIGHT].can_tx_id = 2;
+ pid_create(&motors[FRONT_RIGHT].pid,
+ &motors[FRONT_RIGHT].info.speed, //input : le retour sur la quelle ont veut atteintre la consigne
+ &motors[FRONT_RIGHT].command, //output: la commande que l'on envoie au moteur
+ &motors[FRONT_RIGHT].consigne, //consigne: On veut que le moteur soit à cette position ou tourne a cette vitesse
+ pid_chassis_p, pid_chassis_i, pid_chassis_d); //k, i, d : les coefficient de régulation : http://www.ferdinandpiette.com/blog/2011/08/implementer-un-pid-sans-faire-de-calculs/
+ pid_limits(&motors[FRONT_RIGHT].pid, -16384, 16384); //Minimum et maximum de la commande envoyable au moteur
+
+ strcpy(motors[BACK_RIGHT].debug_name, "BACK RIGHT");
+ motors[BACK_RIGHT].type = M3508;
+ motors[BACK_RIGHT].can_rx_id = 0x200+3; // ID = 3
+ motors[BACK_RIGHT].can_tx_frame = 0x200;
+ motors[BACK_RIGHT].can_tx_id = 3;
+ pid_create(&motors[BACK_RIGHT].pid,
+ &motors[BACK_RIGHT].info.speed, //input : le retour sur la quelle ont veut atteintre la consigne
+ &motors[BACK_RIGHT].command, //output: la commande que l'on envoie au moteur
+ &motors[BACK_RIGHT].consigne, //consigne: On veut que le moteur soit à cette position ou tourne a cette vitesse
+ pid_chassis_p, pid_chassis_i, pid_chassis_d); //k, i, d : les coefficient de régulation : http://www.ferdinandpiette.com/blog/2011/08/implementer-un-pid-sans-faire-de-calculs/
+ pid_limits(&motors[BACK_RIGHT].pid, -16384, 16384); //Minimum et maximum de la commande envoyable au moteur
+
+ strcpy(motors[BACK_LEFT].debug_name, "BACK LEFT");
+ motors[BACK_LEFT].type = M3508;
+ motors[BACK_LEFT].can_rx_id = 0x200+4; // ID = 4
+ motors[BACK_LEFT].can_tx_frame = 0x200;
+ motors[BACK_LEFT].can_tx_id = 4;
+ pid_create(&motors[BACK_LEFT].pid,
+ &motors[BACK_LEFT].info.speed, //input : le retour sur la quelle ont veut atteintre la consigne
+ &motors[BACK_LEFT].command, //output: la commande que l'on envoie au moteur
+ &motors[BACK_LEFT].consigne, //consigne: On veut que le moteur soit à cette position ou tourne a cette vitesse
+ pid_chassis_p, pid_chassis_i, pid_chassis_d); //k, i, d : les coefficient de régulation : http://www.ferdinandpiette.com/blog/2011/08/implementer-un-pid-sans-faire-de-calculs/
+ pid_limits(&motors[BACK_LEFT].pid, -16384, 16384); //Minimum et maximum de la commande envoyable au moteur
+
+ strcpy(motors[TOURELLE_PITCH].debug_name, "TOURELLE PITCH");
+ motors[TOURELLE_PITCH].type = GM6020;
+ motors[TOURELLE_PITCH].can_rx_id = 0x204+1; // ID = 1
+ motors[TOURELLE_PITCH].can_tx_frame = 0x1FF;
+ motors[TOURELLE_PITCH].can_tx_id = 1;
+ motors[TOURELLE_PITCH].MIN_POSITION = 120; //en deg
+ motors[TOURELLE_PITCH].MAX_POSITION = 213; //en deg //213 120
+ motors[TOURELLE_PITCH].consigne = 213; //en deg //Valeur initiale
+ motors[TOURELLE_PITCH].direction = -1; //permet de choisir la direction de controle (-1 ou 1)
+ pid_create(&motors[TOURELLE_PITCH].pid,
+ &motors[TOURELLE_PITCH].info.angle_360, //input : le retour sur la quelle ont veut atteintre la consigne
+ &motors[TOURELLE_PITCH].command, //output: la commande que l'on envoie au moteur
+ &motors[TOURELLE_PITCH].consigne, //consigne: On veut que le moteur soit à cette position ou tourne a cette vitesse
+ 400, 100, 0); //k, i, d : les coefficient de régulation : http://www.ferdinandpiette.com/blog/2011/08/implementer-un-pid-sans-faire-de-calculs/
+ pid_circulaire(&motors[TOURELLE_PITCH].pid, 360); //Asservissement circulaire, permet, comme on fait une régulation en position, quand on est a position = 350 degrée, que la consigne est à 10deg, de ne pas faire tout le tour
+ pid_limits(&motors[TOURELLE_PITCH].pid, -30000, 30000); //Minimum et maximum de la commande envoyable au moteur
+
+ strcpy(motors[TOURELLE_YAW].debug_name, "TOURELLE YAW");
+ motors[TOURELLE_YAW].type = GM6020;
+ motors[TOURELLE_YAW].can_rx_id = 0x204+2; // ID = 2
+ motors[TOURELLE_YAW].can_tx_frame = 0x1FF;
+ motors[TOURELLE_YAW].can_tx_id = 2;
+ motors[TOURELLE_YAW].MIN_POSITION = 110.71; //en deg
+ motors[TOURELLE_YAW].MAX_POSITION = 308.44; //en deg
+ motors[TOURELLE_YAW].consigne = 208; //en deg //Valeur initiale
+ motors[TOURELLE_YAW].direction = -1; //permet de choisir la direction de controle (-1 ou 1)
+ pid_create(&motors[TOURELLE_YAW].pid,
+ &motors[TOURELLE_YAW].info.angle_360, //input : le retour sur la quelle ont veut atteintre la consigne
+ &motors[TOURELLE_YAW].command, //output: la commande que l'on envoie au moteur
+ &motors[TOURELLE_YAW].consigne, //consigne: On veut que le moteur soit à cette position ou tourne a cette vitesse
+ 200, 100, 0); //k, i, d : les coefficient de régulation : http://www.ferdinandpiette.com/blog/2011/08/implementer-un-pid-sans-faire-de-calculs/
+ pid_circulaire(&motors[TOURELLE_YAW].pid, 360); //Asservissement circulaire, permet, comme on fait une régulation en position, quand on est a position = 350 degrée, que la consigne est à 10deg, de ne pas faire tout le tour
+ pid_limits(&motors[TOURELLE_YAW].pid, -30000, 30000); //Minimum et maximum de la commande envoyable au moteur
+
+ strcpy(motors[FEEDER].debug_name, "FEEDER");
+ motors[FEEDER].type = M2006;
+ motors[FEEDER].can_rx_id = 0x200+7; // ID = 7
+ motors[FEEDER].can_tx_frame = 0x1FF;
+ motors[FEEDER].can_tx_id = 7-4;
+ pid_create(&motors[FEEDER].pid,
+ &motors[FEEDER].info.speed, //input : le retour sur la quelle ont veut atteintre la consigne
+ &motors[FEEDER].command, //output: la commande que l'on envoie au moteur
+ &motors[FEEDER].consigne, //consigne: On veut que le moteur soit à cette position ou tourne a cette vitesse
+ 0.5, 0.5, 0); //k, i, d : les coefficient de régulation : http://www.ferdinandpiette.com/blog/2011/08/implementer-un-pid-sans-faire-de-calculs/
+ pid_limits(&motors[FEEDER].pid, -10000, 10000); //Minimum et maximum de la commande envoyable au moteur
+
+ break;
+ case 5: //ROBOT DJI
strcpy(motors[FRONT_LEFT].debug_name, "FRONT_LEFT");
motors[FRONT_LEFT].type = M3508;
motors[FRONT_LEFT].can_rx_id = 0x200+1; // ID = 1
@@ -118,6 +214,7 @@ void robotInit(uint8_t robot_id){
motors[TOURELLE_PITCH].MIN_POSITION = 245; //en deg
motors[TOURELLE_PITCH].MAX_POSITION = 293; //en deg
motors[TOURELLE_PITCH].consigne = 250; //en deg //Valeur initiale
+ motors[TOURELLE_PITCH].direction = 1; //permet de choisir la direction de controle (-1 ou 1)
pid_create(&motors[TOURELLE_PITCH].pid,
&motors[TOURELLE_PITCH].info.angle_360, //input : le retour sur la quelle ont veut atteintre la consigne
&motors[TOURELLE_PITCH].command, //output: la commande que l'on envoie au moteur
@@ -134,6 +231,7 @@ void robotInit(uint8_t robot_id){
motors[TOURELLE_YAW].MIN_POSITION = 110.71; //en deg
motors[TOURELLE_YAW].MAX_POSITION = 308.44; //en deg
motors[TOURELLE_YAW].consigne = 208; //en deg //Valeur initiale
+ motors[TOURELLE_YAW].direction = -1; //permet de choisir la direction de controle (-1 ou 1)
pid_create(&motors[TOURELLE_YAW].pid,
&motors[TOURELLE_YAW].info.angle_360, //input : le retour sur la quelle ont veut atteintre la consigne
&motors[TOURELLE_YAW].command, //output: la commande que l'on envoie au moteur
diff --git a/Src/traitement.c b/Src/traitement.c
index 80597e1..d944c4b 100644
--- a/Src/traitement.c
+++ b/Src/traitement.c
@@ -25,7 +25,7 @@ void traitement_pids_compute(){
/* Fonctions qui fait les liens entre les entrées (capteurs, radio controller, CV, ...) et les sorties (consignes moteurs), on peut créer plusieurs traitements */
void traitement_1(){
add_consigne_position(&motors[TOURELLE_PITCH], (float)receiver_RadioController.data.ch2_float, 0.00005);
- add_consigne_position(&motors[TOURELLE_YAW], (float)receiver_RadioController.data.ch1_float, -0.00005);
+ add_consigne_position(&motors[TOURELLE_YAW], (float)receiver_RadioController.data.ch1_float, 0.00005);
switch(receiver_RadioController.data.sw1){
--
GitLab