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.
167 lines
2.6 KiB
167 lines
2.6 KiB
/*
|
|
* robot_state.c
|
|
*
|
|
* Created on: Aug 2, 2024
|
|
* Author: akeguo
|
|
*/
|
|
#include "robot_state.h"
|
|
#include "fsm.h"
|
|
|
|
#include "BSP/bsp_GPIO.h"
|
|
#include "MSP/msp_PID.h"
|
|
|
|
|
|
int32_t *Robot_Platform_Speed[10]; //目标速度
|
|
|
|
int32_t *DHRoughening_LaneChangeDistance[10]; //目标速度
|
|
|
|
int32_t *Robot_LeftCompensation;
|
|
int32_t *Robot_RightCompensation;
|
|
int32_t *Roughenging_LeftCompensation_Range;
|
|
int32_t *Robot_RightCompensation_Range;
|
|
int32_t *Robot_Angle_Start_BASE;
|
|
int32_t *Robot_Angle;
|
|
int32_t *Robot_Deri_Speed;
|
|
|
|
|
|
double Angle_Error_LLL=0;
|
|
int CRLU_Flag=0;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
//Manual_State_Entry, Manual_State_Do, Manual_State_Exit
|
|
void Calbrate_Robot_Positon(int position);
|
|
int32_t Horizontal_Target_Angle = 0;
|
|
|
|
void Move_Forwards_Do(int32_t Target_Angle);
|
|
void Move_Backwards_Do(int32_t Target_Angle);
|
|
|
|
void Move_PushRod_Halt_Func_Do(void)
|
|
{
|
|
GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 1);
|
|
GF_BSP_GPIO_SetIO(3, 1);
|
|
}
|
|
void Move_PushRod_Up_Func_Do(void)
|
|
{
|
|
GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 0);
|
|
GF_BSP_GPIO_SetIO(3, 1);
|
|
|
|
}
|
|
void Move_PushRod_Down_Func_Do(void)
|
|
{
|
|
GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 1);
|
|
GF_BSP_GPIO_SetIO(3, 0);
|
|
}
|
|
|
|
void Move_PushRod_Halt_Func_Do_1(void)
|
|
{
|
|
GF_BSP_GPIO_SetIO(4, 1);
|
|
GF_BSP_GPIO_SetIO(5, 1);
|
|
}
|
|
|
|
void Move_PushRod_Up_Func_Do_1(void)
|
|
{
|
|
GF_BSP_GPIO_SetIO(4, 0);
|
|
GF_BSP_GPIO_SetIO(5, 1);
|
|
|
|
}
|
|
|
|
void Move_PushRod_Down_Func_Do_1(void)
|
|
{
|
|
GF_BSP_GPIO_SetIO(4, 1);
|
|
GF_BSP_GPIO_SetIO(5, 0);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////////////////////
|
|
void Abnormal_State_Do(void)
|
|
{
|
|
LOG("Abnormal_State_Do");
|
|
|
|
}
|
|
|
|
|
|
|
|
void HALT_State_Do(void)
|
|
{
|
|
GV.LeftMotor.Target_Velcity = 0;
|
|
GV.RightMotor.Target_Velcity = 0;
|
|
}
|
|
|
|
|
|
char is_within_the_target_angle_flag =0;
|
|
//水平方向前进，保持水平方向
|
|
|
|
//水平方向后退，保持水平方向
|
|
|
|
|
|
|
|
/* 终端上下移动，在洗舱里是抬头低头，这里是上下，直接沿用Tilt Up 和 TiltDown函数
|
|
* */
|
|
void TiltUp_Do()
|
|
{
|
|
// GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 0);
|
|
// GF_BSP_GPIO_SetIO(Lifting_IO_CTL_1, 1);
|
|
|
|
GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 1);
|
|
GF_BSP_GPIO_SetIO(Lifting_IO_CTL_1, 0);
|
|
}
|
|
|
|
void TiltDown_Do()
|
|
{
|
|
|
|
// if (GV.ForceValue >= CV.PV.EndPushForceValue)
|
|
// {
|
|
// GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 1);
|
|
// GF_BSP_GPIO_SetIO(Lifting_IO_CTL_1, 1);
|
|
// }
|
|
// else
|
|
// {
|
|
// GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 1);
|
|
// GF_BSP_GPIO_SetIO(Lifting_IO_CTL_1, 0);
|
|
|
|
GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 0);
|
|
GF_BSP_GPIO_SetIO(Lifting_IO_CTL_1, 1);
|
|
// }
|
|
}
|
|
|
|
void TiltHalt_Do()
|
|
{
|
|
GF_BSP_GPIO_SetIO(Lifting_IO_CTL_0, 1);
|
|
GF_BSP_GPIO_SetIO(Lifting_IO_CTL_1, 1);
|
|
}
|
|
|
|
/* 拉毛函数控制
|
|
* */
|
|
|
|
|
|
|
|
|