From 3703310bb1d2ef0274c59a2733b34c685d2494e8 Mon Sep 17 00:00:00 2001
From: Nathan Girard <nathan.girard@polymtl.ca>
Date: Sun, 15 Mar 2020 18:30:33 -0400
Subject: [PATCH] Cleaning et standardisation
---
Device/list_motor.c | 30 +++++----
Device/motor.c | 155 +++++++++++++++++++++++++++++++++-----------
Device/motor.h | 13 ++--
Driver/bsp_uart.c | 3 +-
4 files changed, 141 insertions(+), 60 deletions(-)
diff --git a/Device/list_motor.c b/Device/list_motor.c
index 82b8094..5053a54 100644
--- a/Device/list_motor.c
+++ b/Device/list_motor.c
@@ -10,30 +10,36 @@ motor_t m3508_backleft;
motor_t m3508_backright;
motor_t m2006;
+/****************************************/
+/* PID PARAMETERS */
+/****************************************/
-const pid_struct_t M3508_pid_speed = { //A VERIFIER
+const pid_struct_t M3508_pid_speed = {
1, 2, 0.005, 2, 16000, 16000
};
-const pid_struct_t M3508_pid_pos = { //A VERIFIER
+const pid_struct_t M3508_pid_pos = {
0, 0.1, 0.005, 0, 16000, 16000
};
-const pid_struct_t GM6020_pid_speed = { //A VERIFIER
+const pid_struct_t GM6020_pid_speed = {
1, 40, 10, 0, 30000, 30000
};
-const pid_struct_t GM6020_pid_pos = { //A VERIFIER
+const pid_struct_t GM6020_pid_pos = {
0, 12, 3, 5, 10000, 10000
};
-const pid_struct_t M2006_pid_speed = { //A FAIRE
+const pid_struct_t M2006_pid_speed = {
1, 1.5, 0.1, 0, 16384, 16384
};
-const pid_struct_t M2006_pid_pos = { //A FAIRE
+const pid_struct_t M2006_pid_pos = {
0, 12, 3, 5, 10000, 10000
};
+/****************************************/
+/* MOTORS INITIALISATION */
+/****************************************/
void motors_init(motor_t *motor, int16_t motor_rx_id, enum motors mtr){
switch (mtr){
@@ -74,12 +80,12 @@ void motors_all_init(void){
switch (ROBOT_ID){
case 1: // Paramètres StandardMeca
- gm6020_up.MIN_POSITION = 2220;
- gm6020_up.MAX_POSITION = 3840;
- gm6020_up.target = 3450;
- gm6020_down.MIN_POSITION = 2800;
- gm6020_down.MAX_POSITION = 6700;
- gm6020_down.target = 4750;
+ gm6020_up.MIN_POSITION = 7000;
+ gm6020_up.MAX_POSITION = 8000;
+ gm6020_up.target = 7450;
+ gm6020_down.MIN_POSITION = -1300;
+ gm6020_down.MAX_POSITION = 2740;
+ gm6020_down.target = 660;
break;
case 2: // Paramètres ICRA
diff --git a/Device/motor.c b/Device/motor.c
index 62bb0b5..d72b259 100644
--- a/Device/motor.c
+++ b/Device/motor.c
@@ -6,9 +6,21 @@
extern RC_ctrl_t rc_ctrl;
+static int16_t correct_output(motor_t *motor) {
+ return motor->voltage;
+}
+
+/************************************************************/
+/* CAN SEND AND RECEIVE */
+/************************************************************/
+/**
+ * @brief send its command to each motor
+ * @param[in] CAN ID
+ * @param[in] the 4 motors that use this ID (see documentation)
+ */
void set_motor_voltage(uint8_t id, motor_t *motor1, motor_t *motor2, motor_t *motor3, motor_t *motor4) {
int16_t sp1, sp2, sp3, sp4;
- uint16_t tx_id; //can_id était déclaré là mais pas utilisé
+ uint16_t tx_id;
sp1 = sp2 = sp3 = sp4 = 0;
tx_id = id;
@@ -21,11 +33,38 @@ void set_motor_voltage(uint8_t id, motor_t *motor1, motor_t *motor2, motor_t *mo
}
-static int16_t correct_output(motor_t *motor) {
- return motor->voltage;
+/**
+ * @brief reads the CAN buff and calls the function to get data from a motor
+ * @param[in] motor
+ */
+uint8_t get_motor_data(motor_t *motor) {
+ static uint8_t motor_buf[CAN_DATA_SIZE];
+ can1_read(motor->can_info.rx_id, motor_buf);
+
+ switch (motor->type) {
+ case GM6020:
+ get_6020_data(motor, motor_buf);
+ return 1;
+ case M3508:
+ get_3508_data(motor, motor_buf);
+ return 1;
+ case M2006:
+ get_2006_data(motor, motor_buf);
+ return 1;
+ default:
+ return 0;
+ }
}
+/************************************************************/
+/* GET MOTORS DATA */
+/************************************************************/
+/**
+ * @brief Get CAN data from a GM6020 motor
+ * @param[in] motor
+ * @param[in] receive buff
+ */
static void get_6020_data(motor_t* motor, uint8_t motor_buf[CAN_DATA_SIZE]){
motor->info.GM6020_info.angle = (int16_t)(motor_buf[0] << 8 | motor_buf[1]);
motor->info.GM6020_info.speed = (int16_t)(motor_buf[2] << 8 | motor_buf[3]);
@@ -33,6 +72,11 @@ static void get_6020_data(motor_t* motor, uint8_t motor_buf[CAN_DATA_SIZE]){
motor->info.GM6020_info.temp = (int8_t)(motor_buf[6]);
}
+/**
+ * @brief Get CAN data from a M3508 motor
+ * @param[in] motor
+ * @param[in] receive buff
+ */
static void get_3508_data(motor_t* motor, uint8_t motor_buf[CAN_DATA_SIZE]){
motor->info.M3508_info.angle = (int16_t)(motor_buf[0] << 8 | motor_buf[1]);
motor->info.M3508_info.speed = (int16_t)(motor_buf[2] << 8 | motor_buf[3]);
@@ -40,6 +84,11 @@ static void get_3508_data(motor_t* motor, uint8_t motor_buf[CAN_DATA_SIZE]){
motor->info.M3508_info.temp = (int8_t)(motor_buf[6]);
}
+/**
+ * @brief Get CAN data from a M2006 motor
+ * @param[in] motor
+ * @param[in] receive buff
+ */
static void get_2006_data(motor_t* motor, uint8_t motor_buf[CAN_DATA_SIZE]){
motor->info.M2006_info.angle = (int16_t)(motor_buf[0] << 8 | motor_buf[1]);
motor->info.M2006_info.speed = (int16_t)(motor_buf[2] << 8 | motor_buf[3]);
@@ -47,33 +96,16 @@ static void get_2006_data(motor_t* motor, uint8_t motor_buf[CAN_DATA_SIZE]){
motor->info.M2006_info.temp = (int8_t)(motor_buf[6]);
}
-void run_motor_from_command(motor_t *motor, pid_struct_t pid_param, float target, float fdb)
-{
- motor->voltage = pid_calc(&pid_param, target, fdb);
-}
-uint8_t get_motor_data(motor_t *motor) {
- static uint8_t motor_buf[CAN_DATA_SIZE];
- can1_read(motor->can_info.rx_id, motor_buf);
-
-
- switch (motor->type) {
- case GM6020:
- get_6020_data(motor, motor_buf);
- return 1;
- case M3508:
- get_3508_data(motor, motor_buf);
- return 1;
- case M2006:
- get_2006_data(motor, motor_buf);
- return 1;
- default:
- return 0;
- }
-}
-
+/************************************************************/
+/* SET TARGET AND RUN MOTORS */
+/************************************************************/
+/**
+ * @brief sets the target (position) of a motor
+ * @param[in] motor
+ */
void set_motor_position(motor_t *motor)
{
float target = motor->target;
@@ -87,10 +119,12 @@ void set_motor_position(motor_t *motor)
/*default:
bsp_error_handler(__FUNCTION__, __LINE__, "motor type undefined");*/
}
-
}
-
+/**
+ * @brief sets the target (speed) of a motor
+ * @param[in] motor
+ */
void set_motor_speed(motor_t *motor)
{
float target = motor->target;
@@ -111,6 +145,26 @@ void set_motor_speed(motor_t *motor)
}
}
+/**
+ * @brief sets the command (voltage) of each motor after PID calculated it
+ * @param[in] motor
+ * @param[in] PID parameters of the motor
+ * @param[in] target (speed or position)
+ * @param[in] feedback (speed or angle) from the motor
+ */
+void run_motor_from_command(motor_t *motor, pid_struct_t pid_param, float target, float fdb)
+{
+ motor->voltage = pid_calc(&pid_param, target, fdb);
+}
+
+
+
+/************************************************************/
+/* GLOBAL RUNNING FUNCTIONS */
+/************************************************************/
+/**
+ * @brief sets all targets from remote controler command
+ */
void set_all_targets_from_rc()
{
/* Switches activation */
@@ -131,20 +185,43 @@ void set_all_targets_from_rc()
{ PWM_SetAllDuty(&htim1, 0.5f, 0.5f, 0.5f, 0.5f); };
/* Setting the target for all motors according to the data received from the remote controler */
- m3508_frontleft.target = MAX_BASE_SPEED_COEFF*(rc_ctrl.rc.ch[0] + rc_ctrl.rc.ch[1] - rc_ctrl.rc.ch[4]);
- m3508_frontright.target = -MAX_BASE_SPEED_COEFF*(rc_ctrl.rc.ch[0] - rc_ctrl.rc.ch[1] + rc_ctrl.rc.ch[4]);
- m3508_backright.target = -MAX_BASE_SPEED_COEFF*(rc_ctrl.rc.ch[0] + rc_ctrl.rc.ch[1] + rc_ctrl.rc.ch[4]);
- m3508_backleft.target = MAX_BASE_SPEED_COEFF*(rc_ctrl.rc.ch[0] - rc_ctrl.rc.ch[1] - rc_ctrl.rc.ch[4]);
-
+ switch (ROBOT_ID){
+ case 1:
+ m3508_frontleft.target = MAX_BASE_SPEED_COEFF*(rc_ctrl.rc.ch[0] + rc_ctrl.rc.ch[1] - rc_ctrl.rc.ch[4]);
+ m3508_frontright.target = -MAX_BASE_SPEED_COEFF*(rc_ctrl.rc.ch[0] - rc_ctrl.rc.ch[1] + rc_ctrl.rc.ch[4]);
+ m3508_backright.target = -MAX_BASE_SPEED_COEFF*(rc_ctrl.rc.ch[0] + rc_ctrl.rc.ch[1] + rc_ctrl.rc.ch[4]);
+ m3508_backleft.target = MAX_BASE_SPEED_COEFF*(rc_ctrl.rc.ch[0] - rc_ctrl.rc.ch[1] - rc_ctrl.rc.ch[4]);
+ case 2:
+ m3508_frontleft.target = MAX_BASE_SPEED_COEFF*(rc_ctrl.rc.ch[0] - rc_ctrl.rc.ch[1] - rc_ctrl.rc.ch[4]);
+ m3508_frontright.target = -MAX_BASE_SPEED_COEFF*(rc_ctrl.rc.ch[0] + rc_ctrl.rc.ch[1] + rc_ctrl.rc.ch[4]);
+ m3508_backright.target = -MAX_BASE_SPEED_COEFF*(rc_ctrl.rc.ch[0] - rc_ctrl.rc.ch[1] + rc_ctrl.rc.ch[4]);
+ m3508_backleft.target = MAX_BASE_SPEED_COEFF*(rc_ctrl.rc.ch[0] + rc_ctrl.rc.ch[1] - rc_ctrl.rc.ch[4]);
+ }
gm6020_down.target = gm6020_down.target - rc_ctrl.rc.ch[3]*0.1;
- gm6020_up.target = gm6020_up.target - rc_ctrl.rc.ch[2]*0.1;
+ gm6020_up.target = gm6020_up.target + rc_ctrl.rc.ch[2]*0.1;
- if (gm6020_down.target > gm6020_down.MAX_POSITION){gm6020_down.target = gm6020_down.MAX_POSITION;}
- if (gm6020_down.target < gm6020_down.MIN_POSITION){gm6020_down.target = gm6020_down.MIN_POSITION;}
- if (gm6020_up.target > gm6020_up.MAX_POSITION){gm6020_up.target = gm6020_up.MAX_POSITION;}
- if (gm6020_up.target < gm6020_up.MIN_POSITION){gm6020_up.target = gm6020_up.MIN_POSITION;}
+ if (gm6020_down.MAX_POSITION > gm6020_down.MIN_POSITION){
+ if (gm6020_down.target > gm6020_down.MAX_POSITION){gm6020_down.target = gm6020_down.MAX_POSITION;}
+ if (gm6020_down.target < gm6020_down.MIN_POSITION){gm6020_down.target = gm6020_down.MIN_POSITION;}
+ }
+ else{
+ if (gm6020_down.target > gm6020_down.MAX_POSITION && gm6020_down.target < gm6020_down.MAX_POSITION+1000){gm6020_down.target = gm6020_down.MAX_POSITION;}
+ if (gm6020_down.target < gm6020_down.MIN_POSITION && gm6020_down.target > gm6020_down.MIN_POSITION-1000){gm6020_down.target = gm6020_down.MIN_POSITION;}
+ }
+
+ if (gm6020_up.MAX_POSITION > gm6020_up.MIN_POSITION){
+ if (gm6020_up.target > gm6020_up.MAX_POSITION){gm6020_up.target = gm6020_up.MAX_POSITION;}
+ if (gm6020_up.target < gm6020_up.MIN_POSITION){gm6020_up.target = gm6020_up.MIN_POSITION;}
+ }
+ else{
+ if (gm6020_up.target > gm6020_up.MAX_POSITION && gm6020_up.target < gm6020_up.MAX_POSITION+1000){gm6020_up.target = gm6020_up.MAX_POSITION;}
+ if (gm6020_up.target < gm6020_up.MIN_POSITION && gm6020_up.target > gm6020_up.MIN_POSITION-1000){gm6020_up.target = gm6020_up.MIN_POSITION;}
+ }
}
+/**
+ * @brief gets the data from each motor and execute the function to run it
+ */
void command_all_motors()
{
/* Get data from all motors*/
diff --git a/Device/motor.h b/Device/motor.h
index 6a6a491..40bca32 100644
--- a/Device/motor.h
+++ b/Device/motor.h
@@ -14,8 +14,8 @@
#define MAX_BASE_SPEED_COEFF 6
-#define ROBOT_ID 1 /*set 1 for Standard méca (homemade)
- 2 for Standard ICRA*/
+#define ROBOT_ID 2 /*set 1 for Standard méca (homemade)
+ 2 for Standard ICRA */
enum motors {
/* can motors */
@@ -77,12 +77,11 @@ typedef union{
// Une struct par moteur
typedef struct
{
- can_info_t can_info; // Can ID information
- //M3508_info_t info;
- motor_info_t info; // Specific info about the motor
- int type; // Type du moteur, ex:GM6020
+ can_info_t can_info; // Can ID information
+ motor_info_t info; // Specific info about the motor
+ int type; // Type du moteur, ex:GM6020
pid_struct_t pid_pos; // Controler
- pid_struct_t pid_speed; // Controler
+ pid_struct_t pid_speed; // Controler
float target;
float voltage;
int MAX_POSITION;
diff --git a/Driver/bsp_uart.c b/Driver/bsp_uart.c
index e80e704..040ef74 100644
--- a/Driver/bsp_uart.c
+++ b/Driver/bsp_uart.c
@@ -100,9 +100,8 @@ void rc_callback_handler(RC_ctrl_t *rc_ctrl, uint8_t *buff)
rc_ctrl->mouse.press_l = buff[12]; //!< Mouse Left Is Press ?
rc_ctrl->mouse.press_r = buff[13]; //!< Mouse Right Is Press ?
rc_ctrl->key.v = buff[14] | (buff[15] << 8); //!< KeyBoard value
- rc_ctrl->rc.ch[4] = buff[16] | (buff[17] << 8); //!< Left Dial
+ rc_ctrl->rc.ch[4] = buff[16] | (buff[17] << 8); //!< Left Dial
rc_ctrl->rc.ch[4] -= 1024;
-
if ((abs(rc_ctrl->rc.ch[0]) > 660) || \
(abs(rc_ctrl->rc.ch[1]) > 660) || \
--
GitLab