m2006

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);