//code adapted from https://www.hackster.io/oztl/near-perfect-gyroscope-e5e10e
/// <summary>
/// Create new MPU 6050 object and initiate device
/// </summary>
/// <param name="busID">Serial bus ID on ESP32 device (1 or 2)</param>
/// <returns></returns>
public static Mpu6050 StartNew(int busID)
{
var tmp = new Mpu6050(busID);
tmp.Start();
return(tmp);
}
public static void Main()
{
_mpu = Mpu6050.StartNew(2);
_mpuArgList = new ArrayList()
{
"", "", ""
};
_led = new PinController.LedBlinker(2);
_vibe = new PinController.LedBlinker(13);
Debug.WriteLine("Init");
_led.On();
Thread.Sleep(100);
_led.Off();
var AppID = "DDRTwistNShout";
var serverUdpPort = 7348;
var nanoConfig = new ArdNetClientConfig(AppID, null, serverUdpPort);
nanoConfig.TCP.HeartbeatConfig.HeartbeatInterval = TimeSpan.FromMilliseconds(2000);
nanoConfig.TCP.HeartbeatConfig.ForceStrictHeartbeat = true;
nanoConfig.TCP.HeartbeatConfig.RespondToHeartbeats = false;
_ardManager = new ArdNetClientManager(SystemConfig.WiFiCredentials, nanoConfig);
_ardManager.TcpEndpointConnected += ArdManager_TcpEndpointConnected;
_ardManager.TcpEndpointDisconnected += NanoClient_ServerDisconnected;
_ardManager.StartWorkerThread();
_ardManager.CommandReceived += ArdManager_CommandReceived;
MpuValue data;
var spinCmd = TcpRequest.CreateOutbound("SPIN", _mpuArgList);
DateTime spinStartTime = DateTime.MinValue;
while (true)
{
data = _mpu.GetData();
//gyro data
//x, y, z
//_mpuArgList[0] = data.GyroX.ToString();
//_mpuArgList[1] = data.GyroY.ToString();
//_mpuArgList[2] = data.GyroZ.ToString();
//Debug.WriteLine($"[{data.GyroX} {data.GyroY} {data.GyroZ}]");
if (abs(data.GyroY) > 15000 && spinStartTime.Equals(DateTime.MinValue))
{
spinStartTime = DateTime.UtcNow;
}
else if (abs(data.GyroY) < 5000 && !spinStartTime.Equals(DateTime.MinValue))
{
var spinTime = (DateTime.UtcNow - spinStartTime).TotalMilliseconds;
Debug.WriteLine($"SPIN: {spinTime}");
spinStartTime = DateTime.MinValue;
if (spinTime > 800)
{
_ardManager.EnqueueTask(x => x.SendCommand(spinCmd));
}
}
Thread.Sleep(10);
}
}
