public static void Main()
{
Debug.Print("BNO055 Test Application.");
//
_resetPort.Write(false);
Thread.Sleep(10);
_resetPort.Write(true);
Thread.Sleep(50);
//
_comPort.Open();
WriteText("Orientation Sensor Test");
WriteText("");
WriteText("Sensor: BNO050");
WriteText("Driver Ver: 1");
WriteText("Unique ID: ");
WriteText("Max Value: 0.0");
WriteText("Min Value: 0.0");
WriteText("Resolution: 0.01");
WriteText("");
Thread.Sleep(500);
//
var bno055 = new BNO055(0x29);
bno055.DisplayRegisters();
bno055.PowerMode = BNO055.PowerModes.Normal;
bno055.OperatingMode = BNO055.OperatingModes.ConfigurationMode;
bno055.OperatingMode = BNO055.OperatingModes.InertialMeasurementUnit;
bno055.DisplayRegisters();
Debug.Print("Current temperature: " + bno055.Temperature.ToString("f2"));
while (true)
{
bno055.Read();
var reading = bno055.EulerOrientation;
string orientationMessage = "Orientation: " + reading.Heading + " " + (-1 * reading.Roll) + " " + reading.Pitch;
string calibrationMessage = "Calibration: " + BooleanToIntString(bno055.IsSystemCalibrated) + " " +
BooleanToIntString(bno055.IsGyroscopeCalibrated) + " " +
BooleanToIntString(bno055.IsAccelerometerCalibrated) + " " +
BooleanToIntString(bno055.IsMagnetometerCalibrated);
Debug.Print(orientationMessage);
Debug.Print(calibrationMessage);
WriteText(orientationMessage);
WriteText(calibrationMessage);
Thread.Sleep(100);
}
}
private void MoveToByAngles(int horAngle, int verAngle)
{
float[] position = positionSensor.Read();
int[] gimbalAngles = gimbal.GetCurrentPosition();
TurnToByCamera(horAngle);
int tmpAngle = Math.Abs(gimbalAngles[0]);
int sign = Math.Sign(gimbalAngles[0]);
float deviceAngle = sign * position[1] * (tmpAngle / quarterCircle) + position[2] * (1 - (tmpAngle / quarterCircle));
float groundAngle = -1 * verAngle + deviceAngle + gimbalAngles[1];
float distance = cameraHeigth / (float)Math.Tan(groundAngle);
//MoveForwardBy(defaultSpeed, distance, true, defaultGear);
#if DEBUG
Console.WriteLine($"Moving by {distance}");
#endif
MoveForwardBy(defaultSpeed, distance, true, defaultGear);
}
