cyy/origincar_controller
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

init

Browse Source
This commit is contained in:
MobKBK
2026-06-20 21:22:34 +08:00
commit 460e0e9e73
320 changed files with 135203 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

750
BALANCE/balance.c Normal file
View File

@@ -0,0 +1,750 @@
#include "balance.h"
int Time_count=0; //Time variable //计时变量
// Robot mode is wrong to detect flag bits
//机器人模式是否出错检测标志位
int robot_mode_check_flag=0;
short test_num;
Encoder OriginalEncoder; //Encoder raw data //编码器原始数据
u8 command_lost_count=0; //串口、CAN控制命令丢失时间计数，丢失1秒后停止控制
/**************************************************************************
Function: The inverse kinematics solution is used to calculate the target speed of each wheel according to the target speed of three axes
Input : X and Y, Z axis direction of the target movement speed
Output : none
函数功能：运动学逆解，根据三轴目标速度计算各车轮目标转速
入口参数：X和Y、Z轴方向的目标运动速度
返回 值：无
**************************************************************************/
void Drive_Motor(float Vx,float Vy,float Vz)
{
float amplitude=3.5; //Wheel target speed limit //车轮目标速度限幅
//Speed smoothing is enabled when moving the omnidirectional trolley
//全向移动小车才开启速度平滑处理
if(Car_Mode==Mec_Car||Car_Mode==Omni_Car)
{
Smooth_control(Vx,Vy,Vz); //Smoothing the input speed //对输入速度进行平滑处理
//Get the smoothed data
//获取平滑处理后的数据
Vx=smooth_control.VX;
Vy=smooth_control.VY;
Vz=smooth_control.VZ;
}
//Mecanum wheel car
//麦克纳姆轮小车
if (Car_Mode==Mec_Car)
{
//Inverse kinematics //运动学逆解
MOTOR_A.Target = +Vy+Vx-Vz*(Axle_spacing+Wheel_spacing);
MOTOR_B.Target = -Vy+Vx-Vz*(Axle_spacing+Wheel_spacing);
MOTOR_C.Target = +Vy+Vx+Vz*(Axle_spacing+Wheel_spacing);
MOTOR_D.Target = -Vy+Vx+Vz*(Axle_spacing+Wheel_spacing);
//Wheel (motor) target speed limit //车轮(电机)目标速度限幅
MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude);
MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude);
MOTOR_C.Target=target_limit_float(MOTOR_C.Target,-amplitude,amplitude);
MOTOR_D.Target=target_limit_float(MOTOR_D.Target,-amplitude,amplitude);
}
//Omni car
//全向轮小车
else if (Car_Mode==Omni_Car)
{
//Inverse kinematics //运动学逆解
MOTOR_A.Target = Vy + Omni_turn_radiaus*Vz;
MOTOR_B.Target = -X_PARAMETER*Vx - Y_PARAMETER*Vy + Omni_turn_radiaus*Vz;
MOTOR_C.Target = +X_PARAMETER*Vx - Y_PARAMETER*Vy + Omni_turn_radiaus*Vz;
//Wheel (motor) target speed limit //车轮(电机)目标速度限幅
MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude);
MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude);
MOTOR_C.Target=target_limit_float(MOTOR_C.Target,-amplitude,amplitude);
MOTOR_D.Target=0; //Out of use //没有使用到
}
//Ackermann structure car
//阿克曼小车
else if (Car_Mode==Akm_Car)
{
//Ackerman car specific related variables //阿克曼小车专用相关变量
float R, Ratio=636.56, AngleR, Angle_Servo;
// For Ackerman small car, Vz represents the front wheel steering Angle
//对于阿克曼小车Vz代表右前轮转向角度
AngleR=Vz;
R=Axle_spacing/tan(AngleR)-0.5f*Wheel_spacing;
//R=Axle_spacing/tan(AngleR);
// Front wheel steering Angle limit (front wheel steering Angle controlled by steering engine), unit: rad
//前轮转向角度限幅(舵机控制前轮转向角度)，单位：rad
AngleR=target_limit_float(AngleR,-0.6f,0.6f);
//Inverse kinematics //运动学逆解
if(AngleR!=0)
{
MOTOR_A.Target = Vx*(R-0.5f*Wheel_spacing)/R;
MOTOR_B.Target = Vx*(R+0.5f*Wheel_spacing)/R;
}
else
{
MOTOR_A.Target = Vx;
MOTOR_B.Target = Vx;
}
// The PWM value of the servo controls the steering Angle of the front wheel
//舵机PWM值，舵机控制前轮转向角度
Angle_Servo = -0.628f*pow(AngleR, 3) + 1.269f*pow(AngleR, 2) - 1.772f*AngleR + 1.573f;
Servo=SERVO_INIT + (Angle_Servo - 1.572f)*Ratio;
// Servo=SERVO_INIT + (Angle_Servo)*Ratio;
//Wheel (motor) target speed limit //车轮(电机)目标速度限幅
MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude);
MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude);
MOTOR_C.Target=0; //Out of use //没有使用到
MOTOR_D.Target=0; //Out of use //没有使用到
Servo=target_limit_int(Servo,800,2200); //Servo PWM value limit //舵机PWM值限幅
}
//Differential car
//差速小车
else if (Car_Mode==Diff_Car)
{
//Inverse kinematics //运动学逆解
MOTOR_A.Target = Vx - Vz * Wheel_spacing / 2.0f; //计算出左轮的目标速度
MOTOR_B.Target = Vx + Vz * Wheel_spacing / 2.0f; //计算出右轮的目标速度
//Wheel (motor) target speed limit //车轮(电机)目标速度限幅
MOTOR_A.Target=target_limit_float( MOTOR_A.Target,-amplitude,amplitude);
MOTOR_B.Target=target_limit_float( MOTOR_B.Target,-amplitude,amplitude);
MOTOR_C.Target=0; //Out of use //没有使用到
MOTOR_D.Target=0; //Out of use //没有使用到
}
//FourWheel car
//四驱车
else if(Car_Mode==FourWheel_Car)
{
//Inverse kinematics //运动学逆解
MOTOR_A.Target = Vx - Vz * (Wheel_spacing + Axle_spacing) / 2.0f; //计算出左轮的目标速度
MOTOR_B.Target = Vx - Vz * (Wheel_spacing + Axle_spacing) / 2.0f; //计算出左轮的目标速度
MOTOR_C.Target = Vx + Vz * (Wheel_spacing + Axle_spacing) / 2.0f; //计算出右轮的目标速度
MOTOR_D.Target = Vx + Vz * (Wheel_spacing + Axle_spacing) / 2.0f; //计算出右轮的目标速度
//Wheel (motor) target speed limit //车轮(电机)目标速度限幅
MOTOR_A.Target=target_limit_float( MOTOR_A.Target,-amplitude,amplitude);
MOTOR_B.Target=target_limit_float( MOTOR_B.Target,-amplitude,amplitude);
MOTOR_C.Target=target_limit_float( MOTOR_C.Target,-amplitude,amplitude);
MOTOR_D.Target=target_limit_float( MOTOR_D.Target,-amplitude,amplitude);
}
//Tank Car
//履带车
else if (Car_Mode==Tank_Car)
{
//Inverse kinematics //运动学逆解
MOTOR_A.Target = Vx - Vz * (Wheel_spacing) / 2.0f; //计算出左轮的目标速度
MOTOR_B.Target = Vx + Vz * (Wheel_spacing) / 2.0f; //计算出右轮的目标速度
//Wheel (motor) target speed limit //车轮(电机)目标速度限幅
MOTOR_A.Target=target_limit_float( MOTOR_A.Target,-amplitude,amplitude);
MOTOR_B.Target=target_limit_float( MOTOR_B.Target,-amplitude,amplitude);
MOTOR_C.Target=0; //Out of use //没有使用到
MOTOR_D.Target=0; //Out of use //没有使用到
}
}
/**************************************************************************
Function: FreerTOS task, core motion control task
Input : none
Output : none
函数功能：FreeRTOS任务，核心运动控制任务
入口参数：无
返回 值：无
**************************************************************************/
void Balance_task(void *pvParameters)
{
u32 lastWakeTime = getSysTickCnt();
while(1)
{
// This task runs at a frequency of 100Hz (10ms control once)
//此任务以100Hz的频率运行（10ms控制一次）
vTaskDelayUntil(&lastWakeTime, F2T(RATE_100_HZ));
//Time count is no longer needed after 30 seconds
//时间计数，30秒后不再需要
if(Time_count<3000)Time_count++;
//Get the encoder data, that is, the real time wheel speed,
//and convert to transposition international units
//获取编码器数据，即车轮实时速度，并转换位国际单位
Get_Velocity_Form_Encoder();
if(Check==0) //If self-check mode is not enabled //如果没有启动自检模式
{
// command_lost_count++; //串口、CAN控制命令丢失时间计数，丢失1秒后停止控制
// if(command_lost_count>RATE_100_HZ && APP_ON_Flag==0 && Remote_ON_Flag==0 && PS2_ON_Flag==0) //不是APP、PS2、航模遥控模式，就是CAN、串口1、串口3控制模式
// Move_X=0, Move_Y=0, Move_Z=0;
if (APP_ON_Flag) Get_RC(); //Handle the APP remote commands //处理APP遥控命令
else if (Remote_ON_Flag) Remote_Control(); //Handle model aircraft remote commands //处理航模遥控命令
else if (PS2_ON_Flag) PS2_control(); //Handle PS2 controller commands //处理PS2手柄控制命令
//CAN, Usart 1, Usart 3, Uart5 control can directly get the three axis target speed,
//without additional processing
//CAN、串口1、串口3(ROS)、串口5控制直接得到三轴目标速度，无须额外处理
else Drive_Motor(Move_X, Move_Y, Move_Z);
//Click the user button to update the gyroscope zero
//单击用户按键更新陀螺仪零点
Key();
//If there is no abnormity in the battery voltage, and the enable switch is in the ON position,
//and the software failure flag is 0
//如果电池电压不存在异常，而且使能开关在ON档位，而且软件失能标志位为0
if(Turn_Off(Voltage)==0)
{
//Speed closed-loop control to calculate the PWM value of each motor,
//PWM represents the actual wheel speed
//速度闭环控制计算各电机PWM值，PWM代表车轮实际转速
MOTOR_A.Motor_Pwm=Incremental_PI_A(MOTOR_A.Encoder, MOTOR_A.Target);
MOTOR_B.Motor_Pwm=Incremental_PI_B(MOTOR_B.Encoder, MOTOR_B.Target);
MOTOR_C.Motor_Pwm=Incremental_PI_C(MOTOR_C.Encoder, MOTOR_C.Target);
MOTOR_D.Motor_Pwm=Incremental_PI_D(MOTOR_D.Encoder, MOTOR_D.Target);
Limit_Pwm(16700);
//Set different PWM control polarity according to different car models
//根据不同小车型号设置不同的PWM控制极性
switch(Car_Mode)
{
case Mec_Car: Set_Pwm( MOTOR_A.Motor_Pwm, -MOTOR_B.Motor_Pwm, -MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0 ); break; //Mecanum wheel car //麦克纳姆轮小车
case Omni_Car: Set_Pwm(-MOTOR_A.Motor_Pwm, MOTOR_B.Motor_Pwm, -MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0 ); break; //Omni car //全向轮小车
case Akm_Car: Set_Pwm( MOTOR_A.Motor_Pwm, MOTOR_B.Motor_Pwm, 16799,-16799 , Servo); break; //Ackermann structure car //阿克曼小车
case Diff_Car: Set_Pwm( MOTOR_A.Motor_Pwm, MOTOR_B.Motor_Pwm, MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0 ); break; //Differential car //两轮差速小车
case FourWheel_Car: Set_Pwm( MOTOR_A.Motor_Pwm, -MOTOR_B.Motor_Pwm, -MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0 ); break; //FourWheel car //四驱车
case Tank_Car: Set_Pwm( MOTOR_A.Motor_Pwm, MOTOR_B.Motor_Pwm, MOTOR_C.Motor_Pwm, MOTOR_D.Motor_Pwm, 0 ); break; //Tank Car //履带车
}
}
//If Turn_Off(Voltage) returns to 1, the car is not allowed to move, and the PWM value is set to 0
//如果Turn_Off(Voltage)返回值为1，不允许控制小车进行运动，PWM值设置为0
else Set_Pwm(0,0,0,0,0);
}
}
}
/**************************************************************************
Function: Assign a value to the PWM register to control wheel speed and direction
Input : PWM
Output : none
函数功能：赋值给PWM寄存器，控制车轮转速与方向
入口参数：PWM
返回 值：无
**************************************************************************/
void Set_Pwm(int motor_a,int motor_b,int motor_c,int motor_d,int servo)
{
//Forward and reverse control of motor
//电机正反转控制
if(motor_a<0) PWMA1=16799,PWMA2=16799+motor_a;
else PWMA2=16799,PWMA1=16799-motor_a;
//Forward and reverse control of motor
//电机正反转控制
if(motor_b<0) PWMB1=16799,PWMB2=16799+motor_b;
else PWMB2=16799,PWMB1=16799-motor_b;
// PWMB1=10000,PWMB2=5000;
//Forward and reverse control of motor
//电机正反转控制
if(motor_c<0) PWMC1=16799,PWMC2=16799+motor_c;
else PWMC2=16799,PWMC1=16799-motor_c;
//Forward and reverse control of motor
//电机正反转控制
if(motor_d<0) PWMD1=16799,PWMD2=16799+motor_d;
else PWMD2=16799,PWMD1=16799-motor_d;
//Servo control
//舵机控制
Servo_PWM =servo;
}
/**************************************************************************
Function: Limit PWM value
Input : Value
Output : none
函数功能：限制PWM值
入口参数：幅值
返回 值：无
**************************************************************************/
void Limit_Pwm(int amplitude)
{
MOTOR_A.Motor_Pwm=target_limit_float(MOTOR_A.Motor_Pwm,-amplitude,amplitude);
MOTOR_B.Motor_Pwm=target_limit_float(MOTOR_B.Motor_Pwm,-amplitude,amplitude);
MOTOR_C.Motor_Pwm=target_limit_float(MOTOR_C.Motor_Pwm,-amplitude,amplitude);
MOTOR_D.Motor_Pwm=target_limit_float(MOTOR_D.Motor_Pwm,-amplitude,amplitude);
}
/**************************************************************************
Function: Limiting function
Input : Value
Output : none
函数功能：限幅函数
入口参数：幅值
返回 值：无
**************************************************************************/
float target_limit_float(float insert,float low,float high)
{
if (insert < low)
return low;
else if (insert > high)
return high;
else
return insert;
}
int target_limit_int(int insert,int low,int high)
{
if (insert < low)
return low;
else if (insert > high)
return high;
else
return insert;
}
/**************************************************************************
Function: Check the battery voltage, enable switch status, software failure flag status
Input : Voltage
Output : Whether control is allowed, 1: not allowed, 0 allowed
函数功能：检查电池电压、使能开关状态、软件失能标志位状态
入口参数：电压
返回 值：是否允许控制，1：不允许，0允许
**************************************************************************/
u8 Turn_Off( int voltage)
{
u8 temp;
if(voltage<10||EN==0||Flag_Stop==1)
{
temp=1;
PWMA1=0;PWMA2=0;
PWMB1=0;PWMB2=0;
PWMC1=0;PWMC1=0;
PWMD1=0;PWMD2=0;
}
else
temp=0;
return temp;
}
/**************************************************************************
Function: Calculate absolute value
Input : long int
Output : unsigned int
函数功能：求绝对值
入口参数：long int
返回 值：unsigned int
**************************************************************************/
u32 myabs(long int a)
{
u32 temp;
if(a<0) temp=-a;
else temp=a;
return temp;
}
/**************************************************************************
Function: Incremental PI controller
Input : Encoder measured value (actual speed), target speed
Output : Motor PWM
According to the incremental discrete PID formula
pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k) represents the current deviation
e(k-1) is the last deviation and so on
PWM stands for incremental output
In our speed control closed loop system, only PI control is used
pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)
函数功能：增量式PI控制器
入口参数：编码器测量值(实际速度)，目标速度
返回 值：电机PWM
根据增量式离散PID公式
pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k)代表本次偏差
e(k-1)代表上一次的偏差 以此类推
pwm代表增量输出
在我们的速度控制闭环系统里面，只使用PI控制
pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)
**************************************************************************/
int Incremental_PI_A (float Encoder,float Target)
{
static float Bias,Pwm,Last_bias;
Bias=Target-Encoder; //Calculate the deviation //计算偏差
Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;
if(Pwm>16700)Pwm=16700;
if(Pwm<-16700)Pwm=-16700;
Last_bias=Bias; //Save the last deviation //保存上一次偏差
return Pwm;
}
int Incremental_PI_B (float Encoder,float Target)
{
static float Bias,Pwm,Last_bias;
Bias=Target-Encoder; //Calculate the deviation //计算偏差
Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;
if(Pwm>16700)Pwm=16700;
if(Pwm<-16700)Pwm=-16700;
Last_bias=Bias; //Save the last deviation //保存上一次偏差
return Pwm;
}
int Incremental_PI_C (float Encoder,float Target)
{
static float Bias,Pwm,Last_bias;
Bias=Target-Encoder; //Calculate the deviation //计算偏差
Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;
if(Pwm>16700)Pwm=16700;
if(Pwm<-16700)Pwm=-16700;
Last_bias=Bias; //Save the last deviation //保存上一次偏差
return Pwm;
}
int Incremental_PI_D (float Encoder,float Target)
{
static float Bias,Pwm,Last_bias;
Bias=Target-Encoder; //Calculate the deviation //计算偏差
Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;
if(Pwm>16700)Pwm=16700;
if(Pwm<-16700)Pwm=-16700;
Last_bias=Bias; //Save the last deviation //保存上一次偏差
return Pwm;
}
/**************************************************************************
Function: Processes the command sent by APP through usart 2
Input : none
Output : none
函数功能：对APP通过串口2发送过来的命令进行处理
入口参数：无
返回 值：无
**************************************************************************/
void Get_RC(void)
{
u8 Flag_Move=1;
if(Car_Mode==Mec_Car||Car_Mode==Omni_Car) //The omnidirectional wheel moving trolley can move laterally //全向轮运动小车可以进行横向移动
{
switch(Flag_Direction) //Handle direction control commands //处理方向控制命令
{
case 1: Move_X=RC_Velocity; Move_Y=0; Flag_Move=1; break;
case 2: Move_X=RC_Velocity; Move_Y=-RC_Velocity; Flag_Move=1; break;
case 3: Move_X=0; Move_Y=-RC_Velocity; Flag_Move=1; break;
case 4: Move_X=-RC_Velocity; Move_Y=-RC_Velocity; Flag_Move=1; break;
case 5: Move_X=-RC_Velocity; Move_Y=0; Flag_Move=1; break;
case 6: Move_X=-RC_Velocity; Move_Y=RC_Velocity; Flag_Move=1; break;
case 7: Move_X=0; Move_Y=RC_Velocity; Flag_Move=1; break;
case 8: Move_X=RC_Velocity; Move_Y=RC_Velocity; Flag_Move=1; break;
default: Move_X=0; Move_Y=0; Flag_Move=0; break;
}
if(Flag_Move==0)
{
//If no direction control instruction is available, check the steering control status
//如果无方向控制指令，检查转向控制状态
if (Flag_Left ==1) Move_Z= PI/2*(RC_Velocity/500); //left rotation //左自转
else if(Flag_Right==1) Move_Z=-PI/2*(RC_Velocity/500); //right rotation //右自转
else Move_Z=0; //stop //停止
}
}
else //Non-omnidirectional moving trolley //非全向移动小车
{
switch(Flag_Direction) //Handle direction control commands //处理方向控制命令
{
case 1: Move_X=+RC_Velocity; Move_Z=0; break;
case 2: Move_X=+RC_Velocity; Move_Z=-PI/2; break;
case 3: Move_X=0; Move_Z=-PI/2; break;
case 4: Move_X=-RC_Velocity; Move_Z=-PI/2; break;
case 5: Move_X=-RC_Velocity; Move_Z=0; break;
case 6: Move_X=-RC_Velocity; Move_Z=+PI/2; break;
case 7: Move_X=0; Move_Z=+PI/2; break;
case 8: Move_X=+RC_Velocity; Move_Z=+PI/2; break;
default: Move_X=0; Move_Z=0; break;
}
if (Flag_Left ==1) Move_Z= PI/2; //left rotation //左自转
else if(Flag_Right==1) Move_Z=-PI/2; //right rotation //右自转
}
//Z-axis data conversion //Z轴数据转化
if(Car_Mode==Akm_Car)
{
//Ackermann structure car is converted to the front wheel steering Angle system target value, and kinematics analysis is pearformed
//阿克曼结构小车转换为前轮转向角度
Move_Z=Move_Z*2/9;
}
else if(Car_Mode==Diff_Car||Car_Mode==Tank_Car||Car_Mode==FourWheel_Car)
{
if(Move_X<0) Move_Z=-Move_Z; //The differential control principle series requires this treatment //差速控制原理系列需要此处理
Move_Z=Move_Z*RC_Velocity/500;
}
//Unit conversion, mm/s -> m/s
//单位转换，mm/s -> m/s
Move_X=Move_X/1000; Move_Y=Move_Y/1000; Move_Z=Move_Z;
//Control target value is obtained and kinematics analysis is performed
//得到控制目标值，进行运动学分析
Drive_Motor(Move_X,Move_Y,Move_Z);
}
/**************************************************************************
Function: Handle PS2 controller control commands
Input : none
Output : none
函数功能：对PS2手柄控制命令进行处理
入口参数：无
返回 值：无
**************************************************************************/
void PS2_control(void)
{
int LX,LY,RY;
int Threshold=20; //Threshold to ignore small movements of the joystick //阈值，忽略摇杆小幅度动作
//128 is the median.The definition of X and Y in the PS2 coordinate system is different from that in the ROS coordinate system
//128为中值。PS2坐标系与ROS坐标系对X、Y的定义不一样
LY=-(PS2_LX-128);
LX=-(PS2_LY-128);
RY=-(PS2_RX-128);
//Ignore small movements of the joystick //忽略摇杆小幅度动作
if(LX>-Threshold&&LX<Threshold)LX=0;
if(LY>-Threshold&&LY<Threshold)LY=0;
if(RY>-Threshold&&RY<Threshold)RY=0;
if (PS2_KEY==11) RC_Velocity+=5; //To accelerate//加速
else if(PS2_KEY==9) RC_Velocity-=5; //To slow down //减速
if(RC_Velocity<0) RC_Velocity=0;
//Handle PS2 controller control commands
//对PS2手柄控制命令进行处理
Move_X=LX*RC_Velocity/128;
Move_Y=LY*RC_Velocity/128;
Move_Z=RY*(PI/2)/128;
//Z-axis data conversion //Z轴数据转化
if(Car_Mode==Mec_Car||Car_Mode==Omni_Car)
{
Move_Z=Move_Z*RC_Velocity/500;
}
else if(Car_Mode==Akm_Car)
{
//Ackermann structure car is converted to the front wheel steering Angle system target value, and kinematics analysis is pearformed
//阿克曼结构小车转换为前轮转向角度
Move_Z=Move_Z*2/9;
}
else if(Car_Mode==Diff_Car||Car_Mode==Tank_Car||Car_Mode==FourWheel_Car)
{
if(Move_X<0) Move_Z=-Move_Z; //The differential control principle series requires this treatment //差速控制原理系列需要此处理
Move_Z=Move_Z*RC_Velocity/500;
}
//Unit conversion, mm/s -> m/s
//单位转换，mm/s -> m/s
Move_X=Move_X/1000;
Move_Y=Move_Y/1000;
Move_Z=Move_Z;
//Control target value is obtained and kinematics analysis is performed
//得到控制目标值，进行运动学分析
Drive_Motor(Move_X,Move_Y,Move_Z);
}
/**************************************************************************
Function: The remote control command of model aircraft is processed
Input : none
Output : none
函数功能：对航模遥控控制命令进行处理
入口参数：无
返回 值：无
**************************************************************************/
void Remote_Control(void)
{
//Data within 1 second after entering the model control mode will not be processed
//对进入航模控制模式后1秒内的数据不处理
static u8 thrice=100;
int Threshold=100; //Threshold to ignore small movements of the joystick //阈值，忽略摇杆小幅度动作
//limiter //限幅
int LX,LY,RY,RX,Remote_RCvelocity;
Remoter_Ch1=target_limit_int(Remoter_Ch1,1000,2000);
Remoter_Ch2=target_limit_int(Remoter_Ch2,1000,2000);
Remoter_Ch3=target_limit_int(Remoter_Ch3,1000,2000);
Remoter_Ch4=target_limit_int(Remoter_Ch4,1000,2000);
// Front and back direction of left rocker. Control forward and backward.
//左摇杆前后方向。控制前进后退。
LX=Remoter_Ch2-1500;
//Left joystick left and right.Control left and right movement. Only the wheelie omnidirectional wheelie will use the channel.
//Ackerman trolleys use this channel as a PWM output to control the steering gear
//左摇杆左右方向。控制左右移动。麦轮全向轮才会使用到改通道。阿克曼小车使用该通道作为PWM输出控制舵机
LY=Remoter_Ch4-1500;
//Front and back direction of right rocker. Throttle/acceleration/deceleration.
//右摇杆前后方向。油门/加减速。
RX=Remoter_Ch3-1500;
//Right stick left and right. To control the rotation.
//右摇杆左右方向。控制自转。
RY=Remoter_Ch1-1500;
if(LX>-Threshold&&LX<Threshold)LX=0;
if(LY>-Threshold&&LY<Threshold)LY=0;
if(RX>-Threshold&&RX<Threshold)RX=0;
if(RY>-Threshold&&RY<Threshold)RY=0;
//Throttle related //油门相关
Remote_RCvelocity=RC_Velocity+RX;
if(Remote_RCvelocity<0)Remote_RCvelocity=0;
//The remote control command of model aircraft is processed
//对航模遥控控制命令进行处理
Move_X= LX*Remote_RCvelocity/500;
Move_Y=-LY*Remote_RCvelocity/500;
Move_Z=-RY*(PI/2)/500;
//Z轴数据转化
if(Car_Mode==Mec_Car||Car_Mode==Omni_Car)
{
Move_Z=Move_Z*Remote_RCvelocity/500;
}
else if(Car_Mode==Akm_Car)
{
//Ackermann structure car is converted to the front wheel steering Angle system target value, and kinematics analysis is pearformed
//阿克曼结构小车转换为前轮转向角度
Move_Z=Move_Z*2/9;
}
else if(Car_Mode==Diff_Car||Car_Mode==Tank_Car||Car_Mode==FourWheel_Car)
{
if(Move_X<0) Move_Z=-Move_Z; //The differential control principle series requires this treatment //差速控制原理系列需要此处理
Move_Z=Move_Z*Remote_RCvelocity/500;
}
//Unit conversion, mm/s -> m/s
//单位转换，mm/s -> m/s
Move_X=Move_X/1000;
Move_Y=Move_Y/1000;
Move_Z=Move_Z;
//Data within 1 second after entering the model control mode will not be processed
//对进入航模控制模式后1秒内的数据不处理
if(thrice>0) Move_X=0,Move_Z=0,thrice--;
//Control target value is obtained and kinematics analysis is performed
//得到控制目标值，进行运动学分析
Drive_Motor(Move_X,Move_Y,Move_Z);
}
/**************************************************************************
Function: Click the user button to update gyroscope zero
Input : none
Output : none
函数功能：单击用户按键更新陀螺仪零点
入口参数：无
返回 值：无
**************************************************************************/
void Key(void)
{
u8 tmp;
tmp=click_N_Double_MPU6050(50);
if(tmp==2)memcpy(Deviation_gyro,Original_gyro,sizeof(gyro)),memcpy(Deviation_accel,Original_accel,sizeof(accel));
}
/**************************************************************************
Function: Read the encoder value and calculate the wheel speed, unit m/s
Input : none
Output : none
函数功能：读取编码器数值并计算车轮速度，单位m/s
入口参数：无
返回 值：无
**************************************************************************/
void Get_Velocity_Form_Encoder(void)
{
//Retrieves the original data of the encoder
//获取编码器的原始数据
float Encoder_A_pr,Encoder_B_pr,Encoder_C_pr,Encoder_D_pr;
OriginalEncoder.A=Read_Encoder(2);
OriginalEncoder.B=Read_Encoder(3);
OriginalEncoder.C=Read_Encoder(4);
OriginalEncoder.D=Read_Encoder(5);
//test_num=OriginalEncoder.B;
//Decide the encoder numerical polarity according to different car models
//根据不同小车型号决定编码器数值极性
switch(Car_Mode)
{
case Mec_Car: Encoder_A_pr= OriginalEncoder.A; Encoder_B_pr= OriginalEncoder.B; Encoder_C_pr=-OriginalEncoder.C; Encoder_D_pr=-OriginalEncoder.D; break;
case Omni_Car: Encoder_A_pr=-OriginalEncoder.A; Encoder_B_pr=-OriginalEncoder.B; Encoder_C_pr=-OriginalEncoder.C; Encoder_D_pr=-OriginalEncoder.D; break;
case Akm_Car: Encoder_A_pr= OriginalEncoder.A; Encoder_B_pr=-OriginalEncoder.B; Encoder_C_pr= OriginalEncoder.C; Encoder_D_pr= OriginalEncoder.D; break;
case Diff_Car: Encoder_A_pr= OriginalEncoder.A; Encoder_B_pr=-OriginalEncoder.B; Encoder_C_pr= OriginalEncoder.C; Encoder_D_pr= OriginalEncoder.D; break;
case FourWheel_Car: Encoder_A_pr= OriginalEncoder.A; Encoder_B_pr= OriginalEncoder.B; Encoder_C_pr=-OriginalEncoder.C; Encoder_D_pr=-OriginalEncoder.D; break;
case Tank_Car: Encoder_A_pr= OriginalEncoder.A; Encoder_B_pr=-OriginalEncoder.B; Encoder_C_pr= OriginalEncoder.C; Encoder_D_pr= OriginalEncoder.D; break;
}
//The encoder converts the raw data to wheel speed in m/s
//编码器原始数据转换为车轮速度，单位m/s
MOTOR_A.Encoder= Encoder_A_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;
MOTOR_B.Encoder= Encoder_B_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;
MOTOR_C.Encoder= Encoder_C_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;
MOTOR_D.Encoder= Encoder_D_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;
}
/**************************************************************************
Function: Smoothing the three axis target velocity
Input : Three-axis target velocity
Output : none
函数功能：对三轴目标速度做平滑处理
入口参数：三轴目标速度
返回 值：无
**************************************************************************/
void Smooth_control(float vx,float vy,float vz)
{
float step=0.01;
if (vx>0) smooth_control.VX+=step;
else if(vx<0) smooth_control.VX-=step;
else if(vx==0) smooth_control.VX=smooth_control.VX*0.9f;
if (vy>0) smooth_control.VY+=step;
else if(vy<0) smooth_control.VY-=step;
else if(vy==0) smooth_control.VY=smooth_control.VY*0.9f;
if (vz>0) smooth_control.VZ+=step;
else if(vz<0) smooth_control.VZ-=step;
else if(vz==0) smooth_control.VZ=smooth_control.VZ*0.9f;
smooth_control.VX=target_limit_float(smooth_control.VX,-float_abs(vx),float_abs(vx));
smooth_control.VY=target_limit_float(smooth_control.VY,-float_abs(vy),float_abs(vy));
smooth_control.VZ=target_limit_float(smooth_control.VZ,-float_abs(vz),float_abs(vz));
}
/**************************************************************************
Function: Floating-point data calculates the absolute value
Input : float
Output : The absolute value of the input number
函数功能：浮点型数据计算绝对值
入口参数：浮点数
返回 值：输入数的绝对值
**************************************************************************/
float float_abs(float insert)
{
if(insert>=0) return insert;
else return -insert;
}
/**************************************************************************
Function: Prevent the potentiometer to choose the wrong mode, resulting in initialization error caused by the motor spinning.Out of service
Input : none
Output : none
函数功能：防止电位器选错模式，导致初始化出错引发电机乱转。已停止使用
入口参数：无
返回 值：无
**************************************************************************/
void robot_mode_check(void)
{
static u8 error=0;
if(abs(MOTOR_A.Motor_Pwm)>2500||abs(MOTOR_B.Motor_Pwm)>2500||abs(MOTOR_C.Motor_Pwm)>2500||abs(MOTOR_D.Motor_Pwm)>2500) error++;
//If the output is close to full amplitude for 6 times in a row, it is judged that the motor rotates wildly and makes the motor incapacitated
//如果连续6次接近满幅输出，判断为电机乱转，让电机失能
if(error>6) EN=0,Flag_Stop=1,robot_mode_check_flag=1;
}