//rtgpsout.unavco.org:2101
//69.44.86.36
/// <summary>
/// </summary>
/// <param name="ntripIpAddress"></param>
/// <param name="ntripPort"></param>
/// <param name="ntripMountPoint"></param>
/// <param name="userName"></param>
/// <param name="password"></param>
/// <param name="display"></param>
public NtripClient(string ntripIpAddress, int ntripPort, string ntripMountPoint, string userName, string password, SparkFunSerial16X2Lcd display)
{
_username = userName;
_password = password;
_ntripPort = ntripPort;
_ntripMountPoint = ntripMountPoint;
_display = display;
try
{
IPAddress.TryParse(ntripIpAddress, out _ip);
if (_ip == null)
return;
_endPoint = new IPEndPoint(_ip, _ntripPort);
}
catch (Exception)
{
//
}
}
private readonly Stopwatch _dpadStopwatch = new Stopwatch();//Basically a "debounce"
internal Hexapi(IkController ikController,
XboxController xboxController,
Navigator navigator,
SparkFunSerial16X2Lcd display,
Gps.Gps gps,
IoTClient ioTClient)
{
_ik = ikController;
_xboxController = xboxController;
_gps = gps;
_navigator = navigator;
_display = display;
_ioTClient = ioTClient;
_functionStopwatch.Start();
_dpadStopwatch.Start();
SetGaitOptions();
XboxController.LeftDirectionChanged += XboxController_LeftDirectionChanged;
XboxController.RightDirectionChanged += XboxController_RightDirectionChanged;
XboxController.DpadDirectionChanged += XboxController_DpadDirectionChanged;
XboxController.LeftTriggerChanged += XboxController_LeftTriggerChanged;
XboxController.RightTriggerChanged += XboxController_RightTriggerChanged;
XboxController.FunctionButtonChanged += XboxController_FunctionButtonChanged;
XboxController.BumperButtonChanged += XboxController_BumperButtonChanged;
XboxController.DisconnectedEvent += XboxController_DisconnectedEvent; ;
}
internal IkController(InverseKinematics inverseKinematics,
SparkFunSerial16X2Lcd display,
IoTClient ioTClient,
Gps.Gps gps)
{
_inverseKinematics = inverseKinematics;
_display = display;
_ioTClient = ioTClient;
_gps = gps;
_perimeterInInches = 15;
_selectedIkFunction = SelectedIkFunction.BodyHeight;
_behavior = Behavior.Bounce;
}
public async void Run(IBackgroundTaskInstance taskInstance)
{
_deferral = taskInstance.GetDeferral();
_display = new SparkFunSerial16X2Lcd();
_xboxController = new XboxController(_display);
_ntripClient = new NtripClient("172.16.0.237", 8000, "", "", "", _display); //172.16.0.227
_ioTClient = new IoTClient(_display);
_gps = new Gps.Gps( _display, _ntripClient);
_inverseKinematics = new InverseKinematics(_display);
_ikController = new IkController(_inverseKinematics, _display, _ioTClient, _gps); //Range and yaw/pitch/roll data from Arduino and SparkFun Razor IMU
_navigator = new Navigator(_ikController, _display, _gps);
_hexapi = new Hexapi(_ikController, _xboxController, _navigator, _display, _gps, _ioTClient);
_initializeTasks.Add(_display.InitializeAsync());
_initializeTasks.Add(_xboxController.InitializeAsync());
_initializeTasks.Add(_ikController.InitializeAsync());
_initializeTasks.Add(_ntripClient.InitializeAsync());
_initializeTasks.Add(_gps.InitializeAsync());
_initializeTasks.Add(_inverseKinematics.InitializeAsync());
_initializeTasks.Add(_ioTClient.InitializeAsync());
_startTasks.Add(_ntripClient.StartAsync());
_startTasks.Add(_ikController.StartAsync());
_startTasks.Add(_gps.StartAsync());
_startTasks.Add(_inverseKinematics.StartAsync());
//_startTasks.Add(_ioTClient.StartAsync());//only needed if expecting messages from the server
//foreach (var d in await SerialDeviceHelper.ListAvailablePorts())
//{
// await _ioTClient.SendEvent(d);
//}
await Task.WhenAll(_initializeTasks.ToArray());
await Task.WhenAll(_startTasks.ToArray());
}
internal XboxController(SparkFunSerial16X2Lcd display)
{
_display = display;
}
internal IoTClient(SparkFunSerial16X2Lcd display)
{
_display = display;
}
internal XboxController(SparkFunSerial16X2Lcd display)
{
_display = display;
}
internal Gps(SparkFunSerial16X2Lcd display, NtripClient ntripClientTcp = null)
{
_display = display;
_ntripClientTcp = ntripClientTcp;
}
internal Navigator(IkController ikController, SparkFunSerial16X2Lcd display, Gps.Gps gps)
{
_ikController = ikController;
_gps = gps;
_display = display;
}
internal InverseKinematics(SparkFunSerial16X2Lcd display)
{
IkController.RangingEvent += RangingEventHandler;
IkController.ImuEvent += ImuEventHandler;
_display = display;
_pi1K = Pi*1000D;
for (var i = 0; i < 6; i++)
LegServos[i] = new int[3];
_movementStarted = false;
for (var legIndex = 0; legIndex <= 5; legIndex++)
{
_legPosX[legIndex] = _initPosX[legIndex]; //Set start positions for each leg
_legPosY[legIndex] = _initPosY[legIndex];
_legPosZ[legIndex] = _initPosZ[legIndex];
LegYHeightCorrector[legIndex] = 0;
}
_oscillateStopwatch.Start();
}