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.
118 lines
1.9 KiB
118 lines
1.9 KiB
1 week ago
|
/*
|
||
|
|
* robot_state.c
|
||
|
|
*
|
||
|
|
* Created on: Aug 2, 2024
|
||
|
|
* Author: akeguo
|
||
|
|
*/
|
||
|
|
#include "robot_state.h"
|
||
|
|
|
||
|
|
//Manual_State_Entry, Manual_State_Do, Manual_State_Exit
|
||
|
|
|
||
|
|
void Manual_State_Do(void)
|
||
|
|
{
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void LaneChangeDistance_Setting_State_Do(void)
|
||
|
|
{
|
||
|
|
if (GV.DH_CAN.Increase == 1)
|
||
|
|
{
|
||
|
|
GV.LaneChangeDistance++;
|
||
|
|
}
|
||
|
|
if (GV.DH_CAN.Decrease == 1)
|
||
|
|
{
|
||
|
|
GV.LaneChangeDistance--;
|
||
|
|
}
|
||
|
|
if (GV.LaneChangeDistance <= 0)
|
||
|
|
{
|
||
|
|
GV.LaneChangeDistance = 0;
|
||
|
|
}
|
||
|
|
if (GV.LaneChangeDistance >= 4096)
|
||
|
|
{
|
||
|
|
GV.LaneChangeDistance = 4096;
|
||
|
|
}
|
||
|
|
|
||
|
|
CV.LaneChangeDistance = GV.LaneChangeDistance;
|
||
|
|
GF_BSP_EEPROM_Set_CV(CV);
|
||
|
|
}
|
||
|
|
|
||
|
|
void BackWardsDistance_Setting_State_Do(void)
|
||
|
|
{
|
||
|
|
if (GV.DH_CAN.Increase == 1)
|
||
|
|
{
|
||
|
|
GV.BackwardDistance++;
|
||
|
|
}
|
||
|
|
if (GV.DH_CAN.Decrease == 1)
|
||
|
|
{
|
||
|
|
GV.BackwardDistance--;
|
||
|
|
}
|
||
|
|
if (GV.BackwardDistance <= 0)
|
||
|
|
{
|
||
|
|
GV.BackwardDistance = 0;
|
||
|
|
}
|
||
|
|
if (GV.BackwardDistance >= 4096)
|
||
|
|
{
|
||
|
|
GV.BackwardDistance = 4096;
|
||
|
|
}
|
||
|
|
|
||
|
|
CV.BackWardsDistance = GV.BackwardDistance;
|
||
|
|
GF_BSP_EEPROM_Set_CV(CV);
|
||
|
|
}
|
||
|
|
|
||
|
|
void Auto_State_Do(void)
|
||
|
|
{
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void Abnormal_State_Do(void)
|
||
|
|
{
|
||
|
|
LOG("Abnormal_State_Do");
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void Forwards_State_Do(void)
|
||
|
|
{
|
||
|
|
GV.LeftMotor.Target_Velcity = GV.Move_Speed;
|
||
|
|
GV.RightMotor.Target_Velcity = GV.Move_Speed;
|
||
|
|
}
|
||
|
|
|
||
|
|
void Backwards_State_Do(void)
|
||
|
|
{
|
||
|
|
GV.LeftMotor.Target_Velcity = -GV.Move_Speed;
|
||
|
|
GV.RightMotor.Target_Velcity = -GV.Move_Speed;
|
||
|
|
}
|
||
|
|
void TurnLeft_State_Do(void)
|
||
|
|
{
|
||
|
|
GV.LeftMotor.Target_Velcity = -GV.Move_Speed;
|
||
|
|
GV.RightMotor.Target_Velcity = GV.Move_Speed;
|
||
|
|
}
|
||
|
|
void TurnRight_State_Do(void)
|
||
|
|
{
|
||
|
|
GV.LeftMotor.Target_Velcity = GV.Move_Speed;
|
||
|
|
GV.RightMotor.Target_Velcity = -GV.Move_Speed;
|
||
|
|
}
|
||
|
|
void HALT_State_Do(void)
|
||
|
|
{
|
||
|
|
GV.LeftMotor.Target_Velcity = 0;
|
||
|
|
GV.RightMotor.Target_Velcity = 0;
|
||
|
|
}
|
||
|
|
//Swing Motor
|
||
|
|
|
||
|
|
void SwingHALT_State_Do(void)
|
||
|
|
{
|
||
|
|
}
|
||
|
|
void SwingLeft_State_Do(void)
|
||
|
|
{
|
||
|
|
GV.SwingMotor.Target_Position += 100;
|
||
|
|
}
|
||
|
|
void SwingRight_State_Do(void)
|
||
|
|
{
|
||
|
|
GV.SwingMotor.Target_Position -= 100;
|
||
|
|
}
|
||
|
|
|
||
|
|
void OtherMode_State_Do(void)
|
||
|
|
{
|
||
|
|
|
||
|
|
}
|
||
|
|
|