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 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 bool CheckProgramm()
{
switch (CurrentProgramm)
{
case DrillProgrammEnum.None:
{
return(false);
}
case DrillProgrammEnum.GotoHome:
case DrillProgrammEnum.GotoBegin:
{
if (CheckProgrammState())
{
switch (ProgrammState)
{
case ProgrammStateEnum.MoveVertical:
{
ProgrammState = ProgrammStateEnum.TurningMoveHorisontal;
SetRadiusSpeed(FREE_VELOCITY);
SetAngleSpeed(FREE_VELOCITY);
return(false);
}
case ProgrammStateEnum.TurningMoveHorisontal:
{
if (CurrentProgramm == DrillProgrammEnum.GotoHome)
{
ProgrammState = ProgrammStateEnum.None;
return(true);
}
else //CurrentProgramm == DrillProgrammEnum.GotoBegin
{
ProgrammState = ProgrammStateEnum.MoveDown;
DesiredDepth = depth_min;
SetDepthSpeed(FREE_VELOCITY);
return(false);
}
}
case ProgrammStateEnum.MoveDown:
{
ProgrammState = ProgrammStateEnum.None;
return(true);
}
}
}
else
{
if (ProgrammState == ProgrammStateEnum.TurningMoveHorisontal)
{
SetAngleSpeed(FREE_VELOCITY);
}
}
return(false);
}
case DrillProgrammEnum.Drill:
{
if (CheckProgrammState())
{
switch (ProgrammState)
{
case ProgrammStateEnum.Turning:
{
//закончили поворот, сменим направление и пока остановим
//AzimuthRotor.TargetVelocityRad = 0f;
turn_direction = !turn_direction;
if (turn_direction)
{
DesiredAngle = angle_max;
}
else
{
DesiredAngle = angle_min;
}
//сделаем шаг по радиусу
if (radius_direction)
{
DesiredRadius += H_STEP;
if (DesiredRadius > radius_max)
{
DesiredRadius = radius_max;
}
}
else
{
DesiredRadius -= H_STEP;
if (DesiredRadius < radius_min)
{
DesiredRadius = radius_min;
}
}
if (CheckRadius(false))
{
//дошли до конца, сменим направление
radius_direction = !radius_direction;
//if (radius_direction) DesiredRadius = radius_max; else DesiredRadius = radius_min;
//и заглубимся
DesiredDepth += V_DIRECTION * V_STEP;
if (DesiredDepth > depth_max)
{
DesiredDepth = depth_max;
}
if (CheckDepth(false))
{
//мы на максимальной глубине, конец программы
//GotoHome();
return(true);
}
else
{
ProgrammState = ProgrammStateEnum.MoveVertical;
SetDepthSpeed(DRILL_VELOCITY);
return(false);
}
}
else
{
ProgrammState = ProgrammStateEnum.MoveHorizontal;
SetRadiusSpeed(DRILL_VELOCITY);
return(false);
}
}
case ProgrammStateEnum.MoveVertical:
{
ProgrammState = ProgrammStateEnum.Turning;
SetAngleSpeed(DRILL_VELOCITY);
return(false);
}
case ProgrammStateEnum.MoveHorizontal:
{
ProgrammState = ProgrammStateEnum.Turning;
SetAngleSpeed(DRILL_VELOCITY);
return(false);
}
}
}
return(false);
}
case DrillProgrammEnum.Move:
{
if (CheckProgrammState())
{
switch (ProgrammState)
{
case ProgrammStateEnum.Turning:
case ProgrammStateEnum.MoveVertical:
case ProgrammStateEnum.MoveHorizontal:
case ProgrammStateEnum.MoveDown:
return(true);
default:
break;
}
}
return(false);
}
default:
break;
}
return(false);
}
