diff --git a/Device/list_motor.c b/Device/list_motor.c
index d668d8cc67cfb60b89986a18e6094bd34215d6c1..6af5ed8b7bc7040219ad202bd1ed56a1ec713923 100644
--- a/Device/list_motor.c
+++ b/Device/list_motor.c
@@ -10,90 +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
};
-//void motors_all_init(void){
-//
-// m3508_frontleft.type = M3508;
-// m3508_frontleft.can_info.can_id = 0;
-// m3508_frontleft.can_info.tx_id = 0;
-// m3508_frontleft.can_info.rx_id = 0x201;
-// m3508_frontleft.pid_pos = M3508_pid_pos;
-// m3508_frontleft.pid_speed = M3508_pid_speed;
-//
-// m3508_frontright.type = M3508;
-// m3508_frontright.can_info.can_id = 0;
-// m3508_frontright.can_info.tx_id = 0;
-// m3508_frontright.can_info.rx_id = 0x202;
-// m3508_frontright.pid_pos = M3508_pid_pos;
-// m3508_frontright.pid_speed = M3508_pid_speed;
-//
-// m3508_backleft.type = M3508;
-// m3508_backleft.can_info.can_id = 0;
-// m3508_backleft.can_info.tx_id = 0;
-// m3508_backleft.can_info.rx_id = 0x203;
-// m3508_backleft.pid_pos = M3508_pid_pos;
-// m3508_backleft.pid_speed = M3508_pid_speed;
-//
-// m3508_backright.type = M3508;
-// m3508_backright.can_info.can_id = 0;
-// m3508_backright.can_info.tx_id = 0;
-// m3508_backright.can_info.rx_id = 0x204;
-// m3508_backright.pid_pos = M3508_pid_pos;
-// m3508_backright.pid_speed = M3508_pid_speed;
-//
-// m2006.type = M2006;
-// m2006.can_info.can_id = 0;
-// m2006.can_info.tx_id = 1;
-// m2006.can_info.rx_id = 0x207;
-// m2006.pid_pos = M2006_pid_pos;
-// m2006.pid_speed = M2006_pid_speed;
-
-// gm6020_up.type = GM6020;
-// gm6020_up.can_info.can_id = 0;
-// gm6020_up.can_info.tx_id = 1;
-// gm6020_up.can_info.rx_id = 0x205;
-// gm6020_up.pid_pos = GM6020_pid_pos;
-// gm6020_up.pid_speed = GM6020_pid_speed;
-// gm6020_up.MIN_POSITION = 5565;
-// gm6020_up.MAX_POSITION = 6675;
-//
-// gm6020_down.type = GM6020;
-// gm6020_down.can_info.can_id = 0;
-// gm6020_down.can_info.tx_id = 1;
-// gm6020_down.can_info.rx_id = 0x206;
-// gm6020_down.pid_pos = GM6020_pid_pos;
-// gm6020_down.pid_speed = GM6020_pid_speed;
-// gm6020_down.MIN_POSITION = 2519;
-// gm6020_down.MAX_POSITION = 7015;
-//
-//
-// gm6020_down.target = 4750;
-// gm6020_up.target = 6200;
-//}
-
+/****************************************/
+/* MOTORS INITIALISATION */
+/****************************************/
void motors_init(motor_t *motor, int16_t motor_rx_id, enum motors mtr){
switch (mtr){
@@ -132,21 +78,25 @@ void motors_all_init(void){
motors_init(&m3508_backright, 0x203, M3508);
motors_init(&m3508_backleft, 0x204, M3508);
- // Param�tres ICRA
-// gm6020_up.MIN_POSITION = 5565;
-// gm6020_up.MAX_POSITION = 6675;
-// gm6020_up.target = 6200;
-// gm6020_down.MIN_POSITION = 2519;
-// gm6020_down.MAX_POSITION = 7015;
-// gm6020_down.target = 4750;
-
- // 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;
+ switch (ROBOT_ID){
+ case 1: // Param�tres StandardMeca
+ 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
+ gm6020_up.MIN_POSITION = 5565;
+ gm6020_up.MAX_POSITION = 6675;
+ gm6020_up.target = 6200;
+ gm6020_down.MIN_POSITION = 2519;
+ gm6020_down.MAX_POSITION = 7015;
+ gm6020_down.target = 4750;
+ break;
+ }
}
diff --git a/Device/motor.c b/Device/motor.c
index 720826220b50dbb7f532b0e9881da1083278c1ec..108c608062e3b2a386fa3ae6f148ccacea9a0440 100644
--- a/Device/motor.c
+++ b/Device/motor.c
@@ -1,27 +1,70 @@
#include "motor.h"
+#include "list_motor.h"
#include "oled.h"
#include "pid.h"
+#include "bsp_uart.h"
+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é
- sp1 = sp2 = sp3 = sp4 = 0;
- tx_id = id;
-
- if (motor1) { sp1 = correct_output(motor1); }
- if (motor2) { sp2 = correct_output(motor2); }
- if (motor3) { sp3 = correct_output(motor3); }
- if (motor4) { sp4 = correct_output(motor4); }
-
- can1_transmit(tx_id, sp1, sp2, sp3, sp4);
+ int16_t sp1, sp2, sp3, sp4;
+ uint16_t tx_id;
+ sp1 = sp2 = sp3 = sp4 = 0;
+ tx_id = id;
+
+ if (motor1) { sp1 = correct_output(motor1); }
+ if (motor2) { sp2 = correct_output(motor2); }
+ if (motor3) { sp3 = correct_output(motor3); }
+ if (motor4) { sp4 = correct_output(motor4); }
+
+ can1_transmit(tx_id, sp1, sp2, sp3, sp4);
}
-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]);
@@ -29,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]);
@@ -36,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]);
@@ -43,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;
@@ -83,15 +119,18 @@ 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;
switch(motor->type){
case M3508:
+<<<<<<< HEAD
<<<<<<< HEAD
if ((target < 150) && (target > -100)){
run_motor_from_command(motor, motor->pid_speed, target, motor->info.M3508_info.speed);
@@ -107,6 +146,14 @@ void set_motor_speed(motor_t *motor)
run_motor_from_command(motor, motor->pid_speed, target, motor->info.M3508_info.speed);
}
>>>>>>> 623e9ad5906292f23a358f1ea43fd89ee37f30d3
+=======
+ if ((target < 100) && (target > -100)){
+ run_motor_from_command(motor, motor->pid_pos, target, motor->info.M3508_info.angle);
+ }
+ else{
+ run_motor_from_command(motor, motor->pid_speed, target, motor->info.M3508_info.speed);
+ }
+>>>>>>> position_M3508_BSP
case GM6020:
run_motor_from_command(motor, motor->pid_speed, target, motor->info.GM6020_info.speed);
case M2006:
@@ -115,3 +162,105 @@ void set_motor_speed(motor_t *motor)
bsp_error_handler(__FUNCTION__, __LINE__, "motor type undefined");*/
}
}
+
+/**
+ * @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 */
+ // Switch 1 : m2006 motor (feeder)
+ if (rc_ctrl.rc.sw[0] == 2)
+ { m2006.target = 0; };
+ if (rc_ctrl.rc.sw[0] == 3)
+ { m2006.target = 2000; };
+ if (rc_ctrl.rc.sw[0] == 1)
+ { m2006.target = 5000; };
+
+ //Switch 2 : snails (shoot)
+ if (rc_ctrl.rc.sw[1] == 2)
+ { PWM_SetAllDuty(&htim1, 0.0f, 0.0f, 0.0f, 0.0f); };
+ if (rc_ctrl.rc.sw[1] == 3)
+ { PWM_SetAllDuty(&htim1, 0.25f, 0.25f, 0.25f, 0.25f); };
+ if (rc_ctrl.rc.sw[1] == 1)
+ { 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 */
+ 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;
+
+ 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*/
+ get_motor_data(&m3508_frontleft);
+ get_motor_data(&m3508_frontright);
+ get_motor_data(&m3508_backleft);
+ get_motor_data(&m3508_backright);
+ get_motor_data(&m2006);
+ get_motor_data(&gm6020_up);
+ get_motor_data(&gm6020_down);
+
+ /* Calculate the command to be sent */
+ set_motor_speed(&m3508_frontleft);
+ set_motor_speed(&m3508_frontright);
+ set_motor_speed(&m3508_backleft);
+ set_motor_speed(&m3508_backright);
+ set_motor_speed(&m2006);
+ set_motor_position(&gm6020_up);
+ set_motor_position(&gm6020_down);
+
+ /* Transmit the command to the motors */
+ set_motor_voltage(0, &m3508_frontleft, &m3508_frontright, &m3508_backright, &m3508_backleft); //0x200
+ set_motor_voltage(1, &gm6020_up, &gm6020_down, &m2006, NULL); //0x1ff
+}
diff --git a/Device/motor.h b/Device/motor.h
index 7928979bb7bd60510f839a7ccf520fe234d1fe2d..40bca3230831f32a4b094821138c385ecf76e28e 100644
--- a/Device/motor.h
+++ b/Device/motor.h
@@ -14,6 +14,9 @@
#define MAX_BASE_SPEED_COEFF 6
+#define ROBOT_ID 2 /*set 1 for Standard méca (homemade)
+ 2 for Standard ICRA */
+
enum motors {
/* can motors */
M3508,
@@ -74,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;
@@ -98,5 +100,7 @@ static void get_2006_data(motor_t* motor, uint8_t buf[CAN_DATA_SIZE]);
void set_motor_position(motor_t *motor);
void set_motor_speed(motor_t *motor);
void run_motor_from_command(motor_t *motor, pid_struct_t pid_param, float target, float fdb);
+void set_all_targets_from_rc(void);
+void command_all_motors(void);
#endif
diff --git a/Driver/bsp_uart.c b/Driver/bsp_uart.c
index e80e7044b00d939a15be53b84676d4ca655a0845..040ef746f7463dbeda901452265d6ff49c96cca6 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) || \