//// *************************************
// Function for Right 90 degree turns // ****************
void skeleton_onRightQuarterTurn(object sender, SkeletonSlam.SkeletonArgs e)
{
if (tcpClient != null && tcpClient.Status == TCPSlamClient.ClientStatus.Connected)
{
// mine //*******************************
startTurn = DateTime.Now;
endTurn = DateTime.Now;
TimeSpan timeLapse = endTurn - startTurn;
Console.WriteLine("Change the values");
//***************************************
//tcpClient.SendData(TCPSlamBase.MessageType.LeftMotor, BitConverter.GetBytes(e.LHandHeight));
//tcpClient.SendData(TCPSlamBase.MessageType.RightMotor, BitConverter.GetBytes(e.RHandHeight));
tcpClient.SendData(TCPSlamBase.MessageType.LeftMotor, BitConverter.GetBytes(0.5));
tcpClient.SendData(TCPSlamBase.MessageType.RightMotor, BitConverter.GetBytes(-0.5));
if (cameraWindow != null)
{
//Console.WriteLine("The values should be changed");
cameraWindow.Dispatcher.Invoke(DispatcherPriority.Normal, new Action(delegate() { cameraWindow.SetLeftWheelPower(0.5); }));
cameraWindow.Dispatcher.Invoke(DispatcherPriority.Normal, new Action(delegate() { cameraWindow.SetRightWheelPower(-0.5); }));
cameraWindow.Dispatcher.Invoke(DispatcherPriority.Normal, new Action(delegate() { cameraWindow.UpdateLineGraphs(); }));
}
do
{
endTurn = DateTime.Now;
timeLapse = endTurn - startTurn;
//Console.WriteLine("timeLapse = " + timeLapse);
Console.WriteLine("Right Quarter turn: " + timeLapse);
} while (timeLapse.Seconds < 2.5);
Console.WriteLine("Back to normal.");
}
}
private void Window_Loaded(object sender, RoutedEventArgs e)
{
skeleton = new SkeletonSlam();
skeleton.onHandHeightChanged += new EventHandler<SkeletonSlam.SkeletonArgs>(skeleton_onHandHeightChanged);
skeleton.onRightAngleTurn += new EventHandler<SkeletonSlam.SkeletonArgs>(skeleton_onRightAngleTurn);
skeleton.onLeftQuarterTurn += new EventHandler<SkeletonSlam.SkeletonArgs>(skeleton_onLeftQuarterTurn);
skeleton.onRightQuarterTurn += new EventHandler<SkeletonSlam.SkeletonArgs>(skeleton_onRightQuarterTurn);
skeleton.onLeftHalfTurn += new EventHandler<SkeletonSlam.SkeletonArgs>(skeleton_onLeftHalfTurn);
skeleton.onLeftFullTurn += new EventHandler<SkeletonSlam.SkeletonArgs>(skeleton_onLeftFullTurn);
skeleton.onRightHalfTurn += new EventHandler<SkeletonSlam.SkeletonArgs>(skeleton_onRightHalfTurn);
skeleton.onRightFullTurn += new EventHandler<SkeletonSlam.SkeletonArgs>(skeleton_onRightFullTurn);
controller = new ControllerSlam();
controller.OnButtonsChanged += new EventHandler<ControllerSlam.ButtonArgs>(controller_OnButtonsChanged);
tcpClient = new TCPSlamClient();
tcpClient.Port = 9988;
tcpClient.OnConnectionStatusChanged += new EventHandler<TCPSlamClient.ClientStatusArgs>(tcpClient_OnConnectionStatusChanged);
tcpClient.OnDataReceived += new EventHandler<TCPSlamBase.MessageArgs>(tcpClient_OnDataReceived);
txtIP.Text = Common.GetIP();
}
void skeleton_onHandHeightChanged(object sender, SkeletonSlam.SkeletonArgs e)
{
if (tcpClient != null && tcpClient.Status == TCPSlamClient.ClientStatus.Connected)
{
//for(int i =0; i<10; i++)
// Console.WriteLine("R-HandHeight: " + e.RHandHeight);
tcpClient.SendData(TCPSlamBase.MessageType.LeftMotor, BitConverter.GetBytes(e.LHandHeight));
tcpClient.SendData(TCPSlamBase.MessageType.RightMotor, BitConverter.GetBytes(e.RHandHeight));
if (cameraWindow != null)
{
cameraWindow.Dispatcher.Invoke(DispatcherPriority.Normal, new Action(delegate() { cameraWindow.SetLeftWheelPower(e.LHandHeight); }));
cameraWindow.Dispatcher.Invoke(DispatcherPriority.Normal, new Action(delegate() { cameraWindow.SetRightWheelPower(e.RHandHeight); }));
cameraWindow.Dispatcher.Invoke(DispatcherPriority.Normal, new Action(delegate() { cameraWindow.UpdateLineGraphs(); }));
}
}
}