|
|
@@ -24,10 +24,10 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
public eSteeringState SteeringState
|
|
|
{
|
|
|
get { return steeringState; }
|
|
|
- set { SetField(ref this.steeringState, value); }
|
|
|
+ set { SetField( ref this.steeringState , value ); }
|
|
|
}
|
|
|
|
|
|
- public Steering(IIO io, SqliteManager sql, IEventAggregator ea)
|
|
|
+ public Steering( IIO io , SqliteManager sql , IEventAggregator ea )
|
|
|
{
|
|
|
this.iO = io as EzIO;
|
|
|
this.sql = sql;
|
|
|
@@ -35,113 +35,126 @@ namespace VehicleControlSystem.ControlLayer
|
|
|
this.iO.OnChangedIO += IO_OnChangedIO;
|
|
|
}
|
|
|
|
|
|
- private void IO_OnChangedIO(BitBlock bit)
|
|
|
+ private void IO_OnChangedIO( BitBlock bit )
|
|
|
{
|
|
|
- if (bit.Tag.Equals("IN_F_STEERING_DETECT_LEFT") || bit.Tag.Equals("IN_F_STEERING_DETECT_RIGHT"))
|
|
|
+ if ( bit.Tag.Equals( "IN_F_STEERING_DETECT_LEFT" ) || bit.Tag.Equals( "IN_F_STEERING_DETECT_RIGHT" ) )
|
|
|
this.GetSteeringState();
|
|
|
- if (bit.Tag.Equals("IN_R_STEERING_DETECT_LEFT") || bit.Tag.Equals("IN_R_STEERING_DETECT_RIGHT"))
|
|
|
+ if ( bit.Tag.Equals( "IN_R_STEERING_DETECT_LEFT" ) || bit.Tag.Equals( "IN_R_STEERING_DETECT_RIGHT" ) )
|
|
|
this.GetSteeringState();
|
|
|
}
|
|
|
|
|
|
- void GetSteeringState()
|
|
|
+ void GetSteeringState( )
|
|
|
{
|
|
|
eSteeringState frontState = eSteeringState.None;
|
|
|
- if (this.IsFrontLeft() && this.IsFrontRight())
|
|
|
+ if ( this.IsFrontLeft() && this.IsFrontRight() )
|
|
|
frontState = eSteeringState.None;
|
|
|
- if (!this.IsFrontLeft() && !this.IsFrontRight())
|
|
|
+ if ( !this.IsFrontLeft() && !this.IsFrontRight() )
|
|
|
frontState = eSteeringState.None;
|
|
|
|
|
|
- if (this.IsFrontLeft() && !this.IsFrontRight())
|
|
|
+ if ( this.IsFrontLeft() && !this.IsFrontRight() )
|
|
|
frontState = eSteeringState.Left;
|
|
|
- if (!this.IsFrontLeft() && this.IsFrontRight())
|
|
|
+ if ( !this.IsFrontLeft() && this.IsFrontRight() )
|
|
|
frontState = eSteeringState.Right;
|
|
|
|
|
|
eSteeringState rearState = eSteeringState.None;
|
|
|
- if (this.IsRearLeft() && this.IsRearRight())
|
|
|
+ if ( this.IsRearLeft() && this.IsRearRight() )
|
|
|
rearState = eSteeringState.None;
|
|
|
- if (!this.IsRearLeft() && !this.IsRearRight())
|
|
|
+ if ( !this.IsRearLeft() && !this.IsRearRight() )
|
|
|
rearState = eSteeringState.None;
|
|
|
|
|
|
- if (this.IsRearLeft() && !this.IsRearRight())
|
|
|
+ if ( this.IsRearLeft() && !this.IsRearRight() )
|
|
|
rearState = eSteeringState.Left;
|
|
|
- if (!this.IsRearLeft() && this.IsRearRight())
|
|
|
+ if ( !this.IsRearLeft() && this.IsRearRight() )
|
|
|
rearState = eSteeringState.Right;
|
|
|
|
|
|
- if (frontState == eSteeringState.Left && rearState == eSteeringState.Left)
|
|
|
+ if ( frontState == eSteeringState.Left && rearState == eSteeringState.Left )
|
|
|
this.SteeringState = eSteeringState.Left;
|
|
|
- else if (frontState == eSteeringState.Right && rearState == eSteeringState.Right)
|
|
|
+ else if ( frontState == eSteeringState.Right && rearState == eSteeringState.Right )
|
|
|
this.SteeringState = eSteeringState.Right;
|
|
|
else
|
|
|
this.SteeringState = eSteeringState.None;
|
|
|
}
|
|
|
|
|
|
object lockObj = new object();
|
|
|
- public int ControlSteering(bool isLeft = false)
|
|
|
+ bool isExecuteSteering = false;
|
|
|
+ public int ControlSteering( bool isLeft = false )
|
|
|
{
|
|
|
- lock (lockObj)
|
|
|
+ if ( this.isExecuteSteering )
|
|
|
+ return 0;
|
|
|
+
|
|
|
+ this.isExecuteSteering = true;
|
|
|
+
|
|
|
+ if ( isLeft ) //Left IO 가 직진 이라고 생각하자.
|
|
|
{
|
|
|
- if (isLeft) //Left IO 가 직진 이라고 생각하자.
|
|
|
+ this.iO.WriteOutputIO( "OUT_F_STEERING_CWCCW" , false );
|
|
|
+ this.iO.WriteOutputIO( "OUT_R_STEERING_CWCCW" , true );
|
|
|
+ this.SetHightVolt();
|
|
|
+ if ( this.SteeringState == eSteeringState.Left )
|
|
|
{
|
|
|
- this.iO.WriteOutputIO("OUT_F_STEERING_CWCCW", false);
|
|
|
- this.iO.WriteOutputIO("OUT_R_STEERING_CWCCW", false);
|
|
|
- this.SetHightVolt();
|
|
|
+ this.isExecuteSteering = false;
|
|
|
+ return 0;
|
|
|
}
|
|
|
- else
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ this.iO.WriteOutputIO( "OUT_F_STEERING_CWCCW" , true );
|
|
|
+ this.iO.WriteOutputIO( "OUT_R_STEERING_CWCCW" , false );
|
|
|
+ this.SetHightVolt();
|
|
|
+ if ( this.SteeringState == eSteeringState.Right )
|
|
|
{
|
|
|
- this.iO.WriteOutputIO("OUT_F_STEERING_CWCCW", true);
|
|
|
- this.iO.WriteOutputIO("OUT_R_STEERING_CWCCW", true);
|
|
|
- this.SetHightVolt();
|
|
|
+ this.isExecuteSteering = false;
|
|
|
+ return 0;
|
|
|
}
|
|
|
-
|
|
|
- var task = Task<bool>.Run(() =>
|
|
|
- {
|
|
|
- long sTime = SwUtils.CurrentTimeMillis;
|
|
|
- while (true)
|
|
|
- {
|
|
|
- if (SwUtils.Gt(sTime, 5 * ConstUtils.ONE_SECOND)) //Wait 20Sec
|
|
|
- return false;
|
|
|
-
|
|
|
- if (isLeft)
|
|
|
- {
|
|
|
- if (this.IsFrontLeft() && this.IsRearLeft())
|
|
|
- break;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- if (this.IsFrontRight() && this.IsRearRight())
|
|
|
- break;
|
|
|
- }
|
|
|
- }
|
|
|
- return true;
|
|
|
- }).ContinueWith(t =>
|
|
|
- {
|
|
|
- if (!t.Result)
|
|
|
- {
|
|
|
- this.OnSteeringError?.BeginInvoke(this, 9999, null, null);
|
|
|
- }
|
|
|
- else
|
|
|
- this.OnSteeringError?.BeginInvoke(this, 0, null, null);
|
|
|
- });
|
|
|
-
|
|
|
}
|
|
|
|
|
|
+ var task = Task<bool>.Run( ( ) =>
|
|
|
+ {
|
|
|
+ long sTime = SwUtils.CurrentTimeMillis;
|
|
|
+ while ( true )
|
|
|
+ {
|
|
|
+ if ( SwUtils.Gt( sTime , 5 * ConstUtils.ONE_SECOND ) ) //Wait 20Sec
|
|
|
+ return false;
|
|
|
+
|
|
|
+ if ( isLeft )
|
|
|
+ {
|
|
|
+ if ( this.IsFrontLeft() && this.IsRearLeft() )
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ if ( this.IsFrontRight() && this.IsRearRight() )
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ return true;
|
|
|
+ } ).ContinueWith( t =>
|
|
|
+ {
|
|
|
+ if ( !t.Result )
|
|
|
+ this.OnSteeringError?.BeginInvoke( this , 33 , null , null );
|
|
|
+ else
|
|
|
+ this.OnSteeringError?.BeginInvoke( this , 0 , null , null );
|
|
|
+
|
|
|
+ GetSteeringState();
|
|
|
+ this.isExecuteSteering = false;
|
|
|
+ } );
|
|
|
+
|
|
|
return 0;
|
|
|
}
|
|
|
-
|
|
|
- void SetHightVolt()
|
|
|
+
|
|
|
+ void SetHightVolt( )
|
|
|
{
|
|
|
- this.iO.WriteOutputIO("OUT_F_STEERING_DA", true);
|
|
|
- this.iO.WriteOutputIO("OUT_R_STEERING_DA", true);
|
|
|
- this.iO.WriteOutputIO("OUT_F_STEERING_DA", false, 500);
|
|
|
- this.iO.WriteOutputIO("OUT_R_STEERING_DA", false, 500);
|
|
|
+ this.iO.WriteOutputIO( "OUT_F_STEERING_DA" , true );
|
|
|
+ this.iO.WriteOutputIO( "OUT_R_STEERING_DA" , true );
|
|
|
+ this.iO.WriteOutputIO( "OUT_F_STEERING_DA" , false , 500 );
|
|
|
+ this.iO.WriteOutputIO( "OUT_R_STEERING_DA" , false , 500 );
|
|
|
}
|
|
|
|
|
|
- bool IsFrontLeft() => this.iO.IsOn("IN_F_STEERING_DETECT_LEFT");
|
|
|
- bool IsFrontRight() => this.iO.IsOn("IN_F_STEERING_DETECT_RIGHT");
|
|
|
- bool IsRearLeft() => this.iO.IsOn("IN_R_STEERING_DETECT_LEFT");
|
|
|
- bool IsRearRight() => this.iO.IsOn("IN_R_STEERING_DETECT_RIGHT");
|
|
|
+ bool IsFrontLeft( ) => this.iO.IsOn( "IN_F_STEERING_DETECT_LEFT" );
|
|
|
+ bool IsFrontRight( ) => this.iO.IsOn( "IN_F_STEERING_DETECT_RIGHT" );
|
|
|
+ bool IsRearLeft( ) => this.iO.IsOn( "IN_R_STEERING_DETECT_LEFT" );
|
|
|
+ bool IsRearRight( ) => this.iO.IsOn( "IN_R_STEERING_DETECT_RIGHT" );
|
|
|
|
|
|
- public bool IsLeft() => this.IsFrontLeft() && this.IsRearLeft();
|
|
|
- public bool IsRight() => this.IsFrontRight() && this.IsRearRight();
|
|
|
+ public bool IsLeft( ) => this.IsFrontLeft() && this.IsRearLeft();
|
|
|
+ public bool IsRight( ) => this.IsFrontRight() && this.IsRearRight();
|
|
|
}
|
|
|
}
|