Device/list_motor.c

@@ -7,6 +7,7 @@ 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
@@ -22,6 +23,14 @@ const pid_struct_t GM6020_pid_pos = { //A VERIFIER
 		0, 12, 3, 5, 10000, 10000
 	};
 
+const pid_struct_t M2006_pid_speed = { //A FAIRE
+		1, 1.5, 0.1, 0, 16384, 16384
+	};
+
+const pid_struct_t M2006_pid_pos = { //A FAIRE
+		0, 12, 3, 5, 10000, 10000
+	};
+
 void motors_init(void){
 	gm6020.type = GM6020;
 	gm6020.can_info.can_id = 0;
@@ -57,5 +66,12 @@ void motors_init(void){
 	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;
 }
 

Device/motor.c

@@ -36,6 +36,13 @@ 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]);
 }
 
+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]);
+	motor->info.M2006_info.current = (int16_t)(motor_buf[4] << 8 | motor_buf[5]);
+	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);

Device/motor.h

@@ -55,9 +55,17 @@ typedef struct {
 		int8_t			temp;
 }   M3508_info_t;
 
+typedef struct {    
+    int16_t     angle;
+    int16_t     speed;
+    int16_t     current;
+		int8_t			temp;
+}   M2006_info_t;
+
 typedef union{
 	GM6020_info_t GM6020_info;
 	M3508_info_t M3508_info;
+	M2006_info_t M2006_info;
 } motor_info_t;
 
 
@@ -80,6 +88,8 @@ void set_motor_voltage(uint8_t id, motor_t *motor1, motor_t *motor2, motor_t *mo
 static int16_t correct_output(motor_t *motor);
 uint8_t get_motor_data(motor_t *motor);
 static void get_6020_data(motor_t* motor, uint8_t buf[CAN_DATA_SIZE]);
+static void get_3508_data(motor_t* motor, uint8_t buf[CAN_DATA_SIZE]);
+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);

Driver/bsp_uart.c

@@ -91,8 +91,18 @@ void rc_callback_handler(RC_ctrl_t *rc_ctrl, uint8_t *buff)
   rc_ctrl->rc.ch[0] = (buff[4] >> 1 | buff[5] << 7) & 0x07FF;
   rc_ctrl->rc.ch[0] -= 1024;
 
-  rc_ctrl->rc.sw[0] = ((buff[5] >> 4) & 0x000C) >> 2;
-  rc_ctrl->rc.sw[1] = (buff[5] >> 4) & 0x0003;
+  rc_ctrl->rc.sw[0] = ((buff[5] >> 4) & 0x000C) >> 2;							//!< Switch right
+  rc_ctrl->rc.sw[1] = (buff[5] >> 4) & 0x0003;										//!< Switch left
+	
+	rc_ctrl->mouse.x = buff[6] | (buff[7] << 8);                    //!< Mouse X axis
+	rc_ctrl->mouse.y = buff[8] | (buff[9] << 8);                    //!< Mouse Y axis
+	rc_ctrl->mouse.z = buff[10] | (buff[11] << 8);                  //!< Mouse Z axis
+	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] -= 1024;
+
   
   if ((abs(rc_ctrl->rc.ch[0]) > 660) || \
       (abs(rc_ctrl->rc.ch[1]) > 660) || \