|
|
@@ -1,15 +1,10 @@
|
|
|
using GSG.NET.Concurrent;
|
|
|
-using GSG.NET.Extensions;
|
|
|
using GSG.NET.Logging;
|
|
|
using OHV.Common.Shareds;
|
|
|
using OHV.SqliteDAL;
|
|
|
using System;
|
|
|
-using System.Collections.Generic;
|
|
|
-using System.Linq;
|
|
|
-using System.Runtime.InteropServices;
|
|
|
-using System.Text;
|
|
|
-using System.Threading;
|
|
|
-using System.Threading.Tasks;
|
|
|
+using VehicleControlSystem.ControlLayer.DB;
|
|
|
+using VehicleControlSystem.Managers;
|
|
|
|
|
|
namespace VehicleControlSystem.ControlLayer.Drive
|
|
|
{
|
|
|
@@ -25,73 +20,6 @@ namespace VehicleControlSystem.ControlLayer.Drive
|
|
|
None,
|
|
|
}
|
|
|
|
|
|
-
|
|
|
- #region Dll Import
|
|
|
- [DllImport( "OHVdrive.dll" )]
|
|
|
- static extern void OHVdriveCreate();
|
|
|
-
|
|
|
- [DllImport( "OHVdrive.dll", CharSet = CharSet.Unicode, CallingConvention = CallingConvention.Cdecl )]
|
|
|
- static extern void OHVdriveBegin();
|
|
|
-
|
|
|
- [DllImport( "OHVdrive.dll" )]
|
|
|
- static extern void OHVdriveEnd();
|
|
|
-
|
|
|
- [DllImport( "OHVdrive.dll" )]
|
|
|
- static extern void OHVdriveDelete();
|
|
|
-
|
|
|
- //
|
|
|
- [DllImport( "OHVdrive.dll" )]
|
|
|
- static extern int OHVdriveGetTargetSteering();
|
|
|
-
|
|
|
- // driving servo driver
|
|
|
- // - servo_num = 1 = front, -1 = rear
|
|
|
- // - return value
|
|
|
- // 0 = wrong servo_num
|
|
|
- // 1 = Servo On
|
|
|
- // 2 = Servo Off
|
|
|
- // 3 = Servo Stop
|
|
|
- // -1 = Servo Fault Reset
|
|
|
- // others = do nothing
|
|
|
- [DllImport( "OHVdrive.dll" )]
|
|
|
- static extern int OHVdriveGetServoState( int servo_num );
|
|
|
-
|
|
|
- // driving servo driver
|
|
|
- // - return value
|
|
|
- // 1 = Servo On
|
|
|
- // 2 = Servo Off
|
|
|
- // 3 = Servo Stop
|
|
|
- // -1 = Servo Fault Reset
|
|
|
- // others = do nothing
|
|
|
- [DllImport( "OHVdrive.dll" )]
|
|
|
- static extern void OHVdriveSetServoCommand( int command );
|
|
|
-
|
|
|
- // command when manual operation
|
|
|
- // 1 = go forward
|
|
|
- // -1 = go backward
|
|
|
- // 0 or others = stop
|
|
|
- [DllImport( "OHVdrive.dll" )]
|
|
|
- static extern void OHVdriveSetManualOperationCommand( int command );
|
|
|
-
|
|
|
- // operation mode change (manual mode, automatic mode)
|
|
|
- // 1 = change to automatic mode
|
|
|
- // -1 = change to manual mode
|
|
|
- // others = not change
|
|
|
- [DllImport( "OHVdrive.dll" )]
|
|
|
- static extern void OHVdriveSetOperationMode( int mode );
|
|
|
-
|
|
|
- [DllImport( "OHVdrive.dll" )]
|
|
|
- static extern int OHVdriveGetOperationMode();
|
|
|
-
|
|
|
- // state of the laser scanner for detecting obstacles
|
|
|
- // 0 = normal (no obstacles)
|
|
|
- // 1 = field 1 (e-stop)
|
|
|
- // 2 = field 2 (stop)
|
|
|
- // 3 = filed 3 (slow)
|
|
|
- [DllImport( "OHVdrive.dll" )]
|
|
|
- static extern int OHVdriveSetLaserScannerField( int field );
|
|
|
-
|
|
|
- #endregion
|
|
|
-
|
|
|
Logger logger = Logger.GetLogger();
|
|
|
|
|
|
public bool IsStop { get; set; }
|
|
|
@@ -133,28 +61,20 @@ namespace VehicleControlSystem.ControlLayer.Drive
|
|
|
|
|
|
public void Init()
|
|
|
{
|
|
|
-#if SIMULATION
|
|
|
- return;
|
|
|
-#else
|
|
|
- //OHVdriveCreate();
|
|
|
- //OHVdriveBegin();
|
|
|
-
|
|
|
- //this.threadCancel.AddGo( Thread_DriveStateChcker );
|
|
|
-#endif
|
|
|
+ Redis.Instance.Init();
|
|
|
+ this.threadCancel.AddGo( Thread_DriveStateChcker );
|
|
|
+ this.threadCancel.AddGo( Thread_Logger );
|
|
|
+
|
|
|
+ PhysicalCheckupLogger.Instance.Connecte();
|
|
|
}
|
|
|
|
|
|
public void Dispose()
|
|
|
{
|
|
|
-#if SIMULATION
|
|
|
- return;
|
|
|
-#else
|
|
|
- //this.threadCancel.Cancel();
|
|
|
- //LockUtils.Wait( 50 );
|
|
|
- //this.threadCancel.StopWaitAll();
|
|
|
-
|
|
|
- //OHVdriveEnd();
|
|
|
- //OHVdriveDelete();
|
|
|
-#endif
|
|
|
+ Redis.Instance.Dispose();
|
|
|
+
|
|
|
+ this.threadCancel.Cancel();
|
|
|
+ LockUtils.Wait( 50 );
|
|
|
+ this.threadCancel.StopWaitAll();
|
|
|
}
|
|
|
|
|
|
void Thread_DriveStateChcker()
|
|
|
@@ -168,6 +88,10 @@ namespace VehicleControlSystem.ControlLayer.Drive
|
|
|
LockUtils.Wait( 10 );
|
|
|
this.GetReqSteeringState();
|
|
|
//this.GetServoState();
|
|
|
+ //if (Redis.Instance.GetDriveMove() )
|
|
|
+ //{
|
|
|
+ // this.LoggingState();
|
|
|
+ //}
|
|
|
}
|
|
|
catch ( Exception e)
|
|
|
{
|
|
|
@@ -178,68 +102,117 @@ namespace VehicleControlSystem.ControlLayer.Drive
|
|
|
logger.D( "[Drive] - Thread Drive State Checker Disposed" );
|
|
|
}
|
|
|
|
|
|
+ bool isLoggingStart = false;
|
|
|
+ public bool IsloggingStart
|
|
|
+ {
|
|
|
+ get => this.isLoggingStart;
|
|
|
+ set
|
|
|
+ {
|
|
|
+ if ( SetField( ref this.isLoggingStart, value ) )
|
|
|
+ {
|
|
|
+ if ( value )
|
|
|
+ PhysicalCheckupLogger.Instance.SetPLCStartDrive();
|
|
|
+ else
|
|
|
+ PhysicalCheckupLogger.Instance.ResetPLCStartDrive();
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ void Thread_Logger()
|
|
|
+ {
|
|
|
+ while ( !this.threadCancel.Canceled )
|
|
|
+ {
|
|
|
+ try
|
|
|
+ {
|
|
|
+ LockUtils.Wait( 10 );
|
|
|
+
|
|
|
+ if ( Redis.Instance.GetDriveMove() )
|
|
|
+ {
|
|
|
+ this.IsloggingStart = true;
|
|
|
+ this.LoggingState();
|
|
|
+ }
|
|
|
+ else
|
|
|
+ this.IsloggingStart = false;
|
|
|
+ }
|
|
|
+ catch ( Exception e )
|
|
|
+ {
|
|
|
+ logger.E( $"[Drive] - Error : [{e}]" );
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ void LoggingState()
|
|
|
+ {
|
|
|
+ var currentBCR = Redis.Instance.CurrentBCRValue();
|
|
|
+ var speed = Redis.Instance.ActualVelocityToSpeed();
|
|
|
+
|
|
|
+ var fTorque = Redis.Instance.TorqueFront();
|
|
|
+ var fRPM = Redis.Instance.ActualVelocityToFrontRPM();
|
|
|
+ var fLoadFacter = Redis.Instance.LoadFacterFront();
|
|
|
+ PhysicalCheckupLogger.Instance.FrontWheelLogging( speed.ToString(), fTorque.ToString(), fRPM.ToString(), fLoadFacter.ToString(), currentBCR.ToString() );
|
|
|
+
|
|
|
+ var rTorque = Redis.Instance.TorqueRear();
|
|
|
+ var rRPM = Redis.Instance.ActualVelocityToRearRPM();
|
|
|
+ var rLoadFacter = Redis.Instance.LoadFacterRear();
|
|
|
+ PhysicalCheckupLogger.Instance.RearWheelLoggging( speed.ToString(), rTorque.ToString(), rRPM.ToString(), rLoadFacter.ToString(), currentBCR.ToString() );
|
|
|
+ }
|
|
|
+
|
|
|
void GetReqSteeringState()
|
|
|
{
|
|
|
- var gV = OHVdriveGetTargetSteering();
|
|
|
- if ( gV == -1 )
|
|
|
- this.ReqSteeringState = eSteeringState.Right;
|
|
|
- else if ( gV == 1 )
|
|
|
+ var ret = Redis.Instance.GetSteering();
|
|
|
+ if ( ret == 1 )// Left
|
|
|
this.ReqSteeringState = eSteeringState.Left;
|
|
|
- else { }
|
|
|
+ else if ( ret == -1 ) //Right
|
|
|
+ this.ReqSteeringState = eSteeringState.Right;
|
|
|
+ else if ( ret == 0 ) // Do Nothing
|
|
|
+ this.ReqSteeringState = eSteeringState.None;
|
|
|
+ else
|
|
|
+ { }
|
|
|
}
|
|
|
|
|
|
- void GetServoState()
|
|
|
+ bool IsDriveFault()
|
|
|
{
|
|
|
- int fState = OHVdriveGetServoState( 1 );
|
|
|
- int rState = OHVdriveGetServoState( -1 );
|
|
|
-
|
|
|
- if ( fState == 1 && rState == 1 )
|
|
|
- this.DriveServoState = eDriveServoState.ServoOn;
|
|
|
- else if ( fState == 2 && rState == 2 )
|
|
|
- this.DriveServoState = eDriveServoState.ServoOff;
|
|
|
- else if ( fState == 3 && rState == 3 )
|
|
|
- this.DriveServoState = eDriveServoState.ServoStop;
|
|
|
- else if ( fState == -1 || rState == -1 )
|
|
|
- this.DriveServoState = eDriveServoState.ServoFault;
|
|
|
- else
|
|
|
- this.DriveServoState = eDriveServoState.None;
|
|
|
+ var ret = Redis.Instance.GetSystemState();
|
|
|
+ return ret == 2 ? true : false;
|
|
|
}
|
|
|
|
|
|
public void SetDriveOperationMode( eOperatationMode mode )
|
|
|
{
|
|
|
-#if SIMULATION
|
|
|
-#else
|
|
|
- return;
|
|
|
|
|
|
if ( mode == eOperatationMode.ManualMode )
|
|
|
- OHVdriveSetOperationMode( -1 );
|
|
|
+ Redis.Instance.SetSystemOperation( 3 );
|
|
|
else if ( mode == eOperatationMode.AutoMode )
|
|
|
- OHVdriveSetOperationMode( 1 );
|
|
|
+ Redis.Instance.SetSystemOperation( 2 );
|
|
|
else { }
|
|
|
-#endif
|
|
|
}
|
|
|
|
|
|
public eOperatationMode GetDriveOperationMode()
|
|
|
{
|
|
|
- eOperatationMode mode = eOperatationMode.InitialMode;
|
|
|
+ var ret = Redis.Instance.GetSystemState();
|
|
|
|
|
|
- int result = OHVdriveGetOperationMode();
|
|
|
- if ( result == 1 )
|
|
|
- mode = eOperatationMode.AutoMode;
|
|
|
- else if ( result == -1 )
|
|
|
- mode = eOperatationMode.ManualMode;
|
|
|
- else { }
|
|
|
-
|
|
|
- return mode;
|
|
|
+ if ( ret == 3 )
|
|
|
+ return eOperatationMode.ManualMode;
|
|
|
+ else if ( ret == 4 )
|
|
|
+ return eOperatationMode.AutoMode;
|
|
|
+ else
|
|
|
+ return eOperatationMode.InitialMode;
|
|
|
}
|
|
|
|
|
|
public int MoveToPoint( string point, double velocity )
|
|
|
{
|
|
|
- if ( this.DriveServoState != eDriveServoState.ServoStop )
|
|
|
- return 9999;
|
|
|
+ //if ( this.DriveServoState != eDriveServoState.ServoStop )
|
|
|
+ // return 9999;
|
|
|
|
|
|
+ //Drive Fault 상태 확인
|
|
|
+ if ( IsDriveFault() )
|
|
|
+ return 34;
|
|
|
|
|
|
//ToDo: 이동 명령 실행.
|
|
|
+ int result = 0;
|
|
|
+ if (! Redis.Instance.RouteMapMoveTo( point ) )
|
|
|
+ {
|
|
|
+ //Todo: Error 처리
|
|
|
+ }
|
|
|
+
|
|
|
|
|
|
return 0;
|
|
|
}
|
|
|
@@ -247,25 +220,25 @@ namespace VehicleControlSystem.ControlLayer.Drive
|
|
|
/// <summary>
|
|
|
/// 급정시 사용.
|
|
|
/// </summary>
|
|
|
- public void EStop() => OHVdriveSetServoCommand(3);
|
|
|
+ //public void EStop() => OHVdriveSetServoCommand(3);
|
|
|
|
|
|
/// <summary>
|
|
|
/// Manual 상태에서 가감속 Stop
|
|
|
/// </summary>
|
|
|
public void Stop()
|
|
|
{
|
|
|
- OHVdriveSetManualOperationCommand( 0 );
|
|
|
}
|
|
|
|
|
|
- public void JogForWard() => OHVdriveSetManualOperationCommand( 1 );
|
|
|
- public void JogBackward() => OHVdriveSetManualOperationCommand( -1 );
|
|
|
- public void ServoOn() => OHVdriveSetServoCommand( 1 );
|
|
|
- public void ServoOff() => OHVdriveSetServoCommand( 2 );
|
|
|
- public void AmpFaultReset() => OHVdriveSetServoCommand( -1 );
|
|
|
+ //public void JogForWard() => OHVdriveSetManualOperationCommand( 1 );
|
|
|
+ //public void JogBackward() => OHVdriveSetManualOperationCommand( -1 );
|
|
|
+ //public void ServoOn() => OHVdriveSetServoCommand( 1 );
|
|
|
+ //public void ServoOff() => OHVdriveSetServoCommand( 2 );
|
|
|
+ //public void AmpFaultReset() => OHVdriveSetServoCommand( -1 );
|
|
|
|
|
|
public void SetObstacleState( eObstacleState state )
|
|
|
{
|
|
|
int field = 0;
|
|
|
+
|
|
|
switch ( state )
|
|
|
{
|
|
|
case eObstacleState.Normal:
|
|
|
@@ -283,7 +256,6 @@ namespace VehicleControlSystem.ControlLayer.Drive
|
|
|
default:
|
|
|
break;
|
|
|
}
|
|
|
- OHVdriveSetLaserScannerField( field );
|
|
|
}
|
|
|
|
|
|
public bool VelocityChainge( double velocity )
|