public NavigationManager()
{
movement = Movement.stop;
monitor = new object();
actualPosition = new Position();
//TODO
//Waiting for the magnetometer...
Thread.Sleep(1500);
initialHeading = this.MAG_Heading;
contingency = new Contingency(left, right, this);
//integration = new Timer(new TimerCallback(this.Integrate), new object(), GlobalVal.integrationPeriod * 10, GlobalVal.integrationPeriod);
}
public void SetObject(Position position)
{
//map[position.x][position.y].isObject = true;
}
public void setActualPosition(Position p)
{
this.actualPosition.x = p.x;
this.actualPosition.y = p.y;
this.actualPosition.angle = p.angle;
}
public void SetMapped(Position position)
{
//map[position.x][position.y].mapped = true;
}
public bool isMapped(Position position)
{
//return map[position.x][position.y].mapped;
return false;
}
/*public short calibrarZ(short cz)
{
return cz;
}*/
private Engine()
{
actualPosition = new Position();
}
public void UpdatePosition(Position p)
{
Position _p = navigation.getActualPosition();
Debug("Position: " + _p.x + " : " + _p.y + " : " + _p.angle + " | " + p.x + " :" + p.y + " : " + p.angle);
navigation.setActualPosition(p);
}
public void DataReceived(byte[] buffer, int offset, int length, TransportAddress t)
{
byte[] tmpBuff = new byte[length];
for (int i = 0; i < length; i++)
{
tmpBuff[i] = buffer[offset + i];
}
switch ((char)tmpBuff[0])
{
case 'f':
Engine.getInstance().ManualForward(ToShort(tmpBuff, 1));
break;
case 'b':
Engine.getInstance().ManualBackward(ToShort(tmpBuff, 1));
break;
case 'r':
Engine.getInstance().ManualRight();
break;
case 'l':
Engine.getInstance().ManualLeft();
break;
case 's':
Engine.getInstance().ManualStop();
break;
case 'm':
Engine.getInstance().ManualMode();
break;
case 'M':
Engine.getInstance().StopManualMode();
break;
case 'V':
Engine.getInstance().ManualSpeed(ToShort(tmpBuff, 1));
break;
case 'v':
Engine.getInstance().ManualTurningSpeed(ToShort(tmpBuff, 1));
break;
case 'p':
int readen = 1;
int temp;
Position p = new Position();
double x = ToDouble(tmpBuff, readen, out temp);
readen += temp;
//Engine.getInstance().Debug("X: " + x.ToString());
double y = ToDouble(tmpBuff, readen, out temp);
readen += temp;
double angle = ToDouble(tmpBuff, readen, out temp);
readen += temp;
p.x = x;
p.y = y;
p.angle = angle;
Engine.getInstance().UpdatePosition(p);
break;
case 'c':
short value = 0;
switch ((char)tmpBuff[1])
{
case 'x':
value = ToShort(tmpBuff, 2);
Engine.getInstance().calibrarX(value);
break;
case 'y':
value = ToShort(tmpBuff, 2);
Engine.getInstance().calibrarY(value);
break;
}
break;
}
}