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