67 lines
1.3 KiB
C
67 lines
1.3 KiB
C
#include "omni_chassis.h"
|
||
#include "pid.h"
|
||
#include "struct_typedef.h"
|
||
#include "CAN_receive.h"
|
||
#include "bsp_delay.h"
|
||
#include "bsp_can.h"
|
||
#include "bsp_rc.h"
|
||
#include "remote_control.h"
|
||
#include "device/bmi088.h"
|
||
pid_type_def motor_pid[4];
|
||
const motor_measure_t *motor_data[4];
|
||
fp32 PID[3]={5,0.3,0}; //P,I,D
|
||
extern RC_ctrl_t rc_ctrl;
|
||
|
||
|
||
void chassis_init(void)
|
||
{
|
||
|
||
for(int i=0;i<4;i++)
|
||
{
|
||
PID_init(&motor_pid[i],PID_POSITION,PID,16000,2000);
|
||
motor_data[i] = get_chassis_motor_measure_point(i);
|
||
}
|
||
}
|
||
int16_t vx_set,vy_set,vw_set;//x,y,w
|
||
int16_t mul;//
|
||
int16_t motor_set_speed[4];//
|
||
int16_t v[4];//
|
||
int16_t gyro_z;
|
||
/*****ʽ*****/
|
||
void RC_chassis_mode(float gyro_Z)
|
||
{
|
||
|
||
|
||
|
||
vx_set = rc_ctrl.ch[1]*0.9;
|
||
vy_set = rc_ctrl.ch[0]*0.9;
|
||
vw_set = rc_ctrl.ch[3]*0.9;
|
||
mul = rc_ctrl.ch[2]*0.0018995;
|
||
|
||
gyro_z = gyro_Z*800;
|
||
if(vw_set!=0 || (vx_set==0&&vy_set==0)) gyro_z=0;
|
||
|
||
|
||
|
||
v[0] = (vx_set+vy_set+vw_set)*mul + gyro_z;//
|
||
v[1] = (-vx_set+vy_set+vw_set)*mul + gyro_z;//
|
||
v[2] = (-vx_set-vy_set+vw_set)*mul + gyro_z;//
|
||
v[3] = (vx_set-vy_set+vw_set)*mul + gyro_z;//
|
||
|
||
for(int i=0;i<4;i++)
|
||
{
|
||
|
||
// M3508_velocity_loop(v[0],v[1],v[2],v[3]);
|
||
motor_set_speed[i] = PID_calc(&motor_pid[i], motor_data[i]->speed_rpm, v[i]);
|
||
}
|
||
|
||
CAN_cmd_chassis(motor_set_speed[0],motor_set_speed[1],motor_set_speed[2],motor_set_speed[3]);
|
||
|
||
delay_ms(2);
|
||
|
||
}
|
||
|
||
|
||
|
||
|