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