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

90
BALANCE/DataScope_DP.C Normal file
View File

@@ -0,0 +1,90 @@
#include "DataScope_DP.h"
/**************************************************************************
作者：平衡小车之家
我的淘宝小店：http://shop114407458.taobao.com/
**************************************************************************/
unsigned char DataScope_OutPut_Buffer[42] = {0}; //串口发送缓冲区
//函数说明：将单精度浮点数据转成4字节数据并存入指定地址
//附加说明：用户无需直接操作此函数
//target:目标单精度数据
//buf:待写入数组
//beg:指定从数组第几个元素开始写入
//函数无返回
void Float2Byte(float *target,unsigned char *buf,unsigned char beg)
{
unsigned char *point;
point = (unsigned char*)target; //得到float的地址
buf[beg] = point[0];
buf[beg+1] = point[1];
buf[beg+2] = point[2];
buf[beg+3] = point[3];
}
//函数说明：将待发送通道的单精度浮点数据写入发送缓冲区
//Data：通道数据
//Channel：选择通道（1-10）
//函数无返回
void DataScope_Get_Channel_Data(float Data,unsigned char Channel)
{
if ( (Channel > 10) || (Channel == 0) ) return; //通道个数大于10或等于0，直接跳出，不执行函数
else
{
switch (Channel)
{
case 1: Float2Byte(&Data,DataScope_OutPut_Buffer,1); break;
case 2: Float2Byte(&Data,DataScope_OutPut_Buffer,5); break;
case 3: Float2Byte(&Data,DataScope_OutPut_Buffer,9); break;
case 4: Float2Byte(&Data,DataScope_OutPut_Buffer,13); break;
case 5: Float2Byte(&Data,DataScope_OutPut_Buffer,17); break;
case 6: Float2Byte(&Data,DataScope_OutPut_Buffer,21); break;
case 7: Float2Byte(&Data,DataScope_OutPut_Buffer,25); break;
case 8: Float2Byte(&Data,DataScope_OutPut_Buffer,29); break;
case 9: Float2Byte(&Data,DataScope_OutPut_Buffer,33); break;
case 10: Float2Byte(&Data,DataScope_OutPut_Buffer,37); break;
}
}
}
//函数说明：生成 DataScopeV1.0 能正确识别的帧格式
//Channel_Number，需要发送的通道个数
//返回发送缓冲区数据个数
//返回0表示帧格式生成失败
unsigned char DataScope_Data_Generate(unsigned char Channel_Number)
{
if ( (Channel_Number > 10) || (Channel_Number == 0) ) { return 0; } //通道个数大于10或等于0，直接跳出，不执行函数
else
{
DataScope_OutPut_Buffer[0] = '$'; //帧头
switch(Channel_Number)
{
case 1: DataScope_OutPut_Buffer[5] = 5; return 6;
case 2: DataScope_OutPut_Buffer[9] = 9; return 10;
case 3: DataScope_OutPut_Buffer[13] = 13; return 14;
case 4: DataScope_OutPut_Buffer[17] = 17; return 18;
case 5: DataScope_OutPut_Buffer[21] = 21; return 22;
case 6: DataScope_OutPut_Buffer[25] = 25; return 26;
case 7: DataScope_OutPut_Buffer[29] = 29; return 30;
case 8: DataScope_OutPut_Buffer[33] = 33; return 34;
case 9: DataScope_OutPut_Buffer[37] = 37; return 38;
case 10: DataScope_OutPut_Buffer[41] = 41; return 42;
}
}
return 0;
}

21
BALANCE/DataScope_DP.h Normal file
View File

@@ -0,0 +1,21 @@
/**************************************************************************
作者：平衡小车之家
我的淘宝小店：http://shop114407458.taobao.com/
**************************************************************************/
#ifndef __DATA_PRTOCOL_H
#define __DATA_PRTOCOL_H
extern unsigned char DataScope_OutPut_Buffer[42]; //待发送帧数据缓存区
void DataScope_Get_Channel_Data(float Data,unsigned char Channel); // 写通道数据至 待发送帧数据缓存区
unsigned char DataScope_Data_Generate(unsigned char Channel_Number); // 发送帧数据生成函数
#endif

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

39
BALANCE/balance.h Normal file
View File

@@ -0,0 +1,39 @@
#ifndef __BALANCE_H
#define __BALANCE_H
#include "sys.h"
#include "system.h"
#define BALANCE_TASK_PRIO 4 //Task priority //任务优先级
#define BALANCE_STK_SIZE 512 //Task stack size //任务堆栈大小
//Parameter of kinematics analysis of omnidirectional trolley
//全向轮小车运动学分析参数
#define X_PARAMETER (sqrt(3)/2.f)
#define Y_PARAMETER (0.5f)
#define L_PARAMETER (1.0f)
extern short test_num;
extern int robot_mode_check_flag;
extern u8 command_lost_count; //串口、CAN控制命令丢失时间计数，丢失1秒后停止控制
void Balance_task(void *pvParameters);
void Set_Pwm(int motor_a,int motor_b,int motor_c,int motor_d,int servo);
void Limit_Pwm(int amplitude);
float target_limit_float(float insert,float low,float high);
int target_limit_int(int insert,int low,int high);
u8 Turn_Off( int voltage);
u32 myabs(long int a);
int Incremental_PI_A (float Encoder,float Target);
int Incremental_PI_B (float Encoder,float Target);
int Incremental_PI_C (float Encoder,float Target);
int Incremental_PI_D (float Encoder,float Target);
void Get_RC(void);
void Remote_Control(void);
void Drive_Motor(float Vx,float Vy,float Vz);
void Key(void);
void Get_Velocity_Form_Encoder(void);
void Smooth_control(float vx,float vy,float vz);
void PS2_control(void);
float float_abs(float insert);
void robot_mode_check(void);
#endif

259
BALANCE/control.c Normal file
View File

@@ -0,0 +1,259 @@
#include "control.h"
#include "filter.h"
/**************************************************************************
作者：平衡小车之家
我的淘宝小店：http://shop114407458.taobao.com/
**************************************************************************/
u8 Flag_Target,Flag_Change; //相关标志位
u8 temp1; //临时变量
float Voltage_Count,Voltage_All; //电压采样相关变量
float Gyro_K=0.004; //陀螺仪比例系数
int j;
#define a_PARAMETER (0.275f)
#define T 0.320f //0.145f
#define L 0.315f //0.17f
#define K 570.8f
/**************************************************************************
函数功能：小车运动数学模型
入口参数：Y轴速度和角度
返回 值：无
**************************************************************************/
void Kinematic_Analysis(float Vy,float angle)
{
Target_A = Vy*(1+T*tan(angle)/2/L);
Target_B = Vy*(1-T*tan(angle)/2/L);
Servo=SERVO_INIT-angle*K;
}
/**************************************************************************
函数功能：所有的控制代码都在这里面
5ms定时中断由MPU6050的INT引脚触发
严格保证采样和数据处理的时间同步
**************************************************************************/
int EXTI15_10_IRQHandler(void)
{
if(INT==0)
{
EXTI->PR=1<<15; //清除LINE5上的中断标志位
Flag_Target=!Flag_Target;
if(delay_flag==1)
{
if(++delay_50==10) delay_50=0,delay_flag=0; //给主函数提供50ms的精准延时
}
if(Flag_Target==1) //5ms读取一次陀螺仪和加速度计的值
{
if(Usart_Flag==0&&PS2_ON_Flag==0&&Usart_ON_Flag==1) memcpy(rxbuf,Urxbuf,8*sizeof(u8)); //如果解锁了串口控制标志位，进入串口控制模式
Read_DMP(); //===更新姿态
Key();//扫描按键变化
return 0;
} //===10ms控制一次，为了保证M法测速的时间基准，首先读取编码器数据
UA_Encoder=Read_Encoder(2); //===读取编码器的值
Encoder_A=UA_Encoder/25;
Position_A+=Encoder_A; //===积分得到速度
UB_Encoder=-Read_Encoder(3); //===读取编码器的值
Encoder_B=UB_Encoder/25;
Position_B+=Encoder_B; //===积分得到速度
Read_DMP(); //===更新姿态
Led_Flash(100); //===LED闪烁;常规模式 1s改变一次指示灯的状态
Voltage_All+=Get_battery_volt(); //多次采样累积
if(++Voltage_Count==100) Voltage=Voltage_All/100,Voltage_All=0,Voltage_Count=0;//求平均值 获取电池电压
if(PS2_KEY==4)PS2_ON_Flag=1,CAN_ON_Flag=0,Usart_ON_Flag=0;
if(CAN_ON_Flag==1||Usart_ON_Flag==1||PS2_ON_Flag==1) CAN_N_Usart_Control(); //接到串口或者CAN遥控解锁指令之后，使能CAN和串口控制输入
if(RC_Velocity>0&&RC_Velocity<15) RC_Velocity=15; //避免电机进入低速非线性区
if(Turn_Off(Voltage)==0) //===如果电池电压不存在异常
{
if(CAN_ON_Flag==0&&Usart_ON_Flag==0&&PS2_ON_Flag==0) Get_RC(Run_Flag);//===串口和CAN控制都未使能，则接收蓝牙遥控指
Motor_A=Incremental_PI_A(Encoder_A,Target_A); //===速度闭环控制计算电机A最终PWM
Motor_B=Incremental_PI_B(Encoder_B,Target_B); //===速度闭环控制计算电机B最终PWM
Xianfu_Pwm(6900); //===PWM限幅
Set_Pwm(-Motor_A,-Motor_B,Servo); //===赋值给PWM寄存器
}
else Set_Pwm(0,0,SERVO_INIT); //===赋值给PWM寄存器
}
return 0;
}
/**************************************************************************
函数功能：赋值给PWM寄存器
入口参数：PWM
返回 值：无
**************************************************************************/
void Set_Pwm(int motor_a,int motor_b,int servo)
{
if(motor_a<0) INA2=1, INA1=0;
else INA2=0, INA1=1;
PWMA=myabs(motor_a);
if(motor_b<0) INB2=1, INB1=0;
else INB2=0, INB1=1;
PWMB=myabs(motor_b);
SERVO=servo;
}
/**************************************************************************
函数功能：限制PWM赋值
入口参数：幅值
返回 值：无
**************************************************************************/
void Xianfu_Pwm(int amplitude)
{
if(Motor_A<-amplitude) Motor_A=-amplitude;
if(Motor_A>amplitude) Motor_A=amplitude;
if(Motor_B<-amplitude) Motor_B=-amplitude;
if(Motor_B>amplitude) Motor_B=amplitude;
if (Servo>1930) Servo=1930;
// if (Servo<1035) Servo=1035;
}
/**************************************************************************
函数功能：位置PID控制过程中速度的设置
入口参数：无、幅值
返回 值：无
**************************************************************************/
void Xianfu_Velocity(int amplitude_A,int amplitude_B,int amplitude_C,int amplitude_D)
{
if(Motor_A<-amplitude_A) Motor_A=-amplitude_A; //位置控制模式中，A电机的运行速度
if(Motor_A>amplitude_A) Motor_A=amplitude_A; //位置控制模式中，A电机的运行速度
if(Motor_B<-amplitude_B) Motor_B=-amplitude_B; //位置控制模式中，B电机的运行速度
if(Motor_B>amplitude_B) Motor_B=amplitude_B; //位置控制模式中，B电机的运行速度
}
/**************************************************************************
函数功能：按键修改小车运行状态
入口参数：无
返回 值：无
**************************************************************************/
void Key(void)
{
u8 tmp;
tmp=click_N_Double(100);
if(tmp==2)Flag_Show=!Flag_Show;//双击控制显示模式
}
/**************************************************************************
函数功能：异常关闭电机
入口参数：电压
返回 值：1：异常 0：正常
**************************************************************************/
u8 Turn_Off( int voltage)
{
u8 temp;
if(voltage<1110)//电池电压过低关闭电机
{
temp=1;
PWMA=0;
PWMB=0;
}
else
temp=0;
return temp;
}
/**************************************************************************
函数功能：绝对值函数
入口参数：long int
返回 值：unsigned int
**************************************************************************/
u32 myabs(long int a)
{
u32 temp;
if(a<0) temp=-a;
else temp=a;
return temp;
}
/**************************************************************************
函数功能：增量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 (int Encoder,int Target)
{
static int Bias,Pwm,Last_bias;
Bias=Encoder-Target; //计算偏差
Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; //增量式PI控制器
if(Pwm>7200)Pwm=7200;
if(Pwm<-7200)Pwm=-7200;
Last_bias=Bias; //保存上一次偏差
return Pwm; //增量输出
}
int Incremental_PI_B (int Encoder,int Target)
{
static int Bias,Pwm,Last_bias;
Bias=Encoder-Target; //计算偏差
Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; //增量式PI控制器
if(Pwm>7200)Pwm=7200;
if(Pwm<-7200)Pwm=-7200;
Last_bias=Bias; //保存上一次偏差
return Pwm; //增量输出
}
/**************************************************************************
函数功能：通过串口指令对小车进行遥控
入口参数：串口指令
返回 值：无
**************************************************************************/
void Get_RC(u8 mode)
{
// float step=0.3f; //设置速度控制步进值。
if(mode==0)//速度
{
switch(Flag_Direction) //方向控制
{
case 1: Move_Y=RC_Velocity; Angle=0; break;
case 2: Move_Y=RC_Velocity; Angle=PI/4; break;
case 3: Move_Y=0; Angle=0; break;
case 4: Move_Y=-RC_Velocity; Angle=-PI/4; break;
case 5: Move_Y=-RC_Velocity; Angle=0; break;
case 6: Move_Y=-RC_Velocity; Angle=PI/4; break;
case 7: Move_Y=0; Angle=0; break;
case 8: Move_Y=+RC_Velocity; Angle=-PI/4; break;
default: Move_Y=0; Angle=0; break;
}
// if(Move_Y<-RC_Velocity) Move_Y=-RC_Velocity;
// if(Move_Y>RC_Velocity) Move_Y=RC_Velocity;
// if(Angle<-RC_Velocity) Angle=-RC_Velocity;
// if(Angle>RC_Velocity) Angle=RC_Velocity;
}
Kinematic_Analysis(Move_Y,Angle);//得到控制目标值，进行运动学分析
}
/**************************************************************************
函数功能：接收CAN或者串口控制指令进行处理
入口参数：无
返回 值：无
**************************************************************************/
void CAN_N_Usart_Control(void)
{
// int flag_1, flag_2;
int RX,LY;
int Yuzhi=20;
if(CAN_ON_Flag==1||Usart_ON_Flag==1)
{
// if(rxbuf[1])flag_1=1; else flag_1=-1; //方向控制位
// if(rxbuf[3])flag_2=1; else flag_2=-1; //方向控制位
// Target_A=flag_1*rxbuf[0];
// Target_B=flag_2*rxbuf[1];
if(rxbuf[1]==0)Move_Y=rxbuf[0]; //识别运动方向
else Move_Y=-rxbuf[0]; //速度
Angle=(rxbuf[2]-90)*PI/180; //角度获取
}
else if (PS2_ON_Flag==1)
{
RX=PS2_RX-128;
LY=PS2_LY-128;
if(RX>-Yuzhi&&RX<Yuzhi)RX=0; //消除非线性区
if(LY>-Yuzhi&&LY<Yuzhi)LY=0;
Angle= RX*PI/4/120;
Move_Y=-LY/2.84;
//if(Move_Y<0)Angle=-Angle;
}
Kinematic_Analysis(Move_Y,Angle);//得到控制目标值，进行运动学分析
}

33
BALANCE/control.h Normal file
View File

@@ -0,0 +1,33 @@
#ifndef __CONTROL_H
#define __CONTROL_H
#include "sys.h"
/**************************************************************************
作者：平衡小车之家
我的淘宝小店：http://shop114407458.taobao.com/
**************************************************************************/
#define PI 3.14159265
#define ZHONGZHI 0
#define DIFFERENCE 100
extern int Balance_Pwm,Velocity_Pwm,Turn_Pwm;
int EXTI15_10_IRQHandler(void);
void Set_Pwm(int motor_a,int motor_b,int servo);
void Kinematic_Analysis(float Vy,float angle);
void Kinematic_Analysis2(float Vy,float Vz);
void Key(void);
void Xianfu_Pwm(int amplitude);
void Xianfu_Velocity(int amplitude_A,int amplitude_B,int amplitude_C,int amplitude_D);
u8 Turn_Off( int voltage);
u32 myabs(long int a);
int Incremental_PI_A (int Encoder,int Target);
int Incremental_PI_B (int Encoder,int Target);
int Incremental_PI_C (int Encoder,int Target);
int Incremental_PI_D (int Encoder,int Target);
int Position_PID_A (int Encoder,int Target);
int Position_PID_B (int Encoder,int Target);
int Position_PID_C (int Encoder,int Target);
int Position_PID_D (int Encoder,int Target);
void Get_RC(u8 mode);
void Count_Velocity(void);
void CAN_N_Usart_Control(void);
#endif

68
BALANCE/filter.c Normal file
View File

@@ -0,0 +1,68 @@
#include "filter.h"
/**************************************************************************
作者：平衡小车之家
我的淘宝小店：http://shop114407458.taobao.com/
**************************************************************************/
float K1 =0.02;
float angle, angle_dot;
float Q_angle=0.001;// 过程噪声的协方差
float Q_gyro=0.003;//0.003 过程噪声的协方差 过程噪声的协方差为一个一行两列矩阵
float R_angle=0.5;// 测量噪声的协方差 既测量偏差
float dt=0.005;//
char C_0 = 1;
float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
/**************************************************************************
函数功能：简易卡尔曼滤波
入口参数：加速度、角速度
返回 值：无
**************************************************************************/
void Kalman_Filter(float Accel,float Gyro)
{
angle+=(Gyro - Q_bias) * dt; //先验估计
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
Pdot[1]=-PP[1][1];
Pdot[2]=-PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - angle; //zk-先验估计
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0; //后验估计误差协方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
angle += K_0 * Angle_err; //后验估计
Q_bias += K_1 * Angle_err; //后验估计
angle_dot = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
}
/**************************************************************************
函数功能：一阶互补滤波
入口参数：加速度、角速度
返回 值：无
**************************************************************************/
void Yijielvbo(float angle_m, float gyro_m)
{
angle = K1 * angle_m+ (1-K1) * (angle + gyro_m * 0.005);
}

11
BALANCE/filter.h Normal file
View File

@@ -0,0 +1,11 @@
#ifndef __FILTER_H
#define __FILTER_H
#include "system.h"
/**************************************************************************
作者：平衡小车之家
我的淘宝小店：http://shop114407458.taobao.com/
**************************************************************************/
extern float angle, angle_dot;
void Kalman_Filter(float Accel,float Gyro);
void Yijielvbo(float angle_m, float gyro_m);
#endif

93
BALANCE/robot_select_init.c Normal file
View File

@@ -0,0 +1,93 @@
#include "robot_select_init.h"
//Initialize the robot parameter structure
//初始化机器人参数结构体
Robot_Parament_InitTypeDef Robot_Parament;
/**************************************************************************
Function: According to the potentiometer switch needs to control the car type
Input : none
Output : none
函数功能：根据电位器切换需要控制的小车类型
入口参数：无
返回 值：无
**************************************************************************/
void Robot_Select(void)
{
//The ADC value is variable in segments, depending on the number of car models. Currently there are 6 car models, CAR_NUMBER=6
//ADC值分段变量，取决于小车型号数量，目前有6种小车型号，CAR_NUMBER=6
Divisor_Mode=2048/CAR_NUMBER+5;
Car_Mode=(int) ((Get_adc_Average(Potentiometer,10))/Divisor_Mode); //Collect the pin information of potentiometer //采集电位器引脚信息
if(Car_Mode>5)Car_Mode=5;
//Car_Mode=0;
switch(Car_Mode)
{
case Mec_Car: Robot_Init(MEC_wheelspacing, MEC_axlespacing, 0, HALL_30F, Hall_13, Mecanum_75); break; //麦克纳姆轮小车
case Omni_Car: Robot_Init(0, 0, Omni_Turn_Radiaus_109, HALL_30F, Hall_13, FullDirecion_60); break; //全向轮小车
case Akm_Car: Robot_Init(Akm_wheelspacing, Akm_axlespacing, 0, HALL_30F, Hall_13, Black_WheelDiameter); break; //阿克曼小车
case Diff_Car: Robot_Init(Diff_wheelSpacing, 0, 0, HALL_30F, Hall_13, Black_WheelDiameter); break; //两轮差速小车
case FourWheel_Car: Robot_Init(Four_Mortor_wheelSpacing, Four_Mortor__axlespacing, 0, HALL_30F, Hall_13, Black_WheelDiameter); break; //四驱车
case Tank_Car: Robot_Init(Tank_wheelSpacing, 0, 0, HALL_30F, Hall_13, Tank_WheelDiameter); break; //履带车
}
//Check the parameters//自检相关参数
switch(Car_Mode)
{
case Mec_Car: CheckPhrase1=8, CheckPhrase2=14; break; //麦克纳姆轮小车
case Omni_Car: CheckPhrase1=6, CheckPhrase2=10; break; //全向轮小车
case Akm_Car: CheckPhrase1=4, CheckPhrase2=7; break; //阿克曼小车
case Diff_Car: CheckPhrase1=4, CheckPhrase2=7; break; //两轮差速小车
case FourWheel_Car: CheckPhrase1=8, CheckPhrase2=11; break; //四驱车
case Tank_Car: CheckPhrase1=4, CheckPhrase2=7; break; //履带车
}
}
/**************************************************************************
Function: Initialize cart parameters
Input : wheelspacing, axlespacing, omni_rotation_radiaus, motor_gear_ratio, Number_of_encoder_lines, tyre_diameter
Output : none
函数功能：初始化小车参数
入口参数：轮距 轴距 自转半径 电机减速比 电机编码器精度 轮胎直径
返回 值：无
**************************************************************************/
void Robot_Init(double wheelspacing, float axlespacing, float omni_turn_radiaus, float gearratio,float Accuracy,float tyre_diameter) //
{
//wheelspacing, Mec_Car is half wheelspacing
//轮距 麦轮车为半轮距
Robot_Parament.WheelSpacing=wheelspacing;
//axlespacing, Mec_Car is half axlespacing
//轴距 麦轮车为半轴距
Robot_Parament.AxleSpacing=axlespacing;
//Rotation radius of omnidirectional trolley
//全向轮小车旋转半径
Robot_Parament.OmniTurnRadiaus=omni_turn_radiaus;
//motor_gear_ratio
//电机减速比
Robot_Parament.GearRatio=gearratio;
//Number_of_encoder_lines
//编码器精度(编码器线数)
Robot_Parament.EncoderAccuracy=Accuracy;
//Diameter of driving wheel
//主动轮直径
Robot_Parament.WheelDiameter=tyre_diameter;
//Encoder value corresponding to 1 turn of motor (wheel)
//电机(车轮)转1圈对应的编码器数值
Encoder_precision=EncoderMultiples*Robot_Parament.EncoderAccuracy*Robot_Parament.GearRatio;
//Driving wheel circumference
//主动轮周长
Wheel_perimeter=Robot_Parament.WheelDiameter*PI;
//wheelspacing, Mec_Car is half wheelspacing
//轮距 麦轮车为半轮距
Wheel_spacing=Robot_Parament.WheelSpacing;
//axlespacing, Mec_Car is half axlespacing
//轴距 麦轮车为半轴距
Axle_spacing=Robot_Parament.AxleSpacing;
//Rotation radius of omnidirectional trolley
//全向轮小车旋转半径
Omni_turn_radiaus=Robot_Parament.OmniTurnRadiaus;
}

108
BALANCE/robot_select_init.h Normal file
View File

@@ -0,0 +1,108 @@
#ifndef __ROBOTSELECTINIT_H
#define __ROBOTSELECTINIT_H
#include "sys.h"
#include "system.h"
//Parameter structure of robot
//机器人参数结构体
typedef struct
{
float WheelSpacing; //Wheelspacing, Mec_Car is half wheelspacing //轮距 麦轮车为半轮距
float AxleSpacing; //Axlespacing, Mec_Car is half axlespacing //轴距 麦轮车为半轴距
int GearRatio; //Motor_gear_ratio //电机减速比
int EncoderAccuracy; //Number_of_encoder_lines //编码器精度(编码器线数)
float WheelDiameter; //Diameter of driving wheel //主动轮直径
float OmniTurnRadiaus; //Rotation radius of omnidirectional trolley //全向轮小车旋转半径
}Robot_Parament_InitTypeDef;
// Encoder structure
//编码器结构体
typedef struct
{
int A;
int B;
int C;
int D;
}Encoder;
//The minimum turning radius of Ackermann models is determined by the mechanical structure:
//the maximum Angle of the wheelbase, wheelbase and front wheels
//阿克曼车型的最小转弯半径，由机械结构决定：轮距、轴距、前轮最大转角
#define MINI_AKM_MIN_TURN_RADIUS 0.350f
//Wheelspacing, Mec_Car is half wheelspacing
//轮距 麦轮是一半
//#define MEC_wheelspacing 0.109
#define MEC_wheelspacing 0.0930 //修正2021.03.30
#define Akm_wheelspacing 0.162f
#define Diff_wheelSpacing 0.177f
#define Four_Mortor_wheelSpacing 0.26f
#define Tank_wheelSpacing 0.235f
//Axlespacing, Mec_Car is half axlespacing
//轴距 麦轮是一半
#define MEC_axlespacing 0.085
#define Akm_axlespacing 0.158f
#define Diff_axlespacing 0.155f
#define Four_Mortor__axlespacing 0.28f
#define Tank_axlespacing 0.222f
//Motor_gear_ratio
//电机减速比
#define HALL_30F 30
#define HALL_60F 60
#define MD36N_5_18 5.18
#define MD36N_27 27
#define MD36N_51 51
#define MD36N_71 71
#define MD60N_18 18
#define MD60N_47 47
//Number_of_encoder_lines
//编码器精度
#define Photoelectric_500 500
#define Hall_13 13
//Mecanum wheel tire diameter series
//麦轮轮胎直径
#define Mecanum_60 0.060f
#define Mecanum_75 0.075f
#define Mecanum_100 0.100f
#define Mecanum_127 0.127f
#define Mecanum_152 0.152f
//Omni wheel tire diameter series
//轮径全向轮直径系列
#define FullDirecion_60 0.060
#define FullDirecion_75 0.075
#define FullDirecion_127 0.127
#define FullDirecion_152 0.152
#define FullDirecion_203 0.203
#define FullDirecion_217 0.217
//Black tire, tank_car wheel diameter
//黑色轮胎、履带车轮直径
#define Black_WheelDiameter 0.065
//#define Tank_WheelDiameter 0.047
#define Tank_WheelDiameter 0.043
//Rotation radius of omnidirectional trolley
//全向轮小车旋转半径
#define Omni_Turn_Radiaus_109 0.109
#define Omni_Turn_Radiaus_164 0.164
#define Omni_Turn_Radiaus_180 0.180
#define Omni_Turn_Radiaus_290 0.290
//The encoder octave depends on the encoder initialization Settings
//编码器倍频数，取决于编码器初始化设置
#define EncoderMultiples 4
//Encoder data reading frequency
//编码器数据读取频率
#define CONTROL_FREQUENCY 100
//#define PI 3.1415f //PI //圆周率
void Robot_Select(void);
void Robot_Init(double wheelspacing, float axlespacing, float omni_turn_radiaus, float gearratio,float Accuracy,float tyre_diameter);
#endif

344
BALANCE/show.c Normal file
View File

@@ -0,0 +1,344 @@
#include "show.h"
int Voltage_Show;
unsigned char i;
unsigned char Send_Count;
extern SEND_DATA Send_Data;
extern int MPU9250ErrorCount, EncoderA_Count, EncoderB_Count, EncoderC_Count, EncoderD_Count;
extern int MPU9250SensorCountA, MPU9250SensorCountB, MPU9250SensorCountC, MPU9250SensorCountD;
extern int Time_count;
/**************************************************************************
Function: Read the battery voltage, buzzer alarm, start the self-test, send data to APP, OLED display task
Input : none
Output : none
函数功能：读取电池电压、蜂鸣器报警、开启自检、向APP发送数据、OLED显示屏显示任务
入口参数：无
返回 值：无
**************************************************************************/
int Buzzer_count=25;
void show_task(void *pvParameters)
{
u32 lastWakeTime = getSysTickCnt();
while(1)
{
int i=0;
static int LowVoltage_1=0, LowVoltage_2=0;
vTaskDelayUntil(&lastWakeTime, F2T(RATE_10_HZ));//This task runs at 10Hz //此任务以10Hz的频率运行
//开机时蜂鸣器短暂蜂鸣，开机提醒
//The buzzer will beep briefly when the machine is switched on
if(Time_count<50)Buzzer=1;
else if(Time_count>=51 && Time_count<100)Buzzer=0;
if(LowVoltage_1==1 || LowVoltage_2==1)Buzzer_count=0;
if(Buzzer_count<5)Buzzer_count++;
if(Buzzer_count<5)Buzzer=1; //The buzzer is buzzing //蜂鸣器蜂鸣
else if(Buzzer_count==5)Buzzer=0;
//Read the battery voltage //读取电池电压
for(i=0;i<10;i++)
{
Voltage_All+=Get_battery_volt();
}
Voltage=Voltage_All/10;
Voltage_All=0;
if(LowVoltage_1==1)LowVoltage_1++; //Make sure the buzzer only rings for 0.5 seconds //确保蜂鸣器只响0.5秒
if(LowVoltage_2==1)LowVoltage_2++; //Make sure the buzzer only rings for 0.5 seconds //确保蜂鸣器只响0.5秒
if(Voltage>=12.6f)Voltage=12.6f;
else if(10<=Voltage && Voltage<10.5f && LowVoltage_1<2)LowVoltage_1++; //10.5V, first buzzer when low battery //10.5V，低电量时蜂鸣器第一次报警
else if(Voltage<10 && LowVoltage_1<2)LowVoltage_2++; //10V, when the car is not allowed to control, the buzzer will alarm the second time //10V，小车禁止控制时蜂鸣器第二次报警
APP_Show(); //Send data to the APP //向APP发送数据
oled_show(); //Tasks are displayed on the screen //显示屏显示任务
}
}
/**************************************************************************
Function: The OLED display displays tasks
Input : none
Output : none
函数功能：OLED显示屏显示任务
入口参数：无
返回 值：无
**************************************************************************/
void oled_show(void)
{
static int count=0;
int Car_Mode_Show;
//Collect the tap information of the potentiometer,
//and display the car model to be fitted when the car starts up in real time
//采集电位器档位信息，实时显示小车开机时要适配的小车型号
Divisor_Mode=2048/CAR_NUMBER+5;
Car_Mode_Show=(int) ((Get_adc_Average(Potentiometer,10))/Divisor_Mode);
if(Car_Mode_Show>5)Car_Mode_Show=5;
//Car_Mode_Show=0;
Voltage_Show=Voltage*100;
count++;
if(Check==0)//The car displays normally when the self-check mode is not enabled //没有开启自检模式时小车正常显示
{
//The first line of the display displays the content//
//显示屏第1行显示内容//
switch(Car_Mode_Show)
{
case Mec_Car: OLED_ShowString(0,0,"Mec "); break;
case Omni_Car: OLED_ShowString(0,0,"Omni"); break;
case Akm_Car: OLED_ShowString(0,0,"Akm "); break;
case Diff_Car: OLED_ShowString(0,0,"Diff"); break;
case FourWheel_Car: OLED_ShowString(0,0,"4WD "); break;
case Tank_Car: OLED_ShowString(0,0,"Tank"); break;
}
if(Car_Mode==Mec_Car||Car_Mode==Omni_Car)
{
//The Mec_car and omni_car show Z-axis angular velocity
//麦轮、全向轮小车显示Z轴角速度
OLED_ShowString(55,0,"GZ");
if( gyro[2]<0) OLED_ShowString(80,0,"-"),OLED_ShowNumber(90,0,-gyro[2],5,12);
else OLED_ShowString(80,0,"+"),OLED_ShowNumber(90,0, gyro[2],5,12);
}
else if(Car_Mode==Akm_Car||Car_Mode==Diff_Car||Car_Mode==FourWheel_Car||Car_Mode==Tank_Car)
{
//Akm_Car, Diff_Car, FourWheel_Car and Tank_Car Displays gyroscope zero
//阿克曼、差速、四驱、履带车显示陀螺仪零点
OLED_ShowString(55,0,"BIAS");
if( Deviation_gyro[2]<0) OLED_ShowString(90,0,"-"),OLED_ShowNumber(100,0,-Deviation_gyro[2],3,12); //Zero-drift data of gyroscope Z axis
else OLED_ShowString(90,0,"+"),OLED_ShowNumber(100,0, Deviation_gyro[2],3,12); //陀螺仪z轴零点漂移数据
}
//The first line of the display displays the content//
//显示屏第1行显示内容//
//The second line of the display displays the content//
//显示屏第2行显示内容//
if(Car_Mode==Mec_Car||Car_Mode==Omni_Car||Car_Mode==FourWheel_Car)
{
//Mec_Car, Omni_Car and FourWheel_Car Display the target speed and current actual speed of motor A
//麦轮、全向轮、四驱车显示电机A的目标速度和当前实际速度
OLED_ShowString(0,10,"A");
if( MOTOR_A.Target<0) OLED_ShowString(15,10,"-"),
OLED_ShowNumber(20,10,-MOTOR_A.Target*1000,5,12);
else OLED_ShowString(15,10,"+"),
OLED_ShowNumber(20,10, MOTOR_A.Target*1000,5,12);
if( MOTOR_A.Encoder<0)OLED_ShowString(60,10,"-"),
OLED_ShowNumber(75,10,-MOTOR_A.Encoder*1000,5,12);
else OLED_ShowString(60,10,"+"),
OLED_ShowNumber(75,10, MOTOR_A.Encoder*1000,5,12);
}
else if(Car_Mode==Akm_Car||Car_Mode==Diff_Car||Car_Mode==Tank_Car)
{
//The Akm_Car, Diff_Car and Tank_Car show Z-axis angular velocity
//阿克曼、差速、坦克小车显示Z轴角速度
OLED_ShowString(00,10,"GYRO_Z:");
if( gyro[2]<0) OLED_ShowString(60,10,"-"),
OLED_ShowNumber(75,10,-gyro[2],5,12);
else OLED_ShowString(60,10,"+"),
OLED_ShowNumber(75,10, gyro[2],5,12);
}
//The second line of the display displays the content//
//显示屏第2行显示内容//
//Lines 3 and 4 of the display screen display content//
//显示屏第3、4行显示内容//
if(Car_Mode==Mec_Car||Car_Mode==Omni_Car||Car_Mode==FourWheel_Car)
{
//Mec_Car, Omni_Car and FourWheel_Car Display the target speed and current actual speed of motor B
//麦轮、全向轮、四驱车显示电机B的目标速度和当前实际速度
OLED_ShowString(0,20,"B");
if( MOTOR_B.Target<0) OLED_ShowString(15,20,"-"),
OLED_ShowNumber(20,20,-MOTOR_B.Target*1000,5,12);
else OLED_ShowString(15,20,"+"),
OLED_ShowNumber(20,20, MOTOR_B.Target*1000,5,12);
if( MOTOR_B.Encoder<0)OLED_ShowString(60,20,"-"),
OLED_ShowNumber(75,20,-MOTOR_B.Encoder*1000,5,12);
else OLED_ShowString(60,20,"+"),
OLED_ShowNumber(75,20, MOTOR_B.Encoder*1000,5,12);
//Mec_Car, Omni_Car and FourWheel_Car Display the target speed and current actual speed of motor C
//麦轮、全向轮、四驱车显示电机C的目标速度和当前实际速度
OLED_ShowString(0,30,"C");
if( MOTOR_C.Target<0) OLED_ShowString(15,30,"-"),
OLED_ShowNumber(20,30,- MOTOR_C.Target*1000,5,12);
else OLED_ShowString(15,30,"+"),
OLED_ShowNumber(20,30, MOTOR_C.Target*1000,5,12);
if( MOTOR_C.Encoder<0)OLED_ShowString(60,30,"-"),
OLED_ShowNumber(75,30,-MOTOR_C.Encoder*1000,5,12);
else OLED_ShowString(60,30,"+"),
OLED_ShowNumber(75,30, MOTOR_C.Encoder*1000,5,12);
}
else if(Car_Mode==Akm_Car||Car_Mode==Diff_Car||Car_Mode==Tank_Car)
{
//Akm_Car, Diff_Car and Tank_Car Display the target speed and current actual speed of motor A
//阿克曼、差速、履带车显示电机A的目标速度和当前实际速度
OLED_ShowString(0,20,"L:");
if( MOTOR_A.Target<0) OLED_ShowString(15,20,"-"),
OLED_ShowNumber(20,20,-MOTOR_A.Target*1000,5,12);
else OLED_ShowString(15,20,"+"),
OLED_ShowNumber(20,20, MOTOR_A.Target*1000,5,12);
if( MOTOR_A.Encoder<0) OLED_ShowString(60,20,"-"),
OLED_ShowNumber(75,20,-MOTOR_A.Encoder*1000,5,12);
else OLED_ShowString(60,20,"+"),
OLED_ShowNumber(75,20, MOTOR_A.Encoder*1000,5,12);
//Akm_Car, Diff_Car and Tank_Car Display the target speed and current actual speed of motor B
//阿克曼、差速、履带车显示电机B的目标速度和当前实际速度
OLED_ShowString(0,30,"R:");
if( MOTOR_B.Target<0) OLED_ShowString(15,30,"-"),
OLED_ShowNumber(20,30,-MOTOR_B.Target*1000,5,12);
else OLED_ShowString(15,30,"+"),
OLED_ShowNumber(20,30, MOTOR_B.Target*1000,5,12);
if( MOTOR_B.Encoder<0) OLED_ShowString(60,30,"-"),
OLED_ShowNumber(75,30,-MOTOR_B.Encoder*1000,5,12);
else OLED_ShowString(60,30,"+"),
OLED_ShowNumber(75,30, MOTOR_B.Encoder*1000,5,12);
// if( Remoter_Ch1<0) OLED_ShowString(15,20,"-"),
// OLED_ShowNumber(20,20,-Remoter_Ch1,5,12);
// else OLED_ShowString(15,20,"+"),
// OLED_ShowNumber(20,20, Remoter_Ch1,5,12);
// if( Remoter_Ch2<0) OLED_ShowString(60,20,"-"),
// OLED_ShowNumber(75,20,-Remoter_Ch2,5,12);
// else OLED_ShowString(60,20,"+"),
// OLED_ShowNumber(75,20, Remoter_Ch2,5,12);
// if( Remoter_Ch3<0) OLED_ShowString(15,30,"-"),
// OLED_ShowNumber(20,30,-Remoter_Ch3,5,12);
// else OLED_ShowString(15,30,"+"),
// OLED_ShowNumber(20,30, Remoter_Ch3,5,12);
// if( Remoter_Ch4<0) OLED_ShowString(60,30,"-"),
// OLED_ShowNumber(75,30,-Remoter_Ch4,5,12);
// else OLED_ShowString(60,30,"+"),
// OLED_ShowNumber(75,30, Remoter_Ch4,5,12);
}
//Lines 3 and 4 of the display screen display content//
//显示屏第3、4行显示内容//
//Line 5 of the display displays the content//
//显示屏第5行显示内容//
if(Car_Mode==Mec_Car||Car_Mode==FourWheel_Car)
{
//Mec_Car Display the target speed and current actual speed of motor D
//麦轮小车显示电机D的目标速度和当前实际速度
OLED_ShowString(0,40,"D");
if( MOTOR_D.Target<0) OLED_ShowString(15,40,"-"),
OLED_ShowNumber(20,40,- MOTOR_D.Target*1000,5,12);
else OLED_ShowString(15,40,"+"),
OLED_ShowNumber(20,40, MOTOR_D.Target*1000,5,12);
if( MOTOR_D.Encoder<0) OLED_ShowString(60,40,"-"),
OLED_ShowNumber(75,40,-MOTOR_D.Encoder*1000,5,12);
else OLED_ShowString(60,40,"+"),
OLED_ShowNumber(75,40, MOTOR_D.Encoder*1000,5,12);
}
else if(Car_Mode==Omni_Car)
{
// The Omni_car shows Z-axis angular velocity (1000 times magnification) in rad/s
//全向轮小车显示Z轴角速度(放大1000倍)，单位rad/s
OLED_ShowString(0,40,"MOVE_Z");
if(Send_Data.Sensor_Str.X_speed<0) OLED_ShowString(60,40,"-"),
OLED_ShowNumber(75,40,-Send_Data.Sensor_Str.X_speed,5,12);
else OLED_ShowString(60,40,"+"),
OLED_ShowNumber(75,40, Send_Data.Sensor_Str.X_speed,5,12);
}
else if(Car_Mode==Akm_Car)
{
//Akm_Car displays the PWM value of the Servo
//阿克曼小车显示舵机的PWM的数值
OLED_ShowString(00,40,"SERVO:");
if( Servo<0) OLED_ShowString(60,40,"-"),
OLED_ShowNumber(80,40,-Servo,4,12);
else OLED_ShowString(60,40,"+"),
OLED_ShowNumber(80,40, Servo,4,12);
}
else if(Car_Mode==Diff_Car||Car_Mode==Tank_Car)
{
// The Diff_Car and Tank_Car displays the PWM values of the left and right motors
//差速小车、履带车显示左右电机的PWM的数值
OLED_ShowString(00,40,"MA");
if( MOTOR_A.Motor_Pwm<0)OLED_ShowString(20,40,"-"),
OLED_ShowNumber(30,40,-MOTOR_A.Motor_Pwm,4,12);
else OLED_ShowString(20,40,"+"),
OLED_ShowNumber(30,40, MOTOR_A.Motor_Pwm,4,12);
OLED_ShowString(60,40,"MB");
if(MOTOR_B.Motor_Pwm<0) OLED_ShowString(80,40,"-"),
OLED_ShowNumber(90,40,-MOTOR_B.Motor_Pwm,4,12);
else OLED_ShowString(80,40,"+"),
OLED_ShowNumber(90,40, MOTOR_B.Motor_Pwm,4,12);
}
//Line 5 of the display displays the content//
//显示屏第5行显示内容//
//Displays the current control mode //显示当前控制模式
if(PS2_ON_Flag==1) OLED_ShowString(0,50,"PS2 ");
else if (APP_ON_Flag==1) OLED_ShowString(0,50,"APP ");
else if (Remote_ON_Flag==1)OLED_ShowString(0,50,"R-C ");
else if (CAN_ON_Flag==1) OLED_ShowString(0,50,"CAN ");
else if ((Usart1_ON_Flag || Usart5_ON_Flag)==1) OLED_ShowString(0,50,"USART");
else OLED_ShowString(0,50,"ROS ");
//Displays whether controls are allowed in the current car
//显示当前小车是否允许控制
if(EN==1&&Flag_Stop==0) OLED_ShowString(45,50,"O N");
else OLED_ShowString(45,50,"OFF");
OLED_ShowNumber(75,50,Voltage_Show/100,2,12);
OLED_ShowString(88,50,".");
OLED_ShowNumber(98,50,Voltage_Show%100,2,12);
OLED_ShowString(110,50,"V");
if(Voltage_Show%100<10) OLED_ShowNumber(92,50,0,2,12);
}
OLED_Refresh_Gram();
}
/**************************************************************************
Function: Send data to the APP
Input : none
Output : none
函数功能：向APP发送数据
入口参数：无
返回 值：无
**************************************************************************/
void APP_Show(void)
{
static u8 flag_show;
int Left_Figure,Right_Figure,Voltage_Show;
//The battery voltage is processed as a percentage
//对电池电压处理成百分比形式
Voltage_Show=(Voltage*1000-10000)/27;
if(Voltage_Show>100)Voltage_Show=100;
//Wheel speed unit is converted to 0.01m/s for easy display in APP
//车轮速度单位转换为0.01m/s，方便在APP显示
Left_Figure=MOTOR_A.Encoder*100; if(Left_Figure<0)Left_Figure=-Left_Figure;
Right_Figure=MOTOR_B.Encoder*100; if(Right_Figure<0)Right_Figure=-Right_Figure;
//Used to alternately print APP data and display waveform
//用于交替打印APP数据和显示波形
flag_show=!flag_show;
if(PID_Send==1)
{
//Send parameters to the APP, the APP is displayed in the debug screen
//发送参数到APP，APP在调试界面显示
printf("{C%d:%d:%d}$",(int)RC_Velocity,(int)Velocity_KP,(int)Velocity_KI);
PID_Send=0;
}
else if(flag_show==0)
{
//Send parameters to the APP and the APP will be displayed on the front page
//发送参数到APP，APP在首页显示
printf("{A%d:%d:%d:%d}$",(u8)Left_Figure,(u8)Right_Figure,Voltage_Show,(int)gyro[2]);
}
else
{
//Send parameters to the APP, the APP is displayed in the waveform interface
//发送参数到APP，APP在波形界面显示
printf("{B%d:%d:%d}$",(int)gyro[0],(int)gyro[1],(int)gyro[2]);
}
}

15
BALANCE/show.h Normal file
View File

@@ -0,0 +1,15 @@
#ifndef __SHOW_H
#define __SHOW_H
#include "sys.h"
#include "oled.h"
#include "system.h"
#define SHOW_TASK_PRIO 3
#define SHOW_STK_SIZE 512
void show_task(void *pvParameters);
void oled_show(void);
void APP_Show(void);
void OLED_ShowCheckConfirming(void);
void OLED_ShowChecking(void);
void OLED_ShowCheckResult(void);
#endif

197
BALANCE/system.c Normal file
View File

@@ -0,0 +1,197 @@
#include "system.h"
//Robot software fails to flag bits
//机器人软件失能标志位
u8 Flag_Stop=1;
//The ADC value is variable in segments, depending on the number of car models. Currently there are 6 car models
//ADC值分段变量，取决于小车型号数量，目前有6种小车型号
int Divisor_Mode;
// Robot type variable
//机器人型号变量
//0=Mec_Car，1=Omni_Car，2=Akm_Car，3=Diff_Car，4=FourWheel_Car，5=Tank_Car
u8 Car_Mode=4;
//Servo control PWM value, Ackerman car special
//舵机控制PWM值，阿克曼小车专用
int Servo;
//Default speed of remote control car, unit: mm/s
//遥控小车的默认速度，单位：mm/s
float RC_Velocity=500;
//Vehicle three-axis target moving speed, unit: m/s
//小车三轴目标运动速度，单位：m/s
float Move_X, Move_Y, Move_Z;
//PID parameters of Speed control
//速度控制PID参数
float Velocity_KP=300,Velocity_KI=800;
//Smooth control of intermediate variables, dedicated to omni-directional moving cars
//平滑控制中间变量，全向移动小车专用
Smooth_Control smooth_control;
//The parameter structure of the motor
//电机的参数结构体
Motor_parameter MOTOR_A,MOTOR_B,MOTOR_C,MOTOR_D;
/************ 小车型号相关变量 **************************/
/************ Variables related to car model ************/
//Encoder accuracy
//编码器精度
float Encoder_precision;
//Wheel circumference, unit: m
//轮子周长，单位：m
float Wheel_perimeter;
//Drive wheel base, unit: m
//主动轮轮距，单位：m
float Wheel_spacing;
//The wheelbase of the front and rear axles of the trolley, unit: m
//小车前后轴的轴距，单位：m
float Axle_spacing;
//All-directional wheel turning radius, unit: m
//全向轮转弯半径，单位：m
float Omni_turn_radiaus;
/************ 小车型号相关变量 **************************/
/************ Variables related to car model ************/
//PS2 controller, Bluetooth APP, aircraft model controller, CAN communication, serial port 1, serial port 5 communication control flag bit.
//These 6 flag bits are all 0 by default, representing the serial port 3 control mode
//PS2手柄、蓝牙APP、航模手柄、CAN通信、串口1、串口5通信控制标志位。这6个标志位默认都为0，代表串口3控制模式
u8 PS2_ON_Flag=0, APP_ON_Flag=0, Remote_ON_Flag=0, CAN_ON_Flag=0, Usart1_ON_Flag, Usart5_ON_Flag;
//Bluetooth remote control associated flag bits
//蓝牙遥控相关的标志位
u8 Flag_Left, Flag_Right, Flag_Direction=0, Turn_Flag;
//Sends the parameter's flag bit to the Bluetooth APP
//向蓝牙APP发送参数的标志位
u8 PID_Send;
//The PS2 gamepad controls related variables
//PS2手柄控制相关变量
float PS2_LX,PS2_LY,PS2_RX,PS2_RY,PS2_KEY;
//Self-check the relevant flag variables
//自检相关标志变量
int Check=0, Checking=0, Checked=0, CheckCount=0, CheckPhrase1=0, CheckPhrase2=0;
//Check the result code
//自检结果代码
long int ErrorCode=0;
void systemInit(void)
{
// //Interrupt priority group setti ng
// //中断优先级分组设置
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);
//
// //Delay function initialization
// //延时函数初始化
delay_init(168);
//Initialize the hardware interface connected to the LED lamp
//初始化与LED灯连接的硬件接口
LED_Init();
//Initialize the hardware interface connected to the buzzer
//初始化与蜂鸣器连接的硬件接口
Buzzer_Init();
//Initialize the hardware interface connected to the enable switch
//初始化与使能开关连接的硬件接口
Enable_Pin();
//Initialize the hardware interface connected to the OLED display
//初始化与OLED显示屏连接的硬件接口
OLED_Init();
//Initialize the hardware interface connected to the user's key
//初始化与用户按键连接的硬件接口
KEY_Init();
//Serial port 1 initialization, communication baud rate 115200,
//can be used to communicate with ROS terminal
//串口1初始化，通信波特率115200，可用于与ROS端通信
uart1_init(115200);
//Serial port 2 initialization, communication baud rate 9600,
//used to communicate with Bluetooth APP terminal
//串口2初始化，通信波特率9600，用于与蓝牙APP端通信
uart2_init(9600);
//Serial port 3 is initialized and the baud rate is 115200.
//Serial port 3 is the default port used to communicate with ROS terminal
//串口3初始化，通信波特率115200，串口3为默认用于与ROS端通信的串口
uart3_init(115200);
//Serial port 5 initialization, communication baud rate 115200,
//can be used to communicate with ROS terminal
//串口5初始化，通信波特率115200，可用于与ROS端通信
uart5_init(115200);
//ADC pin initialization, used to read the battery voltage and potentiometer gear,
//potentiometer gear determines the car after the boot of the car model
//ADC引脚初始化，用于读取电池电压与电位器档位，电位器档位决定小车开机后的小车适配型号
Adc_Init();
Adc_POWER_Init();
//Initialize the CAN communication interface
//CAN通信接口初始化
CAN1_Mode_Init(1,7,6,3,0);
//According to the tap position of the potentiometer, determine which type of car needs to be matched,
//and then initialize the corresponding parameters
//根据电位器的档位判断需要适配的是哪一种型号的小车，然后进行对应的参数初始化
Robot_Select();
//Encoder A is initialized to read the real time speed of motor C
//编码器A初始化，用于读取电机C的实时速度
Encoder_Init_TIM2();
//Encoder B is initialized to read the real time speed of motor D
//编码器B初始化，用于读取电机D的实时速度
Encoder_Init_TIM3();
//Encoder C is initialized to read the real time speed of motor B
//编码器C初始化，用于读取电机B的实时速度
Encoder_Init_TIM4();
//Encoder D is initialized to read the real time speed of motor A
//编码器D初始化，用于读取电机A的实时速度
Encoder_Init_TIM5();
//定时器12用作舵机的PWM接口
TIM12_SERVO_Init(9999,84-1); //APB1的时钟频率为84M , 频率=84M/((9999+1)*(83+1))=100Hz
//普通小车默认定时器8用作航模接口
// TIM8_SERVO_Init(9999,168-1);//APB2的时钟频率为168M , 频率=168M/((9999+1)*(167+1))=100Hz
//Initialize the model remote control interface
//初始化航模遥控接口
TIM8_Cap_Init(9999,168-1); //高级定时器TIM8的时钟频率为168M
//Initialize motor speed control and, for controlling motor speed, PWM frequency 10kHz
//初始化电机速度控制以及，用于控制电机速度，PWM频率10KHZ
//APB2时钟频率为168M，满PWM为16799，频率=168M/((16799+1)*(0+1))=10k
TIM1_PWM_Init(16799,0);
TIM9_PWM_Init(16799,0);
TIM10_PWM_Init(16799,0);
TIM11_PWM_Init(16799,0);
//IIC initialization for MPU6050
//IIC初始化，用于MPU6050
I2C_GPIOInit();
//MPU6050 is initialized to read the vehicle's three-axis attitude,
//three-axis angular velocity and three-axis acceleration information
//MPU6050 初始化，用于读取小车三轴姿态、三轴角速度、三轴加速度信息
MPU6050_initialize();
//Initialize the hardware interface to the PS2 controller
//初始化与PS2手柄连接的硬件接口
PS2_Init();
//PS2 gamepad configuration is initialized and configured in analog mode
//PS2手柄配置初始化,配置为模拟量模式
PS2_SetInit();
}

124
BALANCE/system.h Normal file
View File

@@ -0,0 +1,124 @@
#ifndef __SYSTEM_H
#define __SYSTEM_H
// Refer to all header files you need
//引用所有需要用到的头文件
#include "FreeRTOSConfig.h"
//FreeRTOS相关头文件
//FreeRTOS related header files
#include "FreeRTOS.h"
#include "stm32f4xx.h"
#include "task.h"
#include "queue.h"
#include "timers.h"
#include "semphr.h"
//The associated header file for the peripheral
//外设的相关头文件
#include "sys.h"
#include "delay.h"
#include "usart.h"
#include "balance.h"
#include "led.h"
#include "oled.h"
#include "usart.h"
#include "usartx.h"
#include "adc.h"
#include "can.h"
#include "motor.h"
#include "timer.h"
#include "encoder.h"
#include "show.h"
#include "pstwo.h"
#include "key.h"
#include "robot_select_init.h"
#include "I2C.h"
#include "MPU6050.h"
// Enumeration of car types
//小车型号的枚举定义
typedef enum
{
Mec_Car = 0,
Omni_Car,
Akm_Car,
Diff_Car,
FourWheel_Car,
Tank_Car
} CarMode;
//Motor speed control related parameters of the structure
//电机速度控制相关参数结构体
typedef struct
{
float Encoder; //Read the real time speed of the motor by encoder //编码器数值，读取电机实时速度
float Motor_Pwm; //Motor PWM value, control the real-time speed of the motor //电机PWM数值，控制电机实时速度
float Target; //Control the target speed of the motor //电机目标速度值，控制电机目标速度
float Velocity_KP; //Speed control PID parameters //速度控制PID参数
float Velocity_KI; //Speed control PID parameters //速度控制PID参数
}Motor_parameter;
//Smoothed the speed of the three axes
//平滑处理后的三轴速度
typedef struct
{
float VX;
float VY;
float VZ;
}Smooth_Control;
/****** external variable definition. When system.h is referenced in other C files,
other C files can also use the variable defined by system.c ******/
/****** 外部变量定义，当其它c文件引用system.h时，也可以使用system.c定义的变量 ******/
extern u8 Flag_Stop;
extern int Divisor_Mode;
extern u8 Car_Mode;
extern int Servo;
extern float RC_Velocity;
extern float Move_X, Move_Y, Move_Z;
extern float Velocity_KP, Velocity_KI;
extern Smooth_Control smooth_control;
extern Motor_parameter MOTOR_A, MOTOR_B, MOTOR_C, MOTOR_D;
extern float Encoder_precision;
extern float Wheel_perimeter;
extern float Wheel_spacing;
extern float Axle_spacing;
extern float Omni_turn_radiaus;
extern u8 PS2_ON_Flag, APP_ON_Flag, Remote_ON_Flag, CAN_ON_Flag, Usart1_ON_Flag, Usart5_ON_Flag;
extern u8 Flag_Left, Flag_Right, Flag_Direction, Turn_Flag;
extern u8 PID_Send;
extern float PS2_LX,PS2_LY,PS2_RX,PS2_RY,PS2_KEY;
extern int Check, Checking, Checked, CheckCount, CheckPhrase1, CheckPhrase2;
extern long int ErrorCode;
void systemInit(void);
/***Macros define***/ /***宏定义***/
//After starting the car (1000/100Hz =10) for seconds, it is allowed to control the car to move
//开机(1000/100hz=10)秒后才允许控制小车进行运动
#define CONTROL_DELAY 1000
//The number of robot types to determine the value of Divisor_Mode. There are currently 6 car types
//机器人型号数量，决定Divisor_Mode的值，目前有6种小车类型
#define CAR_NUMBER 6
#define RATE_1_HZ 1
#define RATE_5_HZ 5
#define RATE_10_HZ 10
#define RATE_20_HZ 20
#define RATE_25_HZ 25
#define RATE_50_HZ 50
#define RATE_100_HZ 100
#define RATE_200_HZ 200
#define RATE_250_HZ 250
#define RATE_500_HZ 500
#define RATE_1000_HZ 1000
/***Macros define***/ /***宏定义***/
//C library function related header file
//C库函数的相关头文件
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "stdarg.h"
#endif