Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • polystar/robomaster/controle/bsp
1 result
Show changes
Commits on Source (3)
......@@ -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;
}
......@@ -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);
......
......@@ -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);
......
......@@ -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) || \
......