4.4

Luo25177 · Apr 4, 2024 · 670dff5 · 670dff5
1 parent 84585f2
commit 670dff5
Show file tree

Hide file tree

Showing 15 changed files with 427 additions and 649 deletions.
diff --git a/.clang-format b/.clang-format
@@ -1,168 +1,66 @@
----
-Language: Cpp
-BasedOnStyle:  WebKit
-AccessModifierOffset: 0
+# Generated from CLion C/C++ Code Style settings
+BasedOnStyle: LLVM
+AccessModifierOffset: -2
 AlignAfterOpenBracket: Align
-AlignArrayOfStructures: Left
-AlignConsecutiveAssignments:
-    Enabled: true
-    AcrossEmptyLines: true
-    AcrossComments: true
-    AlignCompound: true
-    PadOperators: true
-AlignConsecutiveBitFields:
-    Enabled: true
-    AcrossEmptyLines: true
-    AcrossComments: true
-AlignConsecutiveDeclarations:
-    Enabled: true
-    AcrossEmptyLines: false
-    AcrossComments: true
-AlignConsecutiveMacros:
-    Enabled: true
-    AcrossEmptyLines: false
-    AcrossComments: true
-AlignEscapedNewlines: Left
-AlignOperands: true
-AlignTrailingComments: true
-AllowAllParametersOfDeclarationOnNextLine: true
+AlignConsecutiveAssignments: None
+AlignOperands: Align
+AllowAllArgumentsOnNextLine: false
+AllowAllConstructorInitializersOnNextLine: false
+AllowAllParametersOfDeclarationOnNextLine: false
 AllowShortBlocksOnASingleLine: Always
 AllowShortCaseLabelsOnASingleLine: false
-AllowShortFunctionsOnASingleLine: true
-AllowShortIfStatementsOnASingleLine: Never
-AllowShortLambdasOnASingleLine: true
+AllowShortFunctionsOnASingleLine: All
+AllowShortIfStatementsOnASingleLine: Always
+AllowShortLambdasOnASingleLine: All
 AllowShortLoopsOnASingleLine: true
 AlwaysBreakAfterReturnType: None
-AlwaysBreakBeforeMultilineStrings: true
 AlwaysBreakTemplateDeclarations: Yes
-BinPackArguments: true
-BinPackParameters: true
-BitFieldColonSpacing: Both
 BreakBeforeBraces: Custom
 BraceWrapping:
-  AfterCaseLabel:  false
-  AfterClass:      false
-  AfterControlStatement: false
-  AfterEnum:       false
-  AfterFunction:   false
-  AfterNamespace:  false
-  AfterObjCDeclaration: false
-  AfterStruct:     false
-  AfterUnion:      false
-  AfterExternBlock: false
-  BeforeCatch:     false
-  BeforeElse:      true
-  BeforeLambdaBody: false
-  BeforeWhile:     false
-  IndentBraces:    false
+  AfterCaseLabel: false
+  AfterClass: false
+  AfterControlStatement: Never
+  AfterEnum: false
+  AfterFunction: false
+  AfterNamespace: false
+  AfterUnion: false
+  BeforeCatch: false
+  BeforeElse: false
+  IndentBraces: false
   SplitEmptyFunction: false
-  SplitEmptyRecord: false
-  SplitEmptyNamespace: false
-BreakAfterJavaFieldAnnotations: false
+  SplitEmptyRecord: true
 BreakBeforeBinaryOperators: None
-BreakBeforeBraces: Attach
-BreakBeforeConceptDeclarations: Never
-BreakBeforeInheritanceComma: false
-BreakConstructorInitializers: BeforeComma
-BreakConstructorInitializersBeforeComma: false
-BreakInheritanceList: AfterComma
-BreakBeforeTernaryOperators: false
-BreakStringLiterals: false
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializers: BeforeColon
+BreakInheritanceList: BeforeColon
 ColumnLimit: 0
-CommentPragmas: "^ IWYU pragma:"
-CompactNamespaces: true
-ConstructorInitializerIndentWidth: 2
+CompactNamespaces: false
 ContinuationIndentWidth: 2
-Cpp11BracedListStyle: false
-DerivePointerAlignment: false
-DisableFormat: false
-FixNamespaceComments: true
-ForEachMacros:
-    - for
-IncludeBlocks: Regroup
-IncludeCategories:
-    - Regex: '.*\.generated\.h'
-      Priority: 100
-    - Regex: ".*(PCH).*"
-      Priority: -1
-    - Regex: '".*"'
-      Priority: 1
-    - Regex: '^<.*\.(h)>'
-      Priority: 3
-    - Regex: "^<.*>"
-      Priority: 4
-IncludeIsMainRegex: "([-_](test|unittest))?$"
 IndentCaseLabels: true
 IndentPPDirectives: None
 IndentWidth: 2
-IndentWrappedFunctionNames: false
-IndentExternBlock: AfterExternBlock
-JavaScriptQuotes: Leave
-JavaScriptWrapImports: true
-KeepEmptyLinesAtTheStartOfBlocks: false
-MacroBlockBegin: ""
-MacroBlockEnd: ""
-MaxEmptyLinesToKeep: 1
-NamespaceIndentation: None
-ObjCBinPackProtocolList: Never
-ObjCBlockIndentWidth: 2
+KeepEmptyLinesAtTheStartOfBlocks: true
+MaxEmptyLinesToKeep: 2
+NamespaceIndentation: All
 ObjCSpaceAfterProperty: false
 ObjCSpaceBeforeProtocolList: true
-PenaltyBreakAssignment: 2
-PenaltyBreakBeforeFirstCallParameter: 1
-PenaltyBreakComment: 300
-PenaltyBreakFirstLessLess: 120
-PenaltyBreakString: 1000
-PenaltyBreakTemplateDeclaration: 10
-PenaltyExcessCharacter: 1000000
-PenaltyReturnTypeOnItsOwnLine: 200
-PointerAlignment: Left
-RawStringFormats:
-    - Language: Cpp
-      Delimiters:
-          - cc
-          - CC
-          - cpp
-          - Cpp
-          - CPP
-          - "c++"
-          - "C++"
-      CanonicalDelimiter: ""
-      BasedOnStyle: google
-    - Language: TextProto
-      Delimiters:
-          - pb
-          - PB
-          - proto
-          - PROTO
-      EnclosingFunctions:
-          - EqualsProto
-          - EquivToProto
-          - PARSE_PARTIAL_TEXT_PROTO
-          - PARSE_TEST_PROTO
-          - PARSE_TEXT_PROTO
-          - ParseTextOrDie
-          - ParseTextProtoOrDie
-      CanonicalDelimiter: ""
-ReflowComments: true
-SortIncludes: true
-SortUsingDeclarations: true
+PointerAlignment: Right
+ReflowComments: false
 SpaceAfterCStyleCast: true
-SpaceAfterTemplateKeyword: true
+SpaceAfterLogicalNot: false
+SpaceAfterTemplateKeyword: false
 SpaceBeforeAssignmentOperators: true
 SpaceBeforeCpp11BracedList: false
 SpaceBeforeCtorInitializerColon: true
 SpaceBeforeInheritanceColon: true
-SpaceBeforeParens: true
-SpaceBeforeRangeBasedForLoopColon: true
+SpaceBeforeParens: ControlStatements
+SpaceBeforeRangeBasedForLoopColon: false
 SpaceInEmptyParentheses: false
-SpacesBeforeTrailingComments: 2
+SpacesBeforeTrailingComments: 0
 SpacesInAngles: false
-SpacesInContainerLiterals: true
 SpacesInCStyleCastParentheses: false
+SpacesInContainerLiterals: false
 SpacesInParentheses: false
 SpacesInSquareBrackets: false
-Standard: Auto
 TabWidth: 2
 UseTab: Never
-
diff --git a/Chassis/inc/linetraj.h b/Chassis/inc/linetraj.h
@@ -0,0 +1,22 @@
+#pragma once
+
+#include "robotparam.h"
+#include <math.h>
+
+typedef struct {
+  bool  finish;
+  float xset;
+  float speed;
+  float x;
+  float startSpeed;
+  float uniformSpeed;
+  float endSpeed;
+  float aimx;
+  float speedUpDis;
+  float speedDownDis;
+} LineTraj;
+
+void LineTrajSpeedUp(LineTraj* linetraj);
+void LineTrajSpeedDown(LineTraj* linetraj);
+void LineTrajRun(LineTraj* linetraj, float xnow);
+void LineTrajInit(LineTraj* linetraj, float startspeed, float uniformspeed, float endspeed, float aimx, float speedUpDis, float speedDownDis);
diff --git a/Chassis/inc/robot.h b/Chassis/inc/robot.h
@@ -14,8 +14,9 @@
 #include "KF.h"
 #include "btoscilloscope.h"
 #include "leg.h"
-#include "yesense.h"
+#include "linetraj.h"
 #include "vesc.h"
+#include "yesense.h"
 
 typedef struct {
   bool flyflag;
@@ -33,16 +34,15 @@ typedef struct {
 
   RobotRunMode mode;
 
-  float        L0Set;  // 设定腿长，也就是当前两条腿的平均腿长
-  float        xSet;
-  float        vSet;
-  float        force;
-  KalmanFilter thetaDot;
-  KalmanFilter pitchNow;
-  KalmanFilter pitchDot;
+  float L0Set;  // 设定腿长，也就是当前两条腿的平均腿长
+  float xSet;
+  float vSet;
+  float force;
 
   PID pitchpid;
   PID xpid;
+
+  LineTraj linetraj;
 } Robot;
 
 extern Robot robot;

diff --git a/Chassis/inc/robotparam.h b/Chassis/inc/robotparam.h
@@ -9,8 +9,11 @@
 //
 //----
 #pragma once
+
 #include "vector.h"
 
+#include <stdbool.h>
+
 // 初始状态：(rad)
 // #define FrontAngleInit  1.762782545f
 // #define BehindAngleInit 1.378810109f
@@ -38,7 +41,7 @@
 // 虚拟力前馈
 #define FFEEDFORWARD    26.934435011f
 // 最小支持力阈值，判断是否离地的支持力的阈值
-#define FORCETHRESHOLD  10.f
+#define FORCETHRESHOLD  5.f
 // 机器人最大速度，如果追踪更大的速度会翻车
 #define MAXROBOTSPEED   1.f
 // 最小腿长

diff --git a/Chassis/src/linetraj.c b/Chassis/src/linetraj.c
@@ -0,0 +1,47 @@
+#include "linetraj.h"
+
+void LineTrajSpeedUp(LineTraj* linetraj) {
+  if (linetraj->speed < linetraj->uniformSpeed)
+    linetraj->speed = linetraj->startSpeed + (linetraj->uniformSpeed - linetraj->startSpeed) * sqrtf(linetraj->x / linetraj->speedUpDis);
+}
+
+void LineTrajSpeedDown(LineTraj* linetraj) {
+  float surplusDis = linetraj->aimx - linetraj->x;
+  if (surplusDis <= linetraj->speedDownDis)
+    linetraj->speed = linetraj->endSpeed + (linetraj->uniformSpeed - linetraj->endSpeed) * sqrtf(surplusDis / linetraj->speedDownDis);
+}
+
+void LineTrajRun(LineTraj* linetraj, float xnow) {
+  if (linetraj->finish) {
+    linetraj->xset     = linetraj->aimx;
+    linetraj->speed = 0;
+    return;
+  }
+  linetraj->x = xnow;
+  if (linetraj->x < 0)
+    linetraj->x = 0;
+  if (xnow < linetraj->speedUpDis)
+    LineTrajSpeedUp(linetraj);
+  else if (xnow > linetraj->aimx) {
+    linetraj->finish = true;
+    linetraj->x      = linetraj->aimx;
+    linetraj->speed  = 0;
+  } else if (xnow > linetraj->aimx - linetraj->speedDownDis)
+    LineTrajSpeedDown(linetraj);
+  else
+    linetraj->speed = linetraj->uniformSpeed;
+  linetraj->xset += linetraj->speed * 0.005f;
+}
+
+void LineTrajInit(LineTraj* linetraj, float startspeed, float uniformspeed, float endspeed, float aimx, float speedUpDis, float speedDownDis) {
+  linetraj->finish       = false;
+  linetraj->startSpeed   = startspeed;
+  linetraj->uniformSpeed = uniformspeed;
+  linetraj->endSpeed     = endspeed;
+  linetraj->aimx         = aimx;
+  linetraj->speedUpDis   = speedUpDis;
+  linetraj->speedDownDis = speedDownDis;
+  linetraj->x            = 0;
+  linetraj->xset         = 0;
+  linetraj->speed        = 0;
+}