舵轮代码解读

2022-03-26

舵轮代码

目前代码给我印象是各个轮子分开控制，和1代舵轮一样都是无优弧劣弧处理，反转需要舵转动180°。3轮头舵轮做转向作用。无运动模型，就是代码判断赋值，在某些情况下能做到更好的处理，但感觉移植性没那么好，等我有空把他代码也做一个总结。

运行时操作手感不是很好，有种急停急开的感觉，还得优化。
贴上部分代码。

遥控器转比例值

无运动模型，将遥控器的数值转换成速度和角度的比例，再运算赋值给底盘。
z:没什么意义，如果有运动模型的话就不用后面那么多处理，还得想着多层逻辑的叠加。

/**
  * @brief  DR16接收的数据转化为角度和速度比
  * @retval None
  */
void D16DataCalculate(void)   
{
	float Max_Value = 660;
	if(dr16_data.rc.s_left != 0){                
		dr16_motion_value.Left = dr16_data.rc.s_left;
	}if(dr16_data.rc.s_right != 0){
		dr16_motion_value.Right = dr16_data.rc.s_right;
	}
	if(fabs((float)dr16_data.rc.ch2_LX) ==1024) 
		dr16_data.rc.ch2_LX = 0;
	if(fabs((float)dr16_data.rc.ch3_LY)== 1024)
		dr16_data.rc.ch3_LY = 0;
	float z = sqrt(powf(dr16_data.rc.ch2_LX,2) + powf(dr16_data.rc.ch3_LY,2)); 
	/*将速度值转化为百分比
	*/
	if(z>=660){
		dr16_motion_value.CSpeed = 1;
	}
	else{
		dr16_motion_value.CSpeed = z/Max_Value;
	}
	
	dr16_motion_value.LastAngle = dr16_motion_value.CAngle;
	// 遥控器转换成角度值
	if(dr16_data.rc.ch2_LX>0&&dr16_data.rc.ch3_LY>=0){
		dr16_motion_value.CAngle = acos(fabsf((float)dr16_data.rc.ch2_LX)/z)*180.0/3.1416; 
	}else if(dr16_data.rc.ch2_LX<=0&&dr16_data.rc.ch3_LY>0){
		dr16_motion_value.CAngle = acos(fabsf((float)dr16_data.rc.ch3_LY)/z)*180.0/3.1416 + 90.0;
	}else if(dr16_data.rc.ch2_LX<0&&dr16_data.rc.ch3_LY<=0){
		dr16_motion_value.CAngle = acos(fabsf((float)dr16_data.rc.ch2_LX)/z)*180.0/3.1416 + 180.0;
	}else if(dr16_data.rc.ch2_LX>=0&&dr16_data.rc.ch3_LY<0){
		dr16_motion_value.CAngle = acos(fabsf((float)dr16_data.rc.ch3_LY)/z)*180.0/3.1416 + 270.0;
	}
	/*多圈旋转判断
	*/
 	dr16_motion_value.AngleVariationV = dr16_motion_value.LastAngle - dr16_motion_value.CAngle;
	if(dr16_motion_value.AngleVariationV <= -270){  
		RondNumber -= 1;
	}else if(dr16_motion_value.AngleVariationV >= 270){
		RondNumber += 1;
	}
//	else dr16_motion_value.CAngle = 0;
	
  /*摇杆的旋转值转化为百分比
	*/
		dr16_motion_value.CRotate = dr16_data.rc.ch0_RX/Max_Value;
	
//	if(dr16_data.rc.ch3_LY<0) dr16_motion_value.CSpeed *= -1.0;
}

自旋量

自旋是自旋前进是前进，无角度叠加。
控制的想法跟我完全不一样，前面提的速度运动一定跑不直，要改成全场定位控制也没有。
全场定位的用处就只是用来控制自动。

if(fabs(dr16_motion_value.CRotate) > 0.2 && fabs(dr16_motion_value.CSpeed)<0.2){     //当只有右摇杆控制时可进行原地旋转
				RPM = MaxRPM*(fabs(dr16_motion_value.CRotate)-0.2); 	//旋转赋值
				if(dr16_motion_value.CRotate>0){      //右摇杆控向右打时
					SendSWAngle(0,135,45);
					SW_RPM_Move(RPM,RPM,RPM * -1.0);
				}else if(dr16_motion_value.CRotate<0){    //右摇杆控向左打时
					SendSWAngle(0,135,45);
					SW_RPM_Move(RPM * -1.0,RPM * -1.0,RPM);
				}
				RPM = 0;

运动模型

由多个if判断组成的运动模型，判断是否相应的模型

else if(fabs(dr16_motion_value.CSpeed) > 0.2){     //左摇杆对底盘方向的控制
				if(dr16_motion_value.AngleVariationV>90 && dr16_motion_value.AngleVariationV<180){
					if(AngleAdd == 180){
						AngleAdd = 0;
					}else AngleAdd = 180;
					SpeedOrienta = SpeedOrienta * -1;
				}else if(dr16_motion_value.AngleVariationV < -90&&dr16_motion_value.AngleVariationV > -180)
				{
					if(AngleAdd == -180){
						AngleAdd = 0;
					}else AngleAdd = -180;
					SpeedOrienta = SpeedOrienta * -1;
				}
				//前轮微小  就是把自旋改成前轮转向，后面两个轮子做差速的三轮车模型
				if(dr16_motion_value.CAngle>60 && dr16_motion_value.CAngle < 120){     //当在跑直线时可用右摇杆对前轮角度进行微小
					SpeedAdd = (float)0.2 * MaxRPM * dr16_motion_value.CRotate;   //调整时后面两轮做速度差
					SendSWAngle(dr16_motion_value.CAngle - (AdjustAngle*dr16_motion_value.CRotate),dr16_motion_value.CAngle,dr16_motion_value.CAngle);
				}else if(dr16_motion_value.CAngle>240 && dr16_motion_value.CAngle < 300){     //当在跑直线时可用右摇杆对前轮角度进行微小
					SpeedAdd = (float)0.2 * MaxRPM * dr16_motion_value.CRotate * (float)-1.0;   //调整时后面两轮做速度差
					SendSWAngle(dr16_motion_value.CAngle - (AdjustAngle*dr16_motion_value.CRotate),dr16_motion_value.CAngle,dr16_motion_value.CAngle);
				}else { 	
					SendSWAngle(dr16_motion_value.CAngle+AngleAdd,dr16_motion_value.CAngle+AngleAdd,dr16_motion_value.CAngle+AngleAdd);
				}
				
				if(dr16_motion_value.CSpeed > 0){
					RPM = MaxRPM*(dr16_motion_value.CSpeed -(float)0.2);
				}else if(dr16_motion_value.CSpeed<0){
					RPM = MaxRPM*(dr16_motion_value.CSpeed + (float)0.2);
				}


				SW_RPM_Move(RPM*SpeedOrienta ,(RPM + SpeedAdd)*SpeedOrienta ,(RPM + (SpeedAdd * -1))*SpeedOrienta);
				RPM = 0;
				SpeedAdd = 0;
			}else{
				SW_RPM_Move(0, 0, 0);
			}