#include "Chassis.h"
#include "define.h"

/*舵轮舵向校准方法：注释掉关于6020反馈角度的处理以及6020数据的发送这两处，
进debug将四个轮子编码器朝右（左右无所谓，可能会导致5065方向反，在解算里加个负号就行）
查看6020反馈值，将6020反馈值放入motor_offset中*/



//底盘初始化
int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq){
	 c->param = param; 	/*初始化参数 */

    //舵轮安装时的6020机械误差，机械校准时1号轮在左前方，所有轮的编码器朝向右面   
	 MotorOffset_t motor_offset = { {90.12088, 269.571480, 208.523989, 28.545959}};	
   c->motoroffset = motor_offset;  // 将 motor_offset 的值赋给 c->motoroffset
	
   //在这里修改雷达校准时sick的值    
	 c->SICK_FALG = 0; 		
   c->sick_set[0] = 2500;
   c->sick_set[1] = 2500;
   c->sick_set[2] = 600;
	 
	 //蜂鸣器初始化
	 Buzzer_Init(&c->buzzer_radar_angle,200,1000,0.5);
	 Buzzer_Init(&c->buzzer_sick_calibration,200,1000,0.5);	 
	
	 for(int i=0;i<4;i++){
	   PID_init(&(c->pid.chassis_6020OmegaPid[i]), PID_POSITION,&(c->param->C6020Omega_param));
	   PID_init(&(c->pid.chassis_6020anglePid[i]), PID_POSITION,&(c->param->C6020Angle_param)); 		 
	 }
	
	 PID_init(&(c->pid.chassis_RadaranglePID),PID_POSITION,&(c->param->RadarAngle_param));
	 PID_init(&(c->pid.chassis_RadarspeedPID),PID_POSITION,&(c->param->RadarSpeed_param));
	
	 PID_init(&(c->pid.chassis_SickVx), PID_POSITION,&(c->param->SickVx_param));
	 PID_init(&(c->pid.chassis_SickVy), PID_POSITION,&(c->param->SickVy_param));	 
	 PID_init(&(c->pid.chassis_SickVw), PID_POSITION,&(c->param->SickVw_param));

	 LowPassFilter2p_Init(&(c->filled[0]),target_freq,80.0f); //给x 做滤波
	 LowPassFilter2p_Init(&(c->filled[1]),target_freq,80.0f); //给y 做滤波	
	 LowPassFilter2p_Init(&(c->filled[2]),target_freq,80.0f); //给w 做滤波
	 LowPassFilter2p_Init(&(c->filled[3]),target_freq,8.0f); //给雷达x做滤波
	 LowPassFilter2p_Init(&(c->filled[4]),target_freq,8.0f); //给雷达y做滤波
	 LowPassFilter2p_Init(&(c->filled[5]),target_freq,8.0f); //给雷达z做滤波
	 LowPassFilter2p_Init(&(c->filled[6]),target_freq,10.0f); //给sick1做滤波
	 LowPassFilter2p_Init(&(c->filled[7]),target_freq,10.0f); //给sick2做滤波
	 LowPassFilter2p_Init(&(c->filled[8]),target_freq,10.0f); //给sick3做滤波
	 return CHASSIS_OK;
}

static int8_t Chassis_SetCtrl(Chassis_t *c,CMD_t *ctrl){
	 c->mode =ctrl->C_cmd.mode;
   c->pos =ctrl->C_cmd.pos;	   
	 return 0;
}

//该函数用来更新其他任务获得的数据
int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {

  for (uint8_t i = 0; i < 4; i++) {
		
		c->motorfeedback.rotor_rpm3508[i] = can->motor.motor3508.as_array[i].rotor_speed;
		c->motorfeedback.rotor_current3508[i] = can->motor.motor3508.as_array[i].torque_current;
	 
    c->motorfeedback.rotor_rpm6020[i] = can->motor.chassis6020.as_array[i].rotor_speed;
		c->motorfeedback.rotor_current6020[i] = can->motor.chassis6020.as_array[i].torque_current;
		c->motorfeedback.rotor_angle6020[i] = can->motor.chassis6020.as_array[i].rotor_angle;
		c->motorfeedback.rotor_temp6020[i] = can->motor.chassis6020.as_array[i].temp;
		
#ifdef calibration
#else  
    //由于安装不能保证0点朝向我们想要朝向的方向，所以进行零点偏移
		c->motorfeedback.rotor_angle6020[i] = fmod(can->motor.chassis6020.as_array[i].rotor_angle - 
		                                                    c->motoroffset.MOTOR_OFFSET[i], 360.0);
		if(c->motorfeedback.rotor_angle6020[i]<0){
		c->motorfeedback.rotor_angle6020[i]+=360;
		}		
#endif
			
	  c->motorfeedback.rotor_rpm5065[i] = can->motor.chassis5065.as_array[i].rotor_speed;
	  c->motorfeedback.torque_current5065[i] = can->motor.chassis5065.as_array[i].torque_current;

 }	
  	//接收sick数据
    for (uint8_t i = 0; i < 3; i++) {
 	    c->sick[i] = can->sickfed.as_array[i].sick_distance;
 			c->sick[i] = LowPassFilter2p_Apply(&(c->filled[i+6]),c->sick[i]);
			c->sick[i] = c->sick[i]/10;
		}
  return CHASSIS_OK;
}

//底盘解算   
void Chassis_speed_calculate(Chassis_t *c,Action_POS_t*pos) {
	 
	// RC模式下松开遥控器防止6020回到默认位置导致侧翻
  if (c->mode == RC&&fabs(c->move_vec.Vx) < 3000 && fabs(c->move_vec.Vy) < 3000 && fabs(c->move_vec.Vw) < 3000) {
    // 如果之前不处于保持模式，则记录当前角度
    if (!c->keeping_angle_flag) {
				c->keeping_angle_flag = 1;  // 进入保持模式
    }
 	    
    // 使用保持的角度，而不是实时反馈值,速度为0
    for (uint8_t i = 0; i < 4; i++) {
      c->hopemotorout.rotor6020_jiesuan_1[i] = c->keep_angle[i];
      c->hopemotorout.rotor5065_jiesuan_1[i] = 0;
		}        
  } 
	else {
    // 如果有速度输入，则退出保持模式
    c->keeping_angle_flag = 0;
		//让保持角度实时等于进入保持阈值前的最后一次角度值
		for (uint8_t i = 0; i < 4; i++) {
      c->keep_angle[i] = c->hopemotorout.rotor6020_jiesuan_1[i];
    }
		
    switch(c->mode){	
			case RC: //RC模式下遥控器控制底盘运动，操控为机器人坐标系  x朝前，y朝右
				//解算得到5065速度，加-号是因为vesc校准时轮向电机默认转动正方向不同，可从上位机里改
				c->hopemotorout.rotor5065_jiesuan_1[0]=-sqrt(
			  (c->move_vec.Vx+c->move_vec.Vw*sin(radians))*(c->move_vec.Vx+c->move_vec.Vw*sin(radians))+
				(c->move_vec.Vy+c->move_vec.Vw*cos(radians))*(c->move_vec.Vy+c->move_vec.Vw*cos(radians)));	
  
				c->hopemotorout.rotor5065_jiesuan_1[1]=sqrt(
				(c->move_vec.Vx-c->move_vec.Vw*sin(radians))*(c->move_vec.Vx-c->move_vec.Vw*sin(radians))+
				(c->move_vec.Vy+c->move_vec.Vw*cos(radians))*(c->move_vec.Vy+c->move_vec.Vw*cos(radians)));	
 
				c->hopemotorout.rotor5065_jiesuan_1[2]=-sqrt(
				(c->move_vec.Vx+c->move_vec.Vw*sin(radians))*(c->move_vec.Vx+c->move_vec.Vw*sin(radians))+
				(c->move_vec.Vy-c->move_vec.Vw*cos(radians))*(c->move_vec.Vy-c->move_vec.Vw*cos(radians)));	
 
				c->hopemotorout.rotor5065_jiesuan_1[3]=sqrt(
				(c->move_vec.Vx-c->move_vec.Vw*sin(radians))*(c->move_vec.Vx-c->move_vec.Vw*sin(radians))+
				(c->move_vec.Vy-c->move_vec.Vw*cos(radians))*(c->move_vec.Vy-c->move_vec.Vw*cos(radians)));
	
				//解算得到6020角度（-180°——+180°）
				c->hopemotorout.rotor6020_jiesuan_1[0]=atan2((c->move_vec.Vy+c->move_vec.Vw*cos(radians)),
																			(c->move_vec.Vx+c->move_vec.Vw*sin(radians)))* (180 / M_PI);
				c->hopemotorout.rotor6020_jiesuan_1[1]=atan2((c->move_vec.Vy+c->move_vec.Vw*cos(radians)),
																			(c->move_vec.Vx-c->move_vec.Vw*sin(radians)))* (180 / M_PI);
				c->hopemotorout.rotor6020_jiesuan_1[2]=atan2((c->move_vec.Vy-c->move_vec.Vw*cos(radians)),
																			(c->move_vec.Vx+c->move_vec.Vw*sin(radians)))* (180 / M_PI);
				c->hopemotorout.rotor6020_jiesuan_1[3]=atan2((c->move_vec.Vy-c->move_vec.Vw*cos(radians)),
																			(c->move_vec.Vx-c->move_vec.Vw*sin(radians)))* (180 / M_PI);
	    break;
	    case STOP: //停止模式下轮子锁死
		    for(int i =0 ; i < 4 ; i++){
			    c->hopemotorout.rotor5065_jiesuan_1[i]=0;	
		    }
			
		    c->hopemotorout.rotor6020_jiesuan_1[0]=315;
		    c->hopemotorout.rotor6020_jiesuan_1[1]=45;
		    c->hopemotorout.rotor6020_jiesuan_1[2]=45;
		    c->hopemotorout.rotor6020_jiesuan_1[3]=315;   
		  break;
	    case NAVI: 
#ifdef radar
      //相机，雷达纠正时就用正常解算
	    c->hopemotorout.rotor5065_jiesuan_1[0]=-sqrt(
			(c->move_vec.Vx+c->move_vec.Vw*sin(radians))*(c->move_vec.Vx+c->move_vec.Vw*sin(radians))+
			(c->move_vec.Vy+c->move_vec.Vw*cos(radians))*(c->move_vec.Vy+c->move_vec.Vw*cos(radians)));	
  
			c->hopemotorout.rotor5065_jiesuan_1[1]=sqrt(
			(c->move_vec.Vx-c->move_vec.Vw*sin(radians))*(c->move_vec.Vx-c->move_vec.Vw*sin(radians))+
			(c->move_vec.Vy+c->move_vec.Vw*cos(radians))*(c->move_vec.Vy+c->move_vec.Vw*cos(radians)));	
 
			c->hopemotorout.rotor5065_jiesuan_1[2]=-sqrt(
			(c->move_vec.Vx+c->move_vec.Vw*sin(radians))*(c->move_vec.Vx+c->move_vec.Vw*sin(radians))+
			(c->move_vec.Vy-c->move_vec.Vw*cos(radians))*(c->move_vec.Vy-c->move_vec.Vw*cos(radians)));	
 
			c->hopemotorout.rotor5065_jiesuan_1[3]=sqrt(
			(c->move_vec.Vx-c->move_vec.Vw*sin(radians))*(c->move_vec.Vx-c->move_vec.Vw*sin(radians))+
			(c->move_vec.Vy-c->move_vec.Vw*cos(radians))*(c->move_vec.Vy-c->move_vec.Vw*cos(radians)));
	
			//解算得到6020角度（-180°——+180°）
			c->hopemotorout.rotor6020_jiesuan_1[0]=atan2((c->move_vec.Vy+c->move_vec.Vw*cos(radians)),
																		(c->move_vec.Vx+c->move_vec.Vw*sin(radians)))* (180 / M_PI);
			c->hopemotorout.rotor6020_jiesuan_1[1]=atan2((c->move_vec.Vy+c->move_vec.Vw*cos(radians)),
																		(c->move_vec.Vx-c->move_vec.Vw*sin(radians)))* (180 / M_PI);
			c->hopemotorout.rotor6020_jiesuan_1[2]=atan2((c->move_vec.Vy-c->move_vec.Vw*cos(radians)),
																		(c->move_vec.Vx+c->move_vec.Vw*sin(radians)))* (180 / M_PI);
			c->hopemotorout.rotor6020_jiesuan_1[3]=atan2((c->move_vec.Vy-c->move_vec.Vw*cos(radians)),
																		(c->move_vec.Vx-c->move_vec.Vw*sin(radians)))* (180 / M_PI);		 
#elif defined(action_sick)
			 //码盘sick模式下操控为世界坐标系，能实现舵轮小陀螺前进
				 
			   c->chassis_yaw = pos->pos_yaw* (M_PI / 180.0f);
			   float cos_yaw = cosf(c->chassis_yaw);
         float sin_yaw = sinf(c->chassis_yaw);
			 
			   // 将速度从世界坐标系转换到底盘坐标系
         float Vx_local = c->move_vec.Vx * cos_yaw + c->move_vec.Vy * sin_yaw;
         float Vy_local = -c->move_vec.Vx * sin_yaw + c->move_vec.Vy * cos_yaw;
 
			   //解算得到5065速度，加-号是因为vesc校准时轮向电机默认转动正方向不同，可从上位机里改
	       c->hopemotorout.rotor5065_jiesuan_1[0]=-sqrt(
	       (Vx_local + c->move_vec.Vw * sin(radians)) * (Vx_local + c->move_vec.Vw * sin(radians)) +
         (Vy_local + c->move_vec.Vw * cos(radians)) * (Vy_local + c->move_vec.Vw * cos(radians)));
  
				 c->hopemotorout.rotor5065_jiesuan_1[1]=sqrt(
	       (Vx_local - c->move_vec.Vw * sin(radians)) * (Vx_local - c->move_vec.Vw * sin(radians)) +
         (Vy_local + c->move_vec.Vw * cos(radians)) * (Vy_local + c->move_vec.Vw * cos(radians)));
 
	       c->hopemotorout.rotor5065_jiesuan_1[2]=-sqrt(
	       (Vx_local + c->move_vec.Vw * sin(radians)) * (Vx_local + c->move_vec.Vw * sin(radians)) +
         (Vy_local - c->move_vec.Vw * cos(radians)) * (Vy_local - c->move_vec.Vw * cos(radians)));
 
         c->hopemotorout.rotor5065_jiesuan_1[3]=sqrt(
	       (Vx_local - c->move_vec.Vw * sin(radians)) * (Vx_local - c->move_vec.Vw * sin(radians)) +
         (Vy_local - c->move_vec.Vw * cos(radians)) * (Vy_local - c->move_vec.Vw * cos(radians)));
	
		     //解算得到6020角度（-180°——+180°）
         c->hopemotorout.rotor6020_jiesuan_1[0]= atan2((Vy_local + c->move_vec.Vw * cos(radians)),
																			 (Vx_local + c->move_vec.Vw * sin(radians))) * (180 / M_PI);
         c->hopemotorout.rotor6020_jiesuan_1[1]= atan2((Vy_local + c->move_vec.Vw * cos(radians)),
				                               (Vx_local - c->move_vec.Vw * sin(radians))) * (180 / M_PI);
         c->hopemotorout.rotor6020_jiesuan_1[2]= atan2((Vy_local - c->move_vec.Vw * cos(radians)),
				                               (Vx_local + c->move_vec.Vw * sin(radians))) * (180 / M_PI);
 	       c->hopemotorout.rotor6020_jiesuan_1[3]= atan2((Vy_local - c->move_vec.Vw * cos(radians)),
				                               (Vx_local - c->move_vec.Vw * sin(radians))) * (180 / M_PI);
#endif	
				break;
     }	
  }
		 //角度归化到0°——360°
		 for (uint8_t i = 0; i < 4; i++) {
			
			 if(c->hopemotorout.rotor6020_jiesuan_1[i]<0){
 				 c->hopemotorout.rotor6020_jiesuan_1[i]+=360;
			 }	
 			}
		
     //舵向电机就近转位
     float angle_error[4];//角度误差         
     for (uint8_t i = 0; i < 4; i++) {
       angle_error[i]=c->hopemotorout.rotor6020_jiesuan_1[i]-c->motorfeedback.rotor_angle6020[i];    
       //误差角度归化到-180°——+180°
       while (angle_error[i] > 180) angle_error[i] -= 360;
       while (angle_error[i] < -180) angle_error[i] += 360;
       /*这里发现如果下面的c->motorfeedback.rotor_angle6020[i]+angle_error[i]变为
			 c->hopemotorout.rotor6020_jiesuan_1[i]会导致6020出现故障*/
       if(angle_error[i]>90&&angle_error[i]<=180){
          c->hopemotorout.rotor5065_jiesuan_2[i]=-c->hopemotorout.rotor5065_jiesuan_1[i];
          c->hopemotorout.rotor6020_jiesuan_2[i]=c->motorfeedback.rotor_angle6020[i]+angle_error[i]-180;
        }
         
        else if(angle_error[i]<-90&&angle_error[i]>=-180){
          c->hopemotorout.rotor5065_jiesuan_2[i]=-c->hopemotorout.rotor5065_jiesuan_1[i];
          c->hopemotorout.rotor6020_jiesuan_2[i]=c->motorfeedback.rotor_angle6020[i]+angle_error[i]+180;
        }
         
        else{
          c->hopemotorout.rotor5065_jiesuan_2[i]=c->hopemotorout.rotor5065_jiesuan_1[i];
          c->hopemotorout.rotor6020_jiesuan_2[i]=c->motorfeedback.rotor_angle6020[i]+angle_error[i];
        }
     }	 
}

//根据陀螺仪反馈的数据来防止速度过快时侧翻
void Chassis_RolPrevent(Chassis_t *c){	
 /*这里pit轴rol轴根据c板安装方向*/
	
	//后侧腾空
  if(c->pos088.imu_eulr.pit > 0.1f)
		c->move_vec.Vx = 10000; //往前挪
	//前侧腾空
	else if(c->pos088.imu_eulr.pit < -0.1f)
		c->move_vec.Vx = -10000; //往后挪

	//右侧腾空
  if(c->pos088.imu_eulr.rol > 0.1f)
		c->move_vec.Vy = -10000;  //往左挪				
	//左侧腾空
	else if(c->pos088.imu_eulr.rol < -0.1f)
		c->move_vec.Vy = 10000; //往右挪

}

//nuc纠正角度（右旋偏移量为正，左旋偏移量为负）
void nuc_angle_correct(Chassis_t *c,CMD_t *ctrl){		
 
	//雷达数据必须滤波，否则波形是0和正常数组成的矩形
  ctrl->nuc.vx = LowPassFilter2p_Apply(&(c->filled[3]),ctrl->nuc.vx);
  ctrl->nuc.vy = LowPassFilter2p_Apply(&(c->filled[4]),ctrl->nuc.vy);
  ctrl->nuc.vw = LowPassFilter2p_Apply(&(c->filled[5]),ctrl->nuc.vw);	
	
	//双环pid
//	fp32 nuc_detangle;	
//	nuc_detangle = - PID_calc(&(c->pid.chassis_RadaranglePID),ctrl->nuc.vw,0);
//  c->radar_yaw = PID_calc(&(c->pid.chassis_RadarspeedPID),c->pos088.bmi088.gyro.z,nuc_detangle);	
	
	//两个单环pid
	if(fabsf(ctrl->nuc.vw)>8){
		 c->radar_yaw = - PID_calc(&(c->pid.chassis_RadaranglePID),ctrl->nuc.vw,0)*8;
	}
	else{
		 c->radar_yaw = - PID_calc(&(c->pid.chassis_RadarspeedPID),ctrl->nuc.vw,0)*20;	
	}
}

//雷达运用sick来校准
void radar_sick_calibration(Chassis_t *c){
   fp32 diff_x = c->sick[2] - c->sick_set[2];
   fp32 diff_y = c->sick[0] - c->sick_set[0];
   fp32 diff_w = (c->sick[0] - c->sick[1]); //放大偏角误差
	
   c->move_sick.Vx = (fabsf(diff_x)>SICKXY_ERROR) ? PID_calc(&(c->pid.chassis_SickVx),diff_x,0): 0;
   c->move_sick.Vy = (fabsf(diff_y)>SICKXY_ERROR) ? PID_calc(&(c->pid.chassis_SickVy),diff_y,0): 0;
   c->move_sick.Vw = (fabsf(diff_w)>SICKW_ERROR) ? PID_calc(&(c->pid.chassis_SickVw),diff_w*4,0): 0;
		
	//判断sick是否到达目标点附近
   static uint8_t reach_cnt = 0; 
   if (fabsf(diff_x) <= SICKXY_ERROR && fabsf(diff_y) <= SICKXY_ERROR &&fabsf(diff_w)<= SICKW_ERROR) {
     reach_cnt++;
     if (reach_cnt >= 50) {
       c->SICK_FALG = 1;
       reach_cnt = 0;				 
     } 
  }		 
    else { 
     c->SICK_FALG = 0; 		
   }
	
  //由于r1到达sick目标点附近可能会震荡，所以必须是锁死时到达目标点才给nuc发数	 
	if(c->SICK_FALG == 1 && c->mode ==STOP){
		c->SICK__SEND_FALG = 1;
	}	
	else { 
    c->SICK__SEND_FALG = 0; 		
   }
}

//底盘控制
int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out, Action_POS_t*pos){    
	
	Chassis_SetCtrl(c,ctrl);

	fp32 chassis6020_detangle[4];
	
#ifdef radar	
  //关于nuc纠正的函数都要一直运行 	
	nuc_angle_correct(c,ctrl);	
	radar_sick_calibration(c);
#endif  
	switch (c->mode){	
		case RC:
	   c->move_vec.Vx = ctrl->Vx*ctrl->throttle*9500;
     c->move_vec.Vy = ctrl->Vy*ctrl->throttle*9500;		
	   c->move_vec.Vw = ctrl->Vw*ctrl->throttle*6000;

	   break;	
		
	  case STOP:
		 c->move_vec.Vx =0;
		 c->move_vec.Vy =0;
		 c->move_vec.Vw =0;
		 Buzzer_Control(&c->buzzer_sick_calibration,(c->SICK_FALG == 1));	
     Buzzer_Control(&c->buzzer_radar_angle,(fabsf(ctrl->nuc.vw) < 1 && ctrl->nuc.vw != 0));					
		 break; 
		
	  case NAVI:					
#ifdef radar
		switch (ctrl->C_cmd.nuc_radar){
		  case ANGLE:
			 /*
			 跑点时只需要xy，不需要转角度，
			 而且如果跑点时有角度车会呈现蠕动的形态，
			 比赛的时候只需要纠正角度，不需要跑点，
			 只是为了方便拟合，而且跑点要准的话要加pid，
			 目前xy的参数还可以
	    */
			 //跑点
			 c->move_vec.Vx = ctrl->nuc.vx*2500;
       c->move_vec.Vy = ctrl->nuc.vy*2500;
			 //纠正角度
//	     c->move_vec.Vx = ctrl->Vx*95000;
//       c->move_vec.Vy = ctrl->Vy*95000;			 
	     c->move_vec.Vw = c->radar_yaw;
			 
       Buzzer_Control(&c->buzzer_radar_angle,(fabsf(ctrl->nuc.vw) < 1 && ctrl->nuc.vw != 0));							 	  
	    break;
			 
		  case FORBIDDEN:
	     c->move_vec.Vx = ctrl->Vx*ctrl->throttle*9500;
       c->move_vec.Vy = ctrl->Vy*ctrl->throttle*9500;
	     c->move_vec.Vw = ctrl->Vw*ctrl->throttle*6000;
	     break;		 
		
		  case RADAR_RESET:		
			 c->move_vec.Vx = c->move_sick.Vx;
			 c->move_vec.Vy = c->move_sick.Vy;
			 c->move_vec.Vw = c->move_sick.Vw;

       Buzzer_Control(&c->buzzer_sick_calibration,(c->SICK_FALG == 1));	
		  break;
	 }
		 
#elif defined(action_sick)
     c->move_vec.Vx =ctrl->C_navi.vx ;
     c->move_vec.Vy =ctrl->C_navi.vy ;    
	   c->move_vec.Vw =ctrl->C_navi.wz ;		 
#endif		   
		 
	   break;		 	 
 } 
	  
     //进行滤波	 
	   c->move_vec.Vx =LowPassFilter2p_Apply(&(c->filled[0]),c->move_vec.Vx);
		 c->move_vec.Vy =LowPassFilter2p_Apply(&(c->filled[1]),c->move_vec.Vy);
     c->move_vec.Vw =LowPassFilter2p_Apply(&(c->filled[2]),c->move_vec.Vw);

    //防翻
    Chassis_RolPrevent(c);  
	  
   Chassis_speed_calculate(c,pos); 	 
#ifdef calibration
#else 
  for (uint8_t i = 0 ; i <4 ; i++){
    
	  c->hopemotorout.motor6020_target[i]=c->hopemotorout.rotor6020_jiesuan_2[i];
	  chassis6020_detangle[i] = PID_calc(&(c->pid.chassis_6020anglePid[i]),c->motorfeedback.rotor_angle6020[i],
		                                                                 c->hopemotorout.motor6020_target[i]);
	  c->final_out.final_6020out[i] = PID_calc(&(c->pid.chassis_6020OmegaPid[i]),c->motorfeedback.rotor_rpm6020[i],
		                                                                                 chassis6020_detangle[i]);
	  out->chassis6020.as_array[i] = c->final_out.final_6020out[i];
		
	  c->hopemotorout.motor5065_target[i]=c->hopemotorout.rotor5065_jiesuan_2[i];
		c->final_out.final_5065out[i]=c->hopemotorout.motor5065_target[i];
		out->chassis5065.erpm[i] = c->final_out.final_5065out[i];       
	}	
#endif
   //vofa发送
//	 c->vofa_send[0] =c->sick[0];
//	 c->vofa_send[1] =c->sick[1];
	 c->vofa_send[2] =c->sick[2];
//	 c->vofa_send[3] =c->sick_set[0];
	 c->vofa_send[4] =c->sick_set[2];
//	 c->vofa_send[5] =;
//	 c->vofa_send[6] =;
//	 c->vofa_send[7] =;	
return CHASSIS_OK;
}