public void updatePosition(double actualDistance, double tickSpeed,
Position targetPosition, Position actualPosition, Position expectedPosition,
double expectedXTickDistance, double expectedYTickDistance, double expectedZTickDistance,
double theta, double phi, double radius)
{
if (actualDistance > tickSpeed)
{
// Move towards the goal using where we think we are
// and allowing that to affect the actual positioning
if (shouldMeasure())
{
getExpectedPosition(actualPosition, expectedPosition,
expectedXTickDistance, expectedYTickDistance, expectedZTickDistance,
theta, phi, radius);
}
else
{
// Else continue on your wrong path
actualPosition.x += expectedXTickDistance;
actualPosition.y += expectedYTickDistance;
actualPosition.z += expectedZTickDistance;
expectedPosition.x += expectedXTickDistance;
expectedPosition.y += expectedYTickDistance;
expectedPosition.z += expectedZTickDistance;
}
}
else
{
// Last step
if (actualDistance > tickSpeed)
{
actualPosition.x += expectedXTickDistance;
actualPosition.y += expectedYTickDistance;
actualPosition.z += expectedZTickDistance;
expectedPosition.Clone(actualPosition);
}
else
{
actualPosition.Clone(targetPosition);
}
}
}
private void getExpectedPosition(Position actualPosition, Position expectedPosition,
double expectedXTickDistance, double expectedYTickDistance, double expectedZTickDistance,
double theta, double phi, double radius)
{
if ((activeRSSI) && (rssi.remoteMeasurementTransmitRequest(ticks))) {
// We need to request a measurement
// Nothing really to do at the moment but right here's where we could remember "when" the request was made
}
if ((activeRSSI) && (rssi.remoteMeasurementRequestReceived(ticks))) {
// We need to request a measurement
calculatedTrilateratePosition = rssi.trilaterate(actualPosition);
}
if (activePerfectPosition)
{
actualPosition.x += expectedXTickDistance;
actualPosition.y += expectedYTickDistance;
actualPosition.z += expectedZTickDistance;
expectedPosition.Clone(actualPosition);
}
else if ((activeGPS) && (gps.shouldMeasure(ticks)))
{
actualPosition.x += expectedXTickDistance;
actualPosition.y += expectedYTickDistance;
actualPosition.z += expectedZTickDistance;
gps.Calculate(expectedPosition, actualPosition);
}
else if ( (activeRSSI) && (rssi.remoteMeasurementRequestReturned(ticks)) && (calculatedTrilateratePosition!= null) )
{
actualPosition.x += expectedXTickDistance;
actualPosition.y += expectedYTickDistance;
actualPosition.z += expectedZTickDistance;
expectedPosition.Clone(calculatedTrilateratePosition);
// Reset the IMU for new theta
imu.reset();
}
else if (activeIMU)
{
// Constantly apply
imu.Calculate(actualPosition, expectedPosition, expectedXTickDistance, expectedYTickDistance, expectedZTickDistance,
theta, phi, radius);
}
else
{
throw new Exception("ERROR: No other position measuring devices exist!");
}
}
