Skip to content
Snippets Groups Projects
Commit da5acdaa authored by Baptiste Toussaint's avatar Baptiste Toussaint
Browse files

m2006

parent 59ef48d5
No related branches found
No related tags found
Loading
...@@ -7,6 +7,7 @@ motor_t m3508_frontleft; ...@@ -7,6 +7,7 @@ motor_t m3508_frontleft;
motor_t m3508_frontright; motor_t m3508_frontright;
motor_t m3508_backleft; motor_t m3508_backleft;
motor_t m3508_backright; motor_t m3508_backright;
motor_t m2006;
const pid_struct_t M3508_pid_speed = { //A VERIFIER const pid_struct_t M3508_pid_speed = { //A VERIFIER
1, 2, 0.005, 2, 16000, 16000 1, 2, 0.005, 2, 16000, 16000
...@@ -22,6 +23,14 @@ const pid_struct_t GM6020_pid_pos = { //A VERIFIER ...@@ -22,6 +23,14 @@ const pid_struct_t GM6020_pid_pos = { //A VERIFIER
0, 12, 3, 5, 10000, 10000 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){ void motors_init(void){
gm6020.type = GM6020; gm6020.type = GM6020;
gm6020.can_info.can_id = 0; gm6020.can_info.can_id = 0;
...@@ -57,5 +66,12 @@ void motors_init(void){ ...@@ -57,5 +66,12 @@ void motors_init(void){
m3508_backright.can_info.rx_id = 0x204; m3508_backright.can_info.rx_id = 0x204;
m3508_backright.pid_pos = M3508_pid_pos; m3508_backright.pid_pos = M3508_pid_pos;
m3508_backright.pid_speed = M3508_pid_speed; 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;
} }
...@@ -36,6 +36,13 @@ static void get_3508_data(motor_t* motor, uint8_t motor_buf[CAN_DATA_SIZE]){ ...@@ -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]); 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) 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); motor->voltage = pid_calc(&pid_param, target, fdb);
......
...@@ -55,9 +55,17 @@ typedef struct { ...@@ -55,9 +55,17 @@ typedef struct {
int8_t temp; int8_t temp;
} M3508_info_t; } M3508_info_t;
typedef struct {
int16_t angle;
int16_t speed;
int16_t current;
int8_t temp;
} M2006_info_t;
typedef union{ typedef union{
GM6020_info_t GM6020_info; GM6020_info_t GM6020_info;
M3508_info_t M3508_info; M3508_info_t M3508_info;
M2006_info_t M2006_info;
} motor_info_t; } motor_info_t;
...@@ -80,6 +88,8 @@ void set_motor_voltage(uint8_t id, motor_t *motor1, motor_t *motor2, motor_t *mo ...@@ -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); static int16_t correct_output(motor_t *motor);
uint8_t get_motor_data(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_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_position(motor_t *motor);
void set_motor_speed(motor_t *motor); void set_motor_speed(motor_t *motor);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment