From 7403263cea17cde4e7126222ead68d24b9255acf Mon Sep 17 00:00:00 2001
From: Nathan Girard <nathan.girard@polymtl.ca>
Date: Sun, 15 Mar 2020 15:12:00 -0400
Subject: [PATCH] cleaning code and add robot_type choice
---
Device/list_motor.c | 94 +++++++---------------------------
Device/motor.c | 119 +++++++++++++++++++++++++++++++++-----------
Device/motor.h | 5 ++
3 files changed, 115 insertions(+), 103 deletions(-)
diff --git a/Device/list_motor.c b/Device/list_motor.c
index 15f3e5c..82b8094 100644
--- a/Device/list_motor.c
+++ b/Device/list_motor.c
@@ -33,66 +33,6 @@ const pid_struct_t M2006_pid_pos = { //A FAIRE
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;
-//}
-
void motors_init(motor_t *motor, int16_t motor_rx_id, enum motors mtr){
@@ -132,21 +72,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 = 2220;
+ gm6020_up.MAX_POSITION = 3840;
+ gm6020_up.target = 3450;
+ gm6020_down.MIN_POSITION = 2800;
+ gm6020_down.MAX_POSITION = 6700;
+ gm6020_down.target = 4750;
+ 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 4a2174c..62bb0b5 100644
--- a/Device/motor.c
+++ b/Device/motor.c
@@ -1,19 +1,23 @@
#include "motor.h"
+#include "list_motor.h"
#include "oled.h"
#include "pid.h"
+#include "bsp_uart.h"
+
+extern RC_ctrl_t rc_ctrl;
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; //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);
}
@@ -50,23 +54,23 @@ void run_motor_from_command(motor_t *motor, pid_struct_t pid_param, float target
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);
-
+ 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;
- }
+
+ 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;
+ }
}
@@ -92,7 +96,7 @@ void set_motor_speed(motor_t *motor)
float target = motor->target;
switch(motor->type){
case M3508:
- if ((target < 100.0) && (target > -100.0)){
+ if ((target < 100) && (target > -100)){
run_motor_from_command(motor, motor->pid_pos, target, motor->info.M3508_info.angle);
}
else{
@@ -106,3 +110,62 @@ void set_motor_speed(motor_t *motor)
bsp_error_handler(__FUNCTION__, __LINE__, "motor type undefined");*/
}
}
+
+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 */
+ 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.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;}
+}
+
+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 7928979..6a6a491 100644
--- a/Device/motor.h
+++ b/Device/motor.h
@@ -14,6 +14,9 @@
#define MAX_BASE_SPEED_COEFF 6
+#define ROBOT_ID 1 /*set 1 for Standard méca (homemade)
+ 2 for Standard ICRA*/
+
enum motors {
/* can motors */
M3508,
@@ -98,5 +101,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
--
GitLab