LiZongdi
/
Huangjiabao_backup
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

pid调试暂存

master
Lizongdi 1 month ago
parent
87d23d029f
commit
99592a3466
1 changed files with 57 additions and 18 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 75
      diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c

75
diaoerqiege/BHBF_Robot_Lifting_Lug/Core/Src/robot_state.c
View File

@ -7,6 +7,7 @@
#include "robot_state.h"
#include "bsp_GPIO.h"
#include "msp_Strong_grinding_machine.h"
#include "msp_PID.h"
#define PI 3.1415926
@ -21,18 +22,42 @@ double RP_Speed_Ctrl = 0;
float speedAdj = 0;
#define MOTOR_TO_COUNT 164.433
float Outspeed[2] = {0};
float Inspeed;
float g_zoom = 0.2;
int g_flag = 0;
int g_flaga = 1;
void SetMoveMotorSpeed(double Speed)
void SetMoveMotorSpeed(float Speed)
{
double Inspeed = (Speed*360/100/101/6*3.1415926*0.325);//转换为m/min进入PID
Inspeed = (float)(Speed*360/100/101/6*3.1415926f*0.325f);//转换为m/min进入PID
float robotRoll = (float)GV.TL720DParameters.RF_Angle_Roll / 100.0f;
TwoWheel_AngleControl(robotRoll, 0.0f, Inspeed, g_zoom, Outspeed);
float Outspeed[2] = {0};
TwoWheel_AngleControl(GV.TL720DParameters.RF_Angle_Roll, 0, Inspeed, 1, Outspeed);
int a = 0;
int b = 1;
GV.LeftFrontMotor.Target_Velcity = Outspeed[0] * MOTOR_TO_COUNT;
GV.RightFrontMotor.Target_Velcity = Outspeed[1] * MOTOR_TO_COUNT;
GV.LeftBackMotor.Target_Velcity = Outspeed[0] * MOTOR_TO_COUNT;
GV.RightBackMotor.Target_Velcity = Outspeed[1] * MOTOR_TO_COUNT;
if (0 == g_flaga)
{
b = 0;
a = 1;
}
if (0 == g_flag)
{
GV.LeftFrontMotor.Target_Velcity = Outspeed[a] * MOTOR_TO_COUNT;
GV.RightFrontMotor.Target_Velcity = -Outspeed[b] * MOTOR_TO_COUNT;
GV.LeftBackMotor.Target_Velcity = Outspeed[a] * MOTOR_TO_COUNT;
GV.RightBackMotor.Target_Velcity = -Outspeed[b] * MOTOR_TO_COUNT;
}
else
{
GV.LeftFrontMotor.Target_Velcity = 0;
GV.RightFrontMotor.Target_Velcity = 0;
GV.LeftBackMotor.Target_Velcity = 0;
GV.RightBackMotor.Target_Velcity = 0;
}
}
void Manual_State_Do(void)
@ -122,10 +147,10 @@ void Manual_State_Do(void)
R2_Speed_Con = 0;
L2_Speed_Con = 0;
}
GV.LeftFrontMotor.Target_Velcity = L1_Speed_Con * Ref_Speed;
GV.RightFrontMotor.Target_Velcity = R1_Speed_Con * Ref_Speed;
GV.LeftBackMotor.Target_Velcity = L2_Speed_Con * Ref_Speed;
GV.RightBackMotor.Target_Velcity = R2_Speed_Con * Ref_Speed;
GV.LeftFrontMotor.Target_Velcity = L1_Speed_Con * Ref_Speed;
GV.RightFrontMotor.Target_Velcity = R1_Speed_Con * Ref_Speed;
GV.LeftBackMotor.Target_Velcity = L2_Speed_Con * Ref_Speed;
GV.RightBackMotor.Target_Velcity = R2_Speed_Con * Ref_Speed;
}
void low_Speed_Manual_State_Do(void)
@ -406,10 +431,17 @@ void Strong_Grinding_Machine_Motion_State_Do(void)
void auto_forward_state_do(void)
{
if((GV.MK32_Key.CH10_LD1 >= -1000) && (GV.MK32_Key.CH10_LD1 <= 1000))
if((GV.MK32_Key.CH10_LD1 >= -1000) && (GV.MK32_Key.CH10_LD1 <= 0))
{
speedAdj = -995.0f + (GV.MK32_Key.CH10_LD1 + 1000.0f) * 0.005f;
Speed_Ctrl = ((speedAdj + 1000) * 360 / 1000);
Speed_Ctrl = (GV.MK32_Key.CH10_LD1 + 1000) * 360 / 1000;
}
else {
Speed_Ctrl = GV.MK32_Key.CH10_LD1 * (4400 - 360) / 1000 + 360;
}
if (GV.MK32_Key.CH4_SA == -1000)
{
Speed_Ctrl = 15;
}
Ref_Speed = Speed_Ctrl;
@ -421,10 +453,17 @@ void auto_forward_state_do(void)
void auto_backward_state_do(void)
{
if((GV.MK32_Key.CH10_LD1 >= -1000) && (GV.MK32_Key.CH10_LD1 <= 1000))
if((GV.MK32_Key.CH10_LD1 >= -1000) && (GV.MK32_Key.CH10_LD1 <= 0))
{
speedAdj = -995.0f + (GV.MK32_Key.CH10_LD1 + 1000.0f) * 0.005f;
Speed_Ctrl = ((speedAdj + 1000) * 360 / 1000);
Speed_Ctrl = (GV.MK32_Key.CH10_LD1 + 1000) * 360 / 1000;
}
else {
Speed_Ctrl = GV.MK32_Key.CH10_LD1 * (4400 - 360) / 1000 + 360;
}
if (GV.MK32_Key.CH4_SA == -1000)
{
Speed_Ctrl = 15;
}
Ref_Speed = Speed_Ctrl;

Write Preview
Loading…
Cancel
Save