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); }