private void HardwareOff()
{
VerticalPistons.ForEach(b =>
{
b.Velocity = 0f;
b.SetValue <float>("MaxImpulseAxis", PISTON_MIN_FORCE);
b.SetValue <float>("MaxImpulseNonAxis", PISTON_MIN_FORCE);
b.Enabled = false;
});
HorizontalPistons.ForEach(b =>
{
b.Velocity = 0f;
b.SetValue <float>("MaxImpulseAxis", PISTON_MIN_FORCE);
b.SetValue <float>("MaxImpulseNonAxis", PISTON_MIN_FORCE);
b.Enabled = false;
});
AzimuthRotor.TargetVelocityRad = 0f;
AzimuthRotor.RotorLock = true;
AzimuthRotor.Torque = 0f;
AzimuthRotor.BrakingTorque = 0f;
AzimuthRotor.Enabled = false;
CurrentProgramm = DrillProgrammEnum.None;
ProgrammState = ProgrammStateEnum.None;
}
private void ContinueWork()
{
Drills.ForEach(b => b.Enabled = true);
switch (ProgrammState)
{
case ProgrammStateEnum.Turning:
{
SetAngleSpeed(DRILL_VELOCITY);
break;
}
case ProgrammStateEnum.MoveVertical:
{
SetDepthSpeed(DRILL_VELOCITY);
break;
}
case ProgrammStateEnum.MoveHorizontal:
{
SetRadiusSpeed(DRILL_VELOCITY);
break;
}
}
CurrentProgramm = DrillProgrammEnum.Drill;
}
private void PauseWork()
{
Drills.ForEach(b => b.Enabled = false);
StopMotion();
CurrentProgramm = DrillProgrammEnum.Paused;
}
private void MoveDown()
{
DesiredDepth = MAX_PISTONS_DEPTH;
SetDepthSpeed(FREE_VELOCITY);
CurrentProgramm = DrillProgrammEnum.Move;
ProgrammState = ProgrammStateEnum.MoveVertical;
}
private void MoveUp()
{
DesiredDepth = 0f;
SetDepthSpeed(FREE_VELOCITY);
CurrentProgramm = DrillProgrammEnum.Move;
ProgrammState = ProgrammStateEnum.MoveVertical;
}
private void MoveBack()
{
DesiredRadius = 0;
SetRadiusSpeed(FREE_VELOCITY);
CurrentProgramm = DrillProgrammEnum.Move;
ProgrammState = ProgrammStateEnum.MoveHorizontal;
}
private void MoveForward()
{
DesiredRadius = MAX_PISTONS_RADIUS;
SetRadiusSpeed(FREE_VELOCITY);
CurrentProgramm = DrillProgrammEnum.Move;
ProgrammState = ProgrammStateEnum.MoveHorizontal;
}
private void MoveRight()
{
DesiredAngle = MAX_ANGLE;
SetAngleSpeed(FREE_VELOCITY);
CurrentProgramm = DrillProgrammEnum.Move;
ProgrammState = ProgrammStateEnum.Turning;
}
private void FullStop()
{
StopMotion();
Drills.ForEach(b => b.Enabled = false);
CurrentProgramm = DrillProgrammEnum.None;
ProgrammState = ProgrammStateEnum.None;
}
private void GotoStartPos()
{
DesiredRadius = radius_min;
DesiredDepth = 0f; // depth_min; через верхнюю точку, в конце опустим
DesiredAngle = angle_min;
Drills.ForEach(b => b.Enabled = false);
SetDepthSpeed(FREE_VELOCITY);
HorizontalPistons.ForEach(b => b.Velocity = 0f);
AzimuthRotor.TargetVelocityRad = 0f;
CurrentProgramm = DrillProgrammEnum.GotoBegin;
ProgrammState = ProgrammStateEnum.MoveVertical;
}
private void GotoHome()
{
DesiredRadius = 0f;
DesiredDepth = 0f;
DesiredAngle = 0f;
Drills.ForEach(b => b.Enabled = false);
SetDepthSpeed(FREE_VELOCITY);
HorizontalPistons.ForEach(b => b.Velocity = 0f);
AzimuthRotor.TargetVelocityRad = 0f;
CurrentProgramm = DrillProgrammEnum.GotoHome;
ProgrammState = ProgrammStateEnum.MoveVertical;
}
private bool LoadState()
{
string[] strs = Storage.Split('\n');
if (strs.Length < 14)
{
return(false);
}
//0 - версия
int saveFormatVersion = Int32.Parse(strs[0]);
//1-2 - программа
CurrentProgramm = (DrillProgrammEnum)Int32.Parse(strs[1]);
ProgrammState = (ProgrammStateEnum)Int32.Parse(strs[2]);
//3-8 - параметры программы
angle_min = float.Parse(strs[3]);
angle_max = float.Parse(strs[4]);
radius_min = float.Parse(strs[5]);
radius_max = float.Parse(strs[6]);
depth_min = float.Parse(strs[7]);
depth_max = float.Parse(strs[8]);
//9-11 - текущие целевые значения
DesiredAngle = float.Parse(strs[9]);
DesiredRadius = float.Parse(strs[10]);
DesiredDepth = float.Parse(strs[11]);
//12-14 - последние заданные скорости
rotor_spd = float.Parse(strs[12]);
rad_spd = float.Parse(strs[13]);
vert_spd = float.Parse(strs[14]);
//15-16 - направление
radius_direction = bool.Parse(strs[15]);
turn_direction = bool.Parse(strs[16]);
return(true);
}
private void StartDrill()
{
DesiredDepth = CurrentDepth();
DesiredRadius = CurrentRadius();
if (turn_direction)
{
DesiredAngle = angle_max;
}
else
{
DesiredAngle = angle_min;
}
Drills.ForEach(b => b.Enabled = true);
VerticalPistons.ForEach(b => b.Velocity = 0f);
HorizontalPistons.ForEach(b => b.Velocity = 0f);
SetAngleSpeed(DRILL_VELOCITY);
CurrentProgramm = DrillProgrammEnum.Drill;
ProgrammState = ProgrammStateEnum.Turning;
}
private void WaitCargoFull()
{
PauseWork();
CurrentProgramm = DrillProgrammEnum.WaitFull;
}
