Device/list_motor.c

@@ -3,12 +3,15 @@
 #include "list_motor.h"
 
 motor_t gm6020;
+motor_t gm6020_up;
+motor_t gm6020_down;
 motor_t m3508_frontleft;
 motor_t m3508_frontright;
 motor_t m3508_backleft;
 motor_t m3508_backright;
 motor_t m2006;
 
+
 const pid_struct_t M3508_pid_speed = { //A VERIFIER
 		1, 2, 0.005, 2, 16000, 16000
 	};
@@ -31,13 +34,13 @@ const pid_struct_t M2006_pid_pos = { //A FAIRE
 		0, 12, 3, 5, 10000, 10000
 	};
 
-void motors_init(void){
-	gm6020.type = GM6020;
+void motors_all_init(void){
+	/*gm6020.type = GM6020;
 	gm6020.can_info.can_id = 0;
 	gm6020.can_info.tx_id = 1;
 	gm6020.can_info.rx_id = 0x205; 
 	gm6020.pid_pos = GM6020_pid_pos;
-	gm6020.pid_speed = GM6020_pid_speed;
+	gm6020.pid_speed = GM6020_pid_speed;*/
 	
 	m3508_frontleft.type = M3508;
 	m3508_frontleft.can_info.can_id = 0;
@@ -73,5 +76,66 @@ void motors_init(void){
 	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){
+	
+	switch (mtr){
+		case GM6020:
+			motor->type = GM6020 ;
+			motor->pid_pos = GM6020_pid_pos;
+			motor->pid_speed = GM6020_pid_speed;
+			motor->can_info.tx_id = 1;
+			break;
+		case M3508:
+			motor->type = M3508 ;
+			motor->pid_pos = M3508_pid_pos;
+			motor->pid_speed = M3508_pid_speed;
+			motor->can_info.tx_id = 0;
+			break;
+		case M2006:
+			motor->type = M2006 ;
+			motor->pid_pos = M2006_pid_pos;
+			motor->pid_speed = M2006_pid_speed;
+			motor->can_info.tx_id = 1;
+			break;
+		default :
+			break;
+	}
+	motor->can_info.can_id = 0;
+	motor->can_info.rx_id = motor_rx_id ;
+}
+
+void motors_all_init(void){
+	motors_init(&m2006,  0x207, M2006);
+	motors_init(&gm6020_down,  0x206, GM6020);
+	motors_init(&gm6020_up,  0x205, GM6020);
+	motors_init(&m3508_frontleft,  0x204, M3508);
+	motors_init(&m3508_frontright,  0x203, M3508);
+	motors_init(&m3508_backleft,  0x202, M3508);
+	motors_init(&m3508_backright,  0x201, M3508);
 }
 
+*/

Device/list_motor.h

@@ -18,4 +18,5 @@ extern const pid_struct_t M3508_pid_pos;
 extern const pid_struct_t GM6020_pid_speed;
 extern const pid_struct_t GM6020_pid_pos;
 
-void motors_init(void);
+void motors_init(motor_t *motor, int16_t motor_rx_id, enum motors mtr);
+void motors_all_init(void);

Device/motor.c

@@ -61,6 +61,9 @@ uint8_t get_motor_data(motor_t *motor) {
 			case M3508:
 				get_3508_data(motor, motor_buf);
 				return 1;
+			case M2006:
+				get_2006_data(motor, motor_buf);
+				return 1;
 			default:
 				return 0;
     }
@@ -75,6 +78,8 @@ void set_motor_position(motor_t *motor)
 				run_motor_from_command(motor, motor->pid_pos, target, motor->info.M3508_info.angle);
 		case GM6020:
 				run_motor_from_command(motor, motor->pid_pos, target, motor->info.GM6020_info.angle);
+		case M2006:
+				run_motor_from_command(motor, motor->pid_pos, target, motor->info.M2006_info.angle);
 		/*default:
 			bsp_error_handler(__FUNCTION__, __LINE__, "motor type undefined");*/
 	}
@@ -87,9 +92,11 @@ void set_motor_speed(motor_t *motor)
 	float target = motor->target;
 	switch(motor->type){
 		case M3508:
-				run_motor_from_command(motor, motor->pid_speed, target, motor->info.M3508_info.speed);;
+				run_motor_from_command(motor, motor->pid_speed, target, motor->info.M3508_info.speed);
 		case GM6020:
-				run_motor_from_command(motor, motor->pid_speed, target, motor->info.GM6020_info.speed);;
+				run_motor_from_command(motor, motor->pid_speed, target, motor->info.GM6020_info.speed);
+		case M2006:
+				run_motor_from_command(motor, motor->pid_speed, target, motor->info.M2006_info.speed);
 		/*default:
 			bsp_error_handler(__FUNCTION__, __LINE__, "motor type undefined");*/
 	}

Device/motor.h

@@ -12,6 +12,8 @@
 #define CAN_RX1_START 0x201
 #define CAN_RX2_START 0x205
 
+#define MAX_BASE_SPEED_COEFF  6
+
 enum motors {
 	/* can motors */
 	M3508,
@@ -80,6 +82,8 @@ typedef struct
 	pid_struct_t pid_speed;			// Controler
 	float target;
 	float voltage;
+	int MAX_POSITION;
+	int MIN_POSITION;
 } motor_t;
 	
 //id:0 -> 0x200, 1 -> 0x1ff

Driver/bsp_can.c

@@ -88,7 +88,7 @@ void can1_read(uint16_t id, uint8_t buf[CAN_DATA_SIZE]) {
 
 /**
   * @brief  send motor control message through can bus
-  * @param  id_range to select can control id 0x1ff or 0x2ff
+  * @param  id_range to select can control id 0x200 or 0x1ff
   * @param  motor voltage 1,2,3,4 or 5,6,7
   * @retval None
   */