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.
145 lines
2.9 KiB
145 lines
2.9 KiB
/*
|
|
* 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_Exit(void)
|
|
{
|
|
LOG("Manual_State_Exit");
|
|
}
|
|
|
|
void Manual_State_Entry(void)
|
|
{
|
|
LOG("Manual_Operation_Entry");
|
|
}
|
|
void Manual_State_Do(void)
|
|
{
|
|
//LOG("Manual_State_Do");
|
|
|
|
double Wheel_Speed_Max=3000;
|
|
double Turning_speed=1000;
|
|
|
|
double Rocker_Range=1000;
|
|
|
|
double Speed_Scale=1;
|
|
double Rocker_factor=1;
|
|
|
|
double X_Value_1=0;
|
|
double Y_Value_1=0;
|
|
double X_Value_2=0;
|
|
double Y_Value_2=0;
|
|
|
|
double R_Speed_Con=0;
|
|
double L_Speed_Con=0;
|
|
|
|
int32_t Sum_T=GV.MK32_Key.CH11_RD1+1000;
|
|
Speed_Scale=((double)Sum_T)/2000;
|
|
|
|
X_Value_1=GV.MK32_Key.CH0_RY_H+0.00001;
|
|
Y_Value_1=GV.MK32_Key.CH1_RY_V+0.00001;
|
|
//Normalization processing
|
|
X_Value_2=X_Value_1/Rocker_Range;
|
|
Y_Value_2=Y_Value_1/Rocker_Range;
|
|
|
|
double Rocker_angle=atan(Y_Value_2/X_Value_2);
|
|
double Vector_modulus=sqrt(X_Value_2*X_Value_2+Y_Value_2*Y_Value_2);
|
|
|
|
|
|
L_Speed_Con=Y_Value_2*Wheel_Speed_Max-X_Value_2*Turning_speed;
|
|
R_Speed_Con=Y_Value_2*Wheel_Speed_Max+X_Value_2*Turning_speed;
|
|
|
|
|
|
//Adjusting speed based on modulus
|
|
if (Vector_modulus>1)
|
|
{
|
|
Vector_modulus=1;
|
|
}
|
|
else if (Vector_modulus<0.1)
|
|
{
|
|
Vector_modulus=0;
|
|
}
|
|
|
|
L_Speed_Con=L_Speed_Con*Vector_modulus;
|
|
R_Speed_Con=R_Speed_Con*Vector_modulus;
|
|
|
|
|
|
|
|
if (fabs(Rocker_angle>1.4835))
|
|
{
|
|
if (R_Speed_Con>0)
|
|
{ R_Speed_Con=Wheel_Speed_Max*Vector_modulus;
|
|
L_Speed_Con=Wheel_Speed_Max*Vector_modulus;
|
|
}
|
|
else
|
|
{
|
|
R_Speed_Con=-Wheel_Speed_Max*Vector_modulus;
|
|
L_Speed_Con=-Wheel_Speed_Max*Vector_modulus;
|
|
}
|
|
}
|
|
|
|
if (fabs(R_Speed_Con)>Wheel_Speed_Max)
|
|
{
|
|
Rocker_factor=fabs(Wheel_Speed_Max/R_Speed_Con);
|
|
}
|
|
else if (fabs(L_Speed_Con)>Wheel_Speed_Max)
|
|
{
|
|
Rocker_factor=fabs(Wheel_Speed_Max/L_Speed_Con);
|
|
}
|
|
else
|
|
{
|
|
Rocker_factor=1;
|
|
}
|
|
|
|
GV.Motor_P1.Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
|
|
GV.Motor_P2.Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
|
|
GV.Motor_P3.Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
|
|
GV.Motor_P4.Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
|
|
|
|
GV.Motor_P1.Target_Velcity= Speed_Scale*Rocker_factor*R_Speed_Con;
|
|
GV.Motor_P2.Target_Velcity=-Speed_Scale*Rocker_factor*L_Speed_Con;
|
|
GV.Motor_P3.Target_Velcity= Speed_Scale*Rocker_factor*R_Speed_Con;
|
|
GV.Motor_P4.Target_Velcity=-Speed_Scale*Rocker_factor*L_Speed_Con;
|
|
|
|
}
|
|
|
|
|
|
void Halt_State_Entry(void)
|
|
{
|
|
LOG("Halt_State_Entry");
|
|
}
|
|
void Halt_State_Do(void)
|
|
{
|
|
|
|
|
|
GV.Motor_P1.Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
|
|
GV.Motor_P1.Target_Velcity=0;
|
|
GV.Motor_P2.Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
|
|
GV.Motor_P2.Target_Velcity=0;
|
|
GV.Motor_P3.Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
|
|
GV.Motor_P3.Target_Velcity=0;
|
|
GV.Motor_P4.Run_Mode=DF_MSP_Ti5Motor_SpeedMode;
|
|
GV.Motor_P4.Target_Velcity=0;
|
|
}
|
|
void Halt_State_Exit(void)
|
|
{
|
|
LOG("Halt_State_Exit");
|
|
}
|
|
|
|
void Abnormal_State_Entry(void)
|
|
{
|
|
LOG("Halt_State_Entry");
|
|
}
|
|
void Abnormal_State_Do(void)
|
|
{
|
|
LOG("Abnormal_State_Do");
|
|
|
|
}
|
|
void Abnormal_State_Exit(void)
|
|
{
|
|
LOG("Abnormal_State_Exit");
|
|
}
|
|
|