YuLin_Zhen
/
ZhenYulin
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
5 Commits
2 Branches
0 Tags
8.6 MiB
 
 
Tree: 2a146842bf
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '2a146842bf'
${ noResults }
ZhenYulin/Core/Src/robot_state.c

651 lines
18 KiB
Raw Blame History

/*
* robot_state.c
*
* Created on: Aug 2, 2024
* Author: akeguo
*/
#include "robot_state.h"
#include "bsp_GPIO.h"
#include <stdio.h>
#include <math.h>
//Manual_State_Entry, Manual_State_Do, Manual_State_Exit
int Swing_Count=0;
int Home_Flag=0;
ChgRoad_Parameters Chg_Pa =
{
.KP11 = 2,
.KP22 = 1.75,
.KP33 = 1.75,
.KP44 = 1.25,
.KPCR1 = 16,
.KDCR1 = 13,
.change_road_status_flag=0, //换道标志位
.change_road_straight_count=0,
.change_road_distance=12, //换道时的距离（执行换道前进程序的次数，可以理解为执行次数越多，机器人走的越远）
.speed = 3,
/**转弯*/
.isTurn = false,
.Ini_Postition_L = 0,
.Final_Postition_L = 0,
.Ini_Postition_R = 0,
.Final_Postition_R = 0,
.ChgWidth = 0,
.change_road_finish_flag = 0
};
int32_t compensate_angle=0; //纠偏补偿角，客户端提供的
int32_t change_road_speed=200; //换道时的速度（RPM） 200/70*0.942=2.69m/min
AutoMove_Parameters AM_Pa ={
/**直线纠偏*/
.KP1 = 2,
.KP2 = 1,
.KP3 = 0.5,
.KP4 = 0.25,
.KPFB1 = 50,
.KDFB1 = 50,
.deltaAngle1 = 5,
.deltaAngle2 = 10,
.deltaAngle3 = 30,
.ExpectationAngle=0
};
int* AuTo_Flag;
double speed[11] ={ 0, 0.1, 0.3, 0.5, 1, 3, 5, 10, 15, 20, 22 };
double speed_Turn = 2.5;
// <summary>
/// 驱动轮减速比
/// </summary>
double DrivingWheelRatio = 80;//mm
/// <summary>
/// 驱动轮轮径 mm
/// </summary>
double DrivingWheelDiameter = 230;//mm
/// <summary>
/// 驱动轮速比
/// </summary>
double DrivingWheelSpeedRatio = 0;
/// <summary>
/// 位置系数
/// </summary>
double DrivingWheelPositionRatio = 0;
/// <summary>
/// 电机转速r/min与轮上线速度m/min的速比
/// </summary>
void WheelMotorSpeedInit()
{
DrivingWheelSpeedRatio = 6 * 100 * (DrivingWheelRatio + 1) / 360 / 3.14 / DrivingWheelDiameter / 0.001;
DrivingWheelPositionRatio = (int)(((65536 * DrivingWheelRatio / (DrivingWheelDiameter * 3.14159)) / 0.00001) * 0.00001);
}
/// <summary>
/// 轮上线速度m/min求电机转速r/min
/// </summary>
/// <param name="speed">轮上线速度 m/min</param>
/// <returns>电机转速 r/min</returns>
int32_t WheelMotorSpeedGet(double speed)
{
int32_t motorSpeed = (int32_t)(speed * DrivingWheelSpeedRatio);
return motorSpeed;
}
/// <summary>
/// 下发参数根据实际输出距离求得
/// </summary>
/// <param name="Position">下发参数pulse</param>
/// <returns>电机转速 r/min</returns>
int32_t WheelMotorPositionGet(double Length)
{
int32_t motorPosition = (int32_t)(Length*DrivingWheelPositionRatio);
return motorPosition;
}
void Manual_State_Do(void)
{
}
void Auto_State_Do(void)
{
}
void Abnormal_State_Do(void)
{
LOG("Abnormal_State_Do");
}
void Forwards_State_Do(void)
{
GV.LeftFrontMotor.Target_Velcity = GV.Robot_ManualSpeed;
GV.RightFrontMotor.Target_Velcity = -GV.Robot_ManualSpeed;
}
void Backwards_State_Do(void)
{
GV.LeftFrontMotor.Target_Velcity = -GV.Robot_ManualSpeed;
GV.RightFrontMotor.Target_Velcity = GV.Robot_ManualSpeed;
}
void TurnLeft_State_Do(void)
{
GV.LeftFrontMotor.Target_Velcity = -CV.TurnLeftRightSpeed;
GV.RightFrontMotor.Target_Velcity = -CV.TurnLeftRightSpeed;
}
void TurnRight_State_Do(void)
{
GV.LeftFrontMotor.Target_Velcity =CV.TurnLeftRightSpeed;
GV.RightFrontMotor.Target_Velcity = CV.TurnLeftRightSpeed;
}
void HALT_State_Do(void)
{
Chg_Pa.change_road_status_flag = 0;
GV.LeftFrontMotor.Target_Velcity = 0;
GV.RightFrontMotor.Target_Velcity = 0;
}
void continueTurn_up(double Angle, double CurrentAngle)
{
double offset = 0.5;
if (CurrentAngle >= Angle - offset && CurrentAngle <= Angle + offset)
{
Chg_Pa.isTurn = true;
}
}
void Auto()
{
double angle = GV.Robot_Angle.RF_Angle_Roll / 100.0;
switch(*AuTo_Flag)
{
case 1:
//水平
if ((angle >= 45 && angle <= 135))
{
AM_Pa.ExpectationAngle = 90 - GV.Right_Compensation;
}
else if ((angle >= -135 && angle <= -45))
{
AM_Pa.ExpectationAngle = -90 + GV.Left_Compensation;
}
//垂直
else if ((angle > -45 && angle < 45))
{
AM_Pa.ExpectationAngle = 0;
}
VehicleAutoTurn((angle), AM_Pa.ExpectationAngle, Chg_Pa.KPCR1, Chg_Pa.KDCR1); //没到-90左转
continueTurn_up(AM_Pa.ExpectationAngle, (angle));
if (Chg_Pa.isTurn)
{
*AuTo_Flag=2;
GV.LeftFrontMotor.Target_Velcity=0;
GV.RightFrontMotor.Target_Velcity=0;
Chg_Pa.isTurn=false;//换完置位
}
break;
case 2:
VehicleAutoForward(angle,
GV.PV.Robot_LeftCompensation, GV.PV.Robot_RightCompensation);//自动前进
break;
case 3:
//水平
if ((angle >= 45 && angle <= 135))
{
AM_Pa.ExpectationAngle = 90 + GV.Left_Compensation;
}
if ((angle >= -135 && angle <= -45))
{
AM_Pa.ExpectationAngle = - 90 - GV.Right_Compensation;
}
//垂直
if ((angle > -45 && angle < 45))
{
AM_Pa.ExpectationAngle = 0;
}
VehicleAutoTurn((angle), AM_Pa.ExpectationAngle, Chg_Pa.KPCR1, Chg_Pa.KDCR1); //没到-90左转
continueTurn_up(AM_Pa.ExpectationAngle, (angle));
if (Chg_Pa.isTurn)
{
*AuTo_Flag=4;
GV.LeftFrontMotor.Target_Velcity=0;
GV.RightFrontMotor.Target_Velcity=0;
Chg_Pa.isTurn=false;//换完置位
}
break;
case 4:
VehicleAutoBackward(angle,
GV.PV.Robot_LeftCompensation, GV.PV.Robot_RightCompensation);//自动后退
break;
}
}
void VehicleAutoForward(double InclinometerAngle, double offsetAngle_left, double offsetAngle_right)
{
double deltaAngle=InclinometerAngle-AM_Pa.ExpectationAngle;
int32_t deltaSpeed=PositionalPID(AM_Pa.ExpectationAngle, InclinometerAngle, AM_Pa.KPFB1, AM_Pa.KDFB1);
if(deltaAngle<0)
{
deltaAngle=-deltaAngle;
}
GV.LeftFrontMotor.Target_Velcity = GV.Robot_AutoSpeed;
GV.RightFrontMotor.Target_Velcity = -GV.Robot_AutoSpeed;
GV.LeftFrontMotor.Target_Velcity=GV.LeftFrontMotor.Target_Velcity+AM_Pa.KP1 * deltaSpeed;
GV.RightFrontMotor.Target_Velcity=GV.RightFrontMotor.Target_Velcity+AM_Pa.KP1 * deltaSpeed;
// if (deltaAngle >= 0 && deltaAngle <= AM_Pa.deltaAngle1)
// {
// Motor[0].Target_Velcity = Motor[0].Target_Velcity + AM_Pa.KP1 * deltaSpeed;
// Motor[1].Target_Velcity = Motor[1].Target_Velcity - AM_Pa.KP1 * deltaSpeed;
// }
// else if (deltaAngle > AM_Pa.deltaAngle1 && deltaAngle <= AM_Pa.deltaAngle2)
// {
// Motor[0].Target_Velcity = Motor[0].Target_Velcity + AM_Pa.KP2 * deltaSpeed;
// Motor[1].Target_Velcity = Motor[1].Target_Velcity - AM_Pa.KP2 * deltaSpeed;
// }
// else if (deltaAngle > AM_Pa.deltaAngle2 && deltaAngle <= AM_Pa.deltaAngle3)
// {
// Motor[0].Target_Velcity = Motor[0].Target_Velcity + AM_Pa.KP3 * deltaSpeed;
// Motor[1].Target_Velcity = Motor[1].Target_Velcity - AM_Pa.KP3 * deltaSpeed;
// }
// else if (deltaAngle > AM_Pa.deltaAngle3 && deltaAngle <= 90)
// {
// Motor[0].Target_Velcity = Motor[0].Target_Velcity + AM_Pa.KP4 * deltaSpeed;
// Motor[1].Target_Velcity = Motor[1].Target_Velcity - AM_Pa.KP4 * deltaSpeed;
// }
}
void VehicleAutoBackward(double InclinometerAngle, double offsetAngle_left, double offsetAngle_right)
{
double deltaAngle=InclinometerAngle-AM_Pa.ExpectationAngle;
double deltaSpeed=PositionalPID(AM_Pa.ExpectationAngle, InclinometerAngle, AM_Pa.KPFB1, AM_Pa.KDFB1);
if(deltaAngle<0)
{
deltaAngle=-deltaAngle;
}
GV.LeftFrontMotor.Target_Velcity = -GV.Robot_AutoSpeed;
GV.RightFrontMotor.Target_Velcity = GV.Robot_AutoSpeed;
//
GV.LeftFrontMotor.Target_Velcity=GV.LeftFrontMotor.Target_Velcity+AM_Pa.KP1 * deltaSpeed;
GV.RightFrontMotor.Target_Velcity=GV.RightFrontMotor.Target_Velcity+AM_Pa.KP1 * deltaSpeed;
// if (deltaAngle >= 0 && deltaAngle <= AM_Pa.deltaAngle1)
// {
// Motor[0].Target_Velcity = Motor[0].Target_Velcity + AM_Pa.KP1 * deltaSpeed;
// Motor[1].Target_Velcity = Motor[1].Target_Velcity - AM_Pa.KP1 * deltaSpeed;
// }
// else if (deltaAngle > AM_Pa.deltaAngle1 && deltaAngle <= AM_Pa.deltaAngle2)
// {
// Motor[0].Target_Velcity = Motor[0].Target_Velcity + AM_Pa.KP2 * deltaSpeed;
// Motor[1].Target_Velcity = Motor[1].Target_Velcity - AM_Pa.KP2 * deltaSpeed;
// }
// else if (deltaAngle > AM_Pa.deltaAngle2 && deltaAngle <= AM_Pa.deltaAngle3)
// {
// Motor[0].Target_Velcity = Motor[0].Target_Velcity + AM_Pa.KP3 * deltaSpeed;
// Motor[1].Target_Velcity = Motor[1].Target_Velcity - AM_Pa.KP3 * deltaSpeed;
// }
// else if (deltaAngle > AM_Pa.deltaAngle3 && deltaAngle <= 90)
// {
// Motor[0].Target_Velcity = Motor[0].Target_Velcity + AM_Pa.KP4 * deltaSpeed;
// Motor[1].Target_Velcity = Motor[1].Target_Velcity - AM_Pa.KP4 * deltaSpeed;
// }
}
void VehicleAutoTurn(double InclinometerAngle, double expectationAngle, double Kp, double Kd)//换道自动转弯
{
double deltaSpeed = 0;
deltaSpeed = PositionalPID(expectationAngle, InclinometerAngle, Kp, Kd); //水平纠偏
double deltaAngle = 0;
deltaAngle = InclinometerAngle - expectationAngle;
if(deltaAngle<0)//取角度差值的绝对值
{
deltaAngle=-deltaAngle;
}
if(deltaSpeed >= 400)
{
deltaSpeed = 400;
}
GV.LeftFrontMotor.Target_Velcity=0;
GV.RightFrontMotor.Target_Velcity=0;
GV.LeftFrontMotor.Target_Velcity= GV.LeftFrontMotor.Target_Velcity + deltaSpeed;
GV.RightFrontMotor.Target_Velcity= GV.RightFrontMotor.Target_Velcity + deltaSpeed;
}
void ChgUp()
{
//上换道
double angle;
angle = GV.Robot_Angle.RF_Angle_Roll / 100.0;
if(Chg_Pa.change_road_finish_flag==0)
{
switch(Chg_Pa.change_road_status_flag)
{
case 0:
if(angle >= -45
&& (angle <= 45))
{
Chg_Pa.change_road_status_flag=1;
GV.Chg_Flag = 0;
Chg_Pa.ChgWidth = 200-10;
WheelMotorSpeedInit();
}
break;
case 1:
VehicleAutoTurn((angle), 90, Chg_Pa.KPCR1, Chg_Pa.KDCR1); //没到-90左转
continueTurn_up(90, (angle));
if (Chg_Pa.isTurn)
{
Chg_Pa.change_road_status_flag=2;
GV.LeftFrontMotor.Target_Velcity=0;
GV.RightFrontMotor.Target_Velcity=0;
Chg_Pa.isTurn=false;//换完置位
//记录电机换道位置
Chg_Pa.Ini_Postition_L = GV.LeftFrontMotor.Position;
Chg_Pa.Final_Postition_L = GV.LeftFrontMotor.Position - WheelMotorPositionGet(Chg_Pa.ChgWidth);
Chg_Pa.Ini_Postition_L = GV.RightFrontMotor.Position;
Chg_Pa.Final_Postition_R = GV.RightFrontMotor.Position + WheelMotorPositionGet(Chg_Pa.ChgWidth);
}
break;
case 2: //到了90°之后就开始水平后退
GV.LeftFrontMotor.Target_Velcity= -WheelMotorSpeedGet(Chg_Pa.speed);
GV.RightFrontMotor.Target_Velcity= WheelMotorSpeedGet(Chg_Pa.speed);
int m = Chg_Pa.Final_Postition_L - GV.LeftFrontMotor.Position;
int n = Chg_Pa.Final_Postition_R - GV.RightFrontMotor.Position;
if (m > 0 && n < 0)
{
Chg_Pa.change_road_status_flag=3;
Chg_Pa.change_road_straight_count=0;
GV.LeftFrontMotor.Target_Velcity=0;
GV.RightFrontMotor.Target_Velcity=0;
}
break;
case 3:
VehicleAutoTurn((angle),0,Chg_Pa.KPCR1,Chg_Pa.KDCR1); //转回来，没到0右转
continueTurn_up(0, (angle));
if (Chg_Pa.isTurn)
{
Chg_Pa.change_road_status_flag=0;
GV.LeftFrontMotor.Target_Velcity=0;
GV.RightFrontMotor.Target_Velcity=0;
Chg_Pa.change_road_finish_flag=1;
GV.Chg_Flag = 1;
Chg_Pa.isTurn=false;
}
break;
default:
break;
}
}
}
void ChgDown()
{
//上换道
double angle;
angle = GV.Robot_Angle.RF_Angle_Roll / 100.0;
if(Chg_Pa.change_road_finish_flag==0)
{
switch(Chg_Pa.change_road_status_flag)
{
case 0:
if(angle >= -45
&& (angle <= 45))
{
Chg_Pa.change_road_status_flag=1;
GV.Chg_Flag = 0;
Chg_Pa.ChgWidth = 200-10;
WheelMotorSpeedInit();
}
break;
case 1:
VehicleAutoTurn((angle), -90, Chg_Pa.KPCR1, Chg_Pa.KDCR1); //没到-90左转
continueTurn_up(-90, (angle));
if (Chg_Pa.isTurn)
{
Chg_Pa.change_road_status_flag=2;
GV.LeftFrontMotor.Target_Velcity=0;
GV.RightFrontMotor.Target_Velcity=0;
Chg_Pa.isTurn=false;//换完置位
//记录电机换道位置
Chg_Pa.Ini_Postition_L = GV.LeftFrontMotor.Position;
Chg_Pa.Final_Postition_L = GV.LeftFrontMotor.Position + WheelMotorPositionGet(Chg_Pa.ChgWidth);
Chg_Pa.Ini_Postition_L = GV.RightFrontMotor.Position;
Chg_Pa.Final_Postition_R = GV.RightFrontMotor.Position - WheelMotorPositionGet(Chg_Pa.ChgWidth);
}
break;
case 2: //到了-90°之后就开始水平前进
GV.LeftFrontMotor.Target_Velcity= WheelMotorSpeedGet(Chg_Pa.speed);
GV.RightFrontMotor.Target_Velcity= -WheelMotorSpeedGet(Chg_Pa.speed);
int m = Chg_Pa.Final_Postition_L - GV.LeftFrontMotor.Position;
int n = Chg_Pa.Final_Postition_R - GV.RightFrontMotor.Position;
if (m < 0 && n > 0)
{
Chg_Pa.change_road_status_flag=3;
Chg_Pa.change_road_straight_count=0;
GV.LeftFrontMotor.Target_Velcity=0;
GV.RightFrontMotor.Target_Velcity=0;
}
break;
case 3:
VehicleAutoTurn((angle),0,Chg_Pa.KPCR1,Chg_Pa.KDCR1); //转回来，没到0右转
continueTurn_up(0, (angle));
if (Chg_Pa.isTurn)
{
Chg_Pa.change_road_status_flag=0;
GV.LeftFrontMotor.Target_Velcity=0;
GV.RightFrontMotor.Target_Velcity=0;
Chg_Pa.change_road_finish_flag=1;
GV.Chg_Flag = 1;
Chg_Pa.isTurn=false;
}
break;
default:
break;
}
}
}
void ChgLeft()
{
//上换道
double angle;
angle = GV.Robot_Angle.RF_Angle_Roll / 100.0;
if(Chg_Pa.change_road_finish_flag==0)
{
switch(Chg_Pa.change_road_status_flag)
{
case 0:
angle = GV.Robot_Angle.RF_Angle_Roll / 100.0;
if((angle >= -135 && angle <= -45)
|| (angle >= 45 && angle <= 135))
{
Chg_Pa.change_road_status_flag=1;
GV.Chg_Flag = 0;
Chg_Pa.ChgWidth = 200-60;
WheelMotorSpeedInit();
}
break;
case 1:
VehicleAutoTurn((angle), 0, Chg_Pa.KPCR1, Chg_Pa.KDCR1); //没到-90左转
continueTurn_up(0, (angle));
if (Chg_Pa.isTurn)
{
Chg_Pa.change_road_status_flag=2;
GV.LeftFrontMotor.Target_Velcity=0;
GV.RightFrontMotor.Target_Velcity=0;
Chg_Pa.isTurn=false;//换完置位
//记录电机换道位置
Chg_Pa.Ini_Postition_L = GV.LeftFrontMotor.Position;
Chg_Pa.Final_Postition_L = GV.LeftFrontMotor.Position - WheelMotorPositionGet(Chg_Pa.ChgWidth - 50);
Chg_Pa.Ini_Postition_L = GV.RightFrontMotor.Position;
Chg_Pa.Final_Postition_R = GV.RightFrontMotor.Position + WheelMotorPositionGet(Chg_Pa.ChgWidth - 50);
}
break;
case 2: //到了90°之后就开始水平后退
GV.LeftFrontMotor.Target_Velcity= -WheelMotorSpeedGet(Chg_Pa.speed);
GV.RightFrontMotor.Target_Velcity= WheelMotorSpeedGet(Chg_Pa.speed);
int m = Chg_Pa.Final_Postition_L - GV.LeftFrontMotor.Position;
int n = Chg_Pa.Final_Postition_R - GV.RightFrontMotor.Position;
if (m > 0 && n < 0)
{
Chg_Pa.change_road_status_flag=3;
Chg_Pa.change_road_straight_count=0;
GV.LeftFrontMotor.Target_Velcity=0;
GV.RightFrontMotor.Target_Velcity=0;
}
break;
case 3:
VehicleAutoTurn((angle),-90,Chg_Pa.KPCR1,Chg_Pa.KDCR1); //转回来，没到0右转
continueTurn_up(-90, (angle));
if (Chg_Pa.isTurn)
{
Chg_Pa.change_road_status_flag=0;
GV.LeftFrontMotor.Target_Velcity=0;
GV.RightFrontMotor.Target_Velcity=0;
Chg_Pa.change_road_finish_flag=1;
GV.Chg_Flag = 1;
Chg_Pa.isTurn=false;
}
break;
default:
break;
}
}
}
void ChgRight()
{
//上换道
double angle;
angle = GV.Robot_Angle.RF_Angle_Roll / 100.0;
if(Chg_Pa.change_road_finish_flag==0)
{
switch(Chg_Pa.change_road_status_flag)
{
case 0:
angle = GV.Robot_Angle.RF_Angle_Roll / 100.0;
if((angle >= -135 && angle <= -45)
|| (angle >= 45 && angle <= 135))
{
Chg_Pa.change_road_status_flag=1;
GV.Chg_Flag = 0;
Chg_Pa.ChgWidth = 200-60;
WheelMotorSpeedInit();
}
break;
case 1:
VehicleAutoTurn((angle), 0, Chg_Pa.KPCR1, Chg_Pa.KDCR1); //没到-90左转
continueTurn_up(0, (angle));
if (Chg_Pa.isTurn)
{
Chg_Pa.change_road_status_flag=2;
GV.LeftFrontMotor.Target_Velcity=0;
GV.RightFrontMotor.Target_Velcity=0;
Chg_Pa.isTurn=false;//换完置位
//记录电机换道位置
Chg_Pa.Ini_Postition_L = GV.LeftFrontMotor.Position;
Chg_Pa.Final_Postition_L = GV.LeftFrontMotor.Position - WheelMotorPositionGet(Chg_Pa.ChgWidth - 50);
Chg_Pa.Ini_Postition_L = GV.RightFrontMotor.Position;
Chg_Pa.Final_Postition_R = GV.RightFrontMotor.Position + WheelMotorPositionGet(Chg_Pa.ChgWidth - 50);
}
break;
case 2: //到了90°之后就开始水平后退
GV.LeftFrontMotor.Target_Velcity= -WheelMotorSpeedGet(Chg_Pa.speed);
GV.RightFrontMotor.Target_Velcity= WheelMotorSpeedGet(Chg_Pa.speed);
int m = Chg_Pa.Final_Postition_L - GV.LeftFrontMotor.Position;
int n = Chg_Pa.Final_Postition_R - GV.RightFrontMotor.Position;
if (m > 0 && n < 0)
{
Chg_Pa.change_road_status_flag=3;
Chg_Pa.change_road_straight_count=0;
GV.LeftFrontMotor.Target_Velcity=0;
GV.RightFrontMotor.Target_Velcity=0;
}
break;
case 3:
VehicleAutoTurn((angle),90,Chg_Pa.KPCR1,Chg_Pa.KDCR1); //转回来，没到0右转
continueTurn_up(90, (angle));
if (Chg_Pa.isTurn)
{
Chg_Pa.change_road_status_flag=0;
GV.LeftFrontMotor.Target_Velcity=0;
GV.RightFrontMotor.Target_Velcity=0;
Chg_Pa.change_road_finish_flag=1;
GV.Chg_Flag = 1;
Chg_Pa.isTurn=false;
}
break;
default:
break;
}
}
}
void ChgFinish()
{
GV.LeftFrontMotor.Target_Velcity = 0;
GV.RightFrontMotor.Target_Velcity = 0;
}