private void MoveSensor(SensorPos pos) { WaitForSensorReady(); if (pos == SensorPos.PARK) { ParkSensorArm(); } else { waitSensor.Reset(); int currTacho = motorSensor.GetTachoCount(); int move = sensorPositions[pos] + motorSensorOffset - currTacho; sbyte speed = move < 0 ? (sbyte)-35 : (sbyte)35; /*WaitHandle waitHandle = */ motorSensor.SpeedProfile(speed, 0, (uint)Math.Abs(move - 5), 5, true); Task.Factory.StartNew(() => { if (Math.Abs(move) > 200) { Thread.Sleep(Math.Abs(move) * 4); } else { Thread.Sleep(Math.Abs(move) * 15); } //waitHandle.WaitOne(); waitSensor.Set(); }); } }
void EmitSensor(SensorPos direction) { int index = (int)direction; RaycastHit hit; Physics.Raycast(castFrom.position, sensorInfo[index].coordinates, out hit, 45.0f); SetRaycastData(ref hit, index); }
private void DoAdjust() { SensorPos pos = SensorPos.READY; MoveSensor(pos); bool adjusted = false; for (;;) { switch (Buttons.GetKeypress()) { case Buttons.ButtonStates.Right: if (adjusted) { AdjustSensorPos(pos); adjusted = false; } if (pos == SensorPos.FIRST || pos == SensorPos.THIRD) { TwistBasket(true, 1, 0); } pos = GetNextPos(pos); MoveSensor(pos); var c = ScanColor(); LcdConsole.WriteLine("Scan: {0}", c); break; case Buttons.ButtonStates.Left: { /*var c2 = ScanColor(); * LcdConsole.WriteLine("Scan: {0}", c2); * Console.WriteLine("Scan: {0}", c2); * Console.WriteLine(motorSensor.GetTachoCount());*/ MoveSensor(SensorPos.READY); } break; case Buttons.ButtonStates.Up: motorSensor.SpeedProfile(25, 3, 12, 3, true); adjusted = true; break; case Buttons.ButtonStates.Down: motorSensor.SpeedProfile(-25, 3, 12, 3, true); adjusted = true; break; case Buttons.ButtonStates.Enter: return; } } }
private SensorPos GetNextPos(SensorPos p) { switch (p) { case SensorPos.READY: return(SensorPos.SECOND); case SensorPos.FIRST: return(SensorPos.SECOND); case SensorPos.SECOND: return(SensorPos.THIRD); case SensorPos.THIRD: return(SensorPos.FIRST); } throw new Exception("Unexpected sensor posisition"); }
private void AdjustSensorPos(SensorPos pos) { int currTacho = motorSensor.GetTachoCount(); motorSensorOffset = currTacho - sensorPositions[pos]; }
private void MoveSensor(SensorPos pos) { WaitForSensorReady(); if (pos == SensorPos.PARK) { ParkSensorArm(); } else { waitSensor.Reset(); int currTacho = motorSensor.GetTachoCount(); int move = sensorPositions[pos] + motorSensorOffset - currTacho; sbyte speed = move < 0 ? (sbyte) -35 : (sbyte) 35; /*WaitHandle waitHandle = */motorSensor.SpeedProfile(speed, 0, (uint) Math.Abs(move - 5), 5, true); Task.Factory.StartNew(() => { if (Math.Abs(move) > 200) Thread.Sleep(Math.Abs(move)*4); else Thread.Sleep(Math.Abs(move)*15); //waitHandle.WaitOne(); waitSensor.Set(); }); } }
private SensorPos GetNextPos(SensorPos p) { switch (p) { case SensorPos.READY: return SensorPos.SECOND; case SensorPos.FIRST: return SensorPos.SECOND; case SensorPos.SECOND: return SensorPos.THIRD; case SensorPos.THIRD: return SensorPos.FIRST; } throw new Exception("Unexpected sensor posisition"); }
public float GetDistance(SensorPos direction) { return(sensorInfo[(int)direction].distance); }