246 lines
9.3 KiB
C#
246 lines
9.3 KiB
C#
|
using ClumsyCore;
|
||
|
|
using ClumsyCore.DTools;
|
||
|
|
using ClumsyCore.Interfaces;
|
||
|
|
using ClumsyCore.Pilot;
|
||
|
|
using ClumsyCore.Sensors;
|
||
|
|
using ClumsyCore.Utilities;
|
||
|
|
using ClumsyDance.ClumsyWalk.Detectors;
|
||
|
|
using ClumsyDance.Sensors;
|
||
|
|
using CommonUsage.Chassis;
|
||
|
|
using FundamentalLib;
|
||
|
|
using MDCSToolBox.Clumsy.Calibration;
|
||
|
|
using MDCSToolBox.Clumsy.HighLevelSecurity;
|
||
|
|
using MDCSToolBox.Clumsy.Movements;
|
||
|
|
using MDCSToolBox.Clumsy.Pilot;
|
||
|
|
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;
|
||
|
|
using System.Linq;
|
||
|
|
using System.Net.Http;
|
||
|
|
using System.Numerics;
|
||
|
|
using System.Reflection;
|
||
|
|
using System.Text;
|
||
|
|
using System.Threading;
|
||
|
|
|
||
|
|
namespace MultiWheelC
|
||
|
|
{
|
||
|
|
public class MultiWheelRotateInPlace : MovementDefinition
|
||
|
|
{
|
||
|
|
/// <summary>
|
||
|
|
/// 旋转目标角度
|
||
|
|
/// </summary>
|
||
|
|
public float AngleTarget;
|
||
|
|
|
||
|
|
public float MaxSpeed;
|
||
|
|
|
||
|
|
public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;
|
||
|
|
|
||
|
|
public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||
|
|
|
||
|
|
public Func<PIDParams> PidparamsRead = () => new PIDParams() { };
|
||
|
|
|
||
|
|
public PIDController thPid;
|
||
|
|
|
||
|
|
private static float RangeAngle(float theta)
|
||
|
|
{
|
||
|
|
return (float)(theta - Math.Round(theta / 360.0f) * 360);
|
||
|
|
}
|
||
|
|
|
||
|
|
public override IEnumerable<bool> Get()
|
||
|
|
{
|
||
|
|
var targetAngle = RangeAngle(AngleTarget);
|
||
|
|
var p = PidparamsRead();
|
||
|
|
thPid = new PIDController(ThetaReader, p.Kp);
|
||
|
|
thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone, p.OutputUpperThreshold, p.SpeedAccPerSec);
|
||
|
|
|
||
|
|
DateTime lastTime = DateTime.Now;
|
||
|
|
while (true)
|
||
|
|
{
|
||
|
|
var s = thPid.GetResponse(targetAngle, true);
|
||
|
|
Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
|
||
|
|
Chassis.SendXYThSpeed(0, 0, s);
|
||
|
|
lastTime = DateTime.Now;
|
||
|
|
if (thPid.IsArrived()) break;
|
||
|
|
yield return true;
|
||
|
|
}
|
||
|
|
Chassis.SendXYThSpeed(0, 0, 0);
|
||
|
|
Console.WriteLine($"final rotate to {targetAngle}");
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
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;
|
||
|
|
public override IEnumerable<bool> Get()
|
||
|
|
{
|
||
|
|
var start = DateTime.Now;
|
||
|
|
while ((DateTime.Now-start).TotalSeconds<Second)
|
||
|
|
{
|
||
|
|
yield return true;
|
||
|
|
Thread.Sleep(1000);
|
||
|
|
Console.WriteLine("Sleep");
|
||
|
|
}
|
||
|
|
|
||
|
|
yield return false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
//直线行走基于detour
|
||
|
|
public class LineTracking1 : MovementDefinition
|
||
|
|
{
|
||
|
|
public float LineDistance = 1000f;
|
||
|
|
public int SrcId = -1;
|
||
|
|
public int DstId = -1;
|
||
|
|
public Action<int> LeaveSrcFunction = null;
|
||
|
|
public Painter painter = UI.GetPainter("Line", false);
|
||
|
|
public override IEnumerable<bool> Get()
|
||
|
|
{
|
||
|
|
var curpose = DetourInterface.getCartLocation();
|
||
|
|
Console.WriteLine($"curpose.th:{curpose.th}");
|
||
|
|
var src = new Vector2((float)curpose.x, (float)curpose.y);
|
||
|
|
var dst = new Vector2((float)curpose.x + LineDistance * (float)Math.Cos(curpose.th),
|
||
|
|
(float)curpose.y + LineDistance * (float)Math.Sin(curpose.th));
|
||
|
|
Console.WriteLine($"src:{src.X} {src.Y}");
|
||
|
|
Console.WriteLine($"dst:{dst.X} {dst.Y}");
|
||
|
|
painter.DrawLine(Color.Green, src.X, src.Y, dst.X, dst.Y, width: 3);
|
||
|
|
|
||
|
|
var tracker = new ChassisController().Get();
|
||
|
|
var linePath = new LineTrack(src, dst) { CarDirectionBias = LineDistance > 0 ? 0 : 180 };
|
||
|
|
tracker.AddTrack(linePath);
|
||
|
|
var _dt = new DriveTask(tracker.Track());
|
||
|
|
_dt.Wait();
|
||
|
|
if (SrcId != -1 && LeaveSrcFunction != null)
|
||
|
|
{
|
||
|
|
LeaveSrcFunction(SrcId);
|
||
|
|
DLog.Log($"释放放车点{SrcId}", "TireFollowing");
|
||
|
|
}
|
||
|
|
yield return false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
//直线行走基于轮里程
|
||
|
|
public class LineTracking : MovementDefinition
|
||
|
|
{
|
||
|
|
public float Target;
|
||
|
|
public float MaxSpeed = PilotDefinition.Conf.LineTrackMaxSpeed;
|
||
|
|
public float Kp = PilotDefinition.Conf.LineTrackKp;
|
||
|
|
public float Ki = PilotDefinition.Conf.LineTrackKi;
|
||
|
|
public float Kd = PilotDefinition.Conf.LineTrackKd;
|
||
|
|
public float DeadZone = PilotDefinition.Conf.LineTrackDeadZone;
|
||
|
|
public int SrcId = -1;
|
||
|
|
public int DstId = -1;
|
||
|
|
public Action<int> LeaveSrcFunction = null;
|
||
|
|
private PIDController pid;
|
||
|
|
|
||
|
|
public override IEnumerable<bool> Get()
|
||
|
|
{
|
||
|
|
pid = new PIDController(() => (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2, Kp, Ki, Kd, 0,
|
||
|
|
DeadZone, MaxSpeed)
|
||
|
|
{ SpeedAccPerSec = MaxSpeed / 2f };
|
||
|
|
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||
|
|
while (true)
|
||
|
|
{
|
||
|
|
var speed = pid.GetResponse(Target);
|
||
|
|
Console.WriteLine($"output: {speed} current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}");
|
||
|
|
var current = (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2;
|
||
|
|
chassis.SendXYThSpeed(speed, 0, 0);
|
||
|
|
//if (Math.Abs(current - Target) < pid.DeadZone)
|
||
|
|
//{
|
||
|
|
// chassis.SendXYThSpeed(0f, 0f, 0f);
|
||
|
|
// Console.WriteLine($"调整退出:当前({current:f2}) ,目标:({Target:f2})");
|
||
|
|
// break;
|
||
|
|
//}
|
||
|
|
if (pid.IsArrived()) break;
|
||
|
|
yield return true;
|
||
|
|
}
|
||
|
|
if (SrcId != -1 && LeaveSrcFunction != null)
|
||
|
|
{
|
||
|
|
LeaveSrcFunction(SrcId);
|
||
|
|
DLog.Log($"释放放车点{SrcId}", "TireFollowing");
|
||
|
|
}
|
||
|
|
yield return false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
public class DriverAble : MovementDefinition
|
||
|
|
{
|
||
|
|
public override IEnumerable<bool> Get()
|
||
|
|
{
|
||
|
|
Console.WriteLine("驱动器上使能");
|
||
|
|
PilotDefinition.Self.ResetFromC = true;
|
||
|
|
Thread.Sleep(100);
|
||
|
|
PilotDefinition.Self.ResetFromC = false;
|
||
|
|
Console.WriteLine("驱动器上使能完成");
|
||
|
|
yield return false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
public class DriverDisable : MovementDefinition
|
||
|
|
{
|
||
|
|
public override IEnumerable<bool> Get()
|
||
|
|
{
|
||
|
|
Console.WriteLine("驱动器下使能");
|
||
|
|
PilotDefinition.Self.DisableFromC = true;
|
||
|
|
Thread.Sleep(100);
|
||
|
|
PilotDefinition.Self.DisableFromC = false;
|
||
|
|
Console.WriteLine("驱动器下使能完成");
|
||
|
|
yield return false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|