First_Dev_Dept/Tutorial
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'master' of http://git.fairylandtech.com/ruifeng.zhou/Tutorial

Browse Source
This commit is contained in:
shuai.li
2026-06-30 14:37:52 +08:00
parent 85e365a22d 14b967f889
commit d5fdf60843
16 changed files with 1355 additions and 236 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheel/MultiWheelC/Movements.cs
-50
View File
@@ -16,7 +16,6 @@ using MDCSToolBox.Clumsy.Tracks;
using MDCSToolBox.Commons;
using MDCSToolBox.Commons.Controllers;
using Newtonsoft.Json;
using OpenCvSharp.Dnn;
using System;
using System.Collections.Generic;
using System.Drawing;
@@ -74,55 +73,6 @@ namespace MultiWheelC
}
public class ClampToTarget : MovementDefinition
{
public float LeftClampTarget;
public float RightClampTarget;
public float MaxClampSpeed = PilotDefinition.Conf.MaxClampSpeed;
public float ClampKp = PilotDefinition.Conf.ClampControlKp;
public float ClampKi = PilotDefinition.Conf.ClampControlKi;
public float ClampKd = PilotDefinition.Conf.ClampControlKd;
public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone;
private PIDController leftpid, rightpid;
public override IEnumerable<bool> Get()
{
leftpid = new PIDController(() => PilotDefinition.Self.ActualPosLeftArm, ClampKp, ClampKi, ClampKd, 0,
ClampDeadZone, MaxClampSpeed)
{ SpeedAccPerSec = MaxClampSpeed / 2f };
rightpid = new PIDController(() => PilotDefinition.Self.ActualPosRightArm, ClampKp, ClampKi, ClampKd, 0,
ClampDeadZone, MaxClampSpeed)
{ SpeedAccPerSec = MaxClampSpeed / 2f };
while (true)
{
var leftspeed = leftpid.GetResponse(LeftClampTarget);
var rightspeed = rightpid.GetResponse(RightClampTarget);
Console.WriteLine($"left arm speed:{leftspeed} right arm speed:{rightspeed}");
PilotDefinition.Self.SpeedLeftArm = leftspeed;
PilotDefinition.Self.SpeedRightArm = rightspeed;
if (leftpid.IsArrived()) PilotDefinition.Self.SpeedLeftArm = 0;
if (rightpid.IsArrived()) PilotDefinition.Self.SpeedRightArm = 0;
//if(Math.Abs(PilotDefinition.Self.LeftArmActualPos - PilotDefinition.Self.RightArmActualPos) > PilotDefinition.Conf.ClampOutOfSyncThr)
//{
// PilotDefinition.Self.ClampOutOfSync = true;
// Console.WriteLine($"左夹臂位置{PilotDefinition.Self.LeftArmActualPos} 右夹臂位置{PilotDefinition.Self.RightArmActualPos}");
// Console.WriteLine($"两个夹臂的位置差了{Math.Abs(PilotDefinition.Self.LeftArmActualPos - PilotDefinition.Self.RightArmActualPos)}");
// break;
//}
if (leftpid.IsArrived() && rightpid.IsArrived()) break;
yield return true;
}
PilotDefinition.Self.SpeedLeftArm = 0;
PilotDefinition.Self.SpeedRightArm = 0;
Console.WriteLine($"left clamp to target:{LeftClampTarget} right clamp to target:{RightClampTarget}");
}
}
public class Sleep : MovementDefinition
{
public float Second = 2;