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");
}
private void AdjustSensorPos(SensorPos pos)
{
int currTacho = motorSensor.GetTachoCount();
motorSensorOffset = currTacho - sensorPositions[pos];
}
public float GetDistance(SensorPos direction)
{
return(sensorInfo[(int)direction].distance);
}
