petejk
/
TargetService
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

fixed track control

main
jkpete 4 weeks ago
parent
23c61600de
commit
2395698807
1 changed files with 0 additions and 255 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 255
      Script/Model/ModelTrackControl.cs

255
Script/Model/ModelTrackControl.cs
Unescape View File

@ -4,23 +4,8 @@ using Tmds.Linux;
public class ModelTrackControl : EGModule, IEGFramework public class ModelTrackControl : EGModule, IEGFramework
{ {
public const int SlaveID = 3;
public bool IsInitTrack { set; get; } = false;
public bool IsScanning { set; get; } = false;
public string PortName { set; get; } = "";
public Vector3 CurrentColorRGB { set; get; } = new Vector3();
public Vector3 CurrentColorHSV { set; get; } = new Vector3();
private IUnRegister ColorSensorEvent { set; get; }
public int CurrentPosition { set; get; } = 0; public int CurrentPosition { set; get; } = 0;
public int TargetPosition { set; get; } = 0;
public StatusTrack MoveStatus { set; get; } = StatusTrack.Stop;
private ushort LowSpeed { set; get; } private ushort LowSpeed { set; get; }
private ushort Speed { set; get; } private ushort Speed { set; get; }
@ -35,108 +20,6 @@ public class ModelTrackControl : EGModule, IEGFramework
ResetTarget(); ResetTarget();
} }
public void EnableTrack()
{
this.EGSendMessage(new RequestTrackEnabled(), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
IsInitTrack = true;
}
public void StartTrack()
{
if (!IsInitTrack)
{
EnableTrack();
}
this.EGSendMessage(new RequestTrackSpeed(Speed), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
this.EGSendMessage(new RequestTrackMove(), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
}
public void StartSlowTrack()
{
if (!IsInitTrack)
{
EnableTrack();
}
this.EGSendMessage(new RequestTrackSpeed(LowSpeed), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
this.EGSendMessage(new RequestTrackMove(), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
}
public void BackTrack()
{
if (!IsInitTrack)
{
EnableTrack();
}
this.EGSendMessage(new RequestTrackSpeed(Speed), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
this.EGSendMessage(new RequestTrackBack(), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
}
public void BackSlowTrack()
{
if (!IsInitTrack)
{
EnableTrack();
}
this.EGSendMessage(new RequestTrackSpeed(LowSpeed), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
this.EGSendMessage(new RequestTrackBack(), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
}
public void StopTrack()
{
if (!IsInitTrack)
{
EnableTrack();
}
this.EGSendMessage(new RequestTrackSpeed(0), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
this.EGSendMessage(new RequestTrackStop(), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
}
public void BreakTrack()
{
if (!IsInitTrack)
{
EnableTrack();
}
this.EGSendMessage(new RequestTrackBreak(), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
this.EGSendMessage(new RequestTrackSpeed(0), this.GetModule<ModelSerialTest>().EngineSerialPort, ProtocolType.SerialPort);
}
public void MoveToPosition(int position)
{
if (this.CurrentPosition == position)
{
EG.Print("Position is same.Nothing need to move!");
return;
}
if (this.MoveStatus != StatusTrack.Stop)
{
EG.Print("Machine should be stop first!");
return;
}
if (!this.GetModule<ModelParamSetting>().Setting.ColorMapping.ContainsKey(position))
{
EG.Print("Position invalid!Please add color first!");
return;
}
TargetPosition = position;
if (this.CurrentPosition > position)
{
this.StartScanColor();
this.StartTrack();
this.MoveStatus = StatusTrack.Start;
}
if (this.CurrentPosition < position)
{
this.StartScanColor();
this.BackTrack();
this.MoveStatus = StatusTrack.Back;
}
}
public void StartTarget() public void StartTarget()
{ {
if (CurrentRotation != 90) if (CurrentRotation != 90)
@ -166,144 +49,6 @@ public class ModelTrackControl : EGModule, IEGFramework
} }
} }
public void StartScanColor()
{
this.EGSendMessage(new RequestColorStartScan(), this.GetModule<ModelSerialTest>().ColorSerialPort, ProtocolType.SerialPort);
ColorSensorEvent = this.EGRegisterMessageEvent<ResponseColorSensor>(OnColorScanned);
IsScanning = true;
}
public void StopScanColor()
{
this.EGSendMessage(new RequestColorStopScan(), this.GetModule<ModelSerialTest>().ColorSerialPort, ProtocolType.SerialPort);
//null exception
ColorSensorEvent.UnRegister();
IsScanning = false;
}
public void ResetTrack()
{
this.CurrentPosition = 0;
this.StartScanColor();
this.StartTrack();
this.MoveStatus = StatusTrack.Reset;
}
private float HueCache { set; get; }
public void OnColorScanned(ResponseColorSensor color, string sender, ProtocolType type)
{
if (type == ProtocolType.SerialPort)
{
this.CurrentColorHSV = color.ColorHSV;
this.CurrentColorRGB = color.ColorRGB;
if (this.MoveStatus == StatusTrack.Start)
{
float currentHue = CurrentColorHSV.X;
if (MathF.Abs(HueCache - currentHue) > this.GetModule<ModelParamSetting>().Setting.ColorOffset)
{
HueCache = currentHue;
return;
}
float targetHue = this.GetModule<ModelParamSetting>().Setting.ColorMapping[TargetPosition].X;
bool compareResult = this.CompareHue(currentHue, targetHue);
if (compareResult)
{
this.StartSlowTrack();
this.MoveStatus = StatusTrack.StartSlow;
BroadCastMoveStatus();
}
}
if (this.MoveStatus == StatusTrack.Back)
{
float currentHue = CurrentColorHSV.X;
if (MathF.Abs(HueCache - currentHue) > this.GetModule<ModelParamSetting>().Setting.ColorOffset)
{
HueCache = currentHue;
return;
}
float targetHue = this.GetModule<ModelParamSetting>().Setting.ColorMapping[TargetPosition].X;
bool compareResult = this.CompareHue(currentHue, targetHue);
if (compareResult)
{
this.BackSlowTrack();
this.MoveStatus = StatusTrack.BackSlow;
BroadCastMoveStatus();
}
}
if (this.MoveStatus == StatusTrack.BackSlow || this.MoveStatus == StatusTrack.StartSlow)
{
float currentHue = CurrentColorHSV.X;
float targetHue = 4.0f;
bool compareResult = this.CompareHue(currentHue, targetHue);
if (compareResult)
{
this.BreakTrack();
this.MoveStatus = StatusTrack.Stop;
this.StopScanColor();
this.CurrentPosition = this.TargetPosition;
BroadCastMoveStatus();
}
}
if(this.MoveStatus == StatusTrack.Reset)
{
float currentHue = CurrentColorHSV.X;
float targetHue = this.GetModule<ModelParamSetting>().Setting.ColorMapping[0].X;
bool compareResult = this.CompareHue(currentHue, targetHue);
if (compareResult)
{
this.BreakTrack();
this.MoveStatus = StatusTrack.Stop;
this.StopScanColor();
this.CurrentPosition = 0;
BroadCastMoveStatus();
}
}
HueCache = CurrentColorHSV.X;
}
}
public bool CompareHue(float hue1,float hue2)
{
if (hue1 > hue2 - this.GetModule<ModelParamSetting>().Setting.ColorOffset &&
hue1 < hue2 + this.GetModule<ModelParamSetting>().Setting.ColorOffset)
{
return true;
}
else
{
return false;
}
}
public Vector3 GetColor()
{
return CurrentColorRGB;
}
public Vector3 GetColorHSV()
{
return CurrentColorHSV;
}
public void BroadCastMoveStatus()
{
if (this.EGTCPServer().LinkedIPs.ContainsKey(6557))
{
foreach (string sender in this.EGTCPServer().LinkedIPs[6557])
{
this.EGSendMessage<RequestControlMoveStatus>(new RequestControlMoveStatus()
{
TargetID = this.GetModule<ModelParamSetting>().Setting.DeviceID,
FunctionCode = 301,
Position = this.GetModule<ModelTrackControl>().CurrentPosition,
Status = MoveStatus
}, sender, ProtocolType.TCPServer);
}
}
}
} }
public enum StatusTrack public enum StatusTrack

Write Preview
Loading…
Cancel
Save