Bert - the phone controlled robot

using System;
using System.Net;
using System.Text;
using System.Net.Sockets;
using System.Threading;
using System.IO.Ports;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using SecretLabs.NETMF.Hardware;
using SecretLabs.NETMF.Hardware.Netduino;

namespace BertLogic
{
    class RunRobot
    {
        bool cmdListenState;             // whether or not we are listening to the input from the phone
        OutputPort led;                  // indicates the cmdListenState
        OutputPort turnLeft, turnRight;  // controlling the turn signals
        Thread leftBlink, rightBlink;    // blinking the tail-lights
        SerialPort hc06;                 // the BT radio
        byte[] cmdBuffer = new byte[32]; // the input buffer for receiveing commands
        char cmd = ' ';                  // the last command received
        OutputPort A1, A2, B1, B2;       // motor controller ports
        AnalogInput foreRange;           // forward range finder input
        AnalogInput backRange;           // rear range finder input
        InterruptPort button;            // the onboard button to turn on and off accepting command input
        Timer rangeReader;               // the timer for the two range finder inputs

        public RunRobot()
        {
            // set up the bluetooth radio communication
            hc06 = new SerialPort(SerialPorts.COM1, 9600, Parity.None, 8, StopBits.One);
            hc06.ReadTimeout = 1000;
            hc06.Open();

            //use the onboard LED to indicate that the robot is accepting commands
            led = new OutputPort(Pins.ONBOARD_LED, false);

            // set up turn signal lights
            turnLeft = new OutputPort(Pins.GPIO_PIN_D6, false);
            turnRight = new OutputPort(Pins.GPIO_PIN_D7, false);

            //use onboard button to turn on/off accepting commands
            button = new InterruptPort(Pins.ONBOARD_SW1, false, Port.ResistorMode.Disabled, Port.InterruptMode.InterruptEdgeHigh);

            //motor controller output
            A1 = new OutputPort(Pins.GPIO_PIN_D2, false);   //A is the left tires when viewed from the front
            A2 = new OutputPort(Pins.GPIO_PIN_D3, false);
            B1 = new OutputPort(Pins.GPIO_PIN_D4, false);   //B is the right tires when viewed from the front
            B2 = new OutputPort(Pins.GPIO_PIN_D5, false);

            // setup range finder for collision and edge avoidance
            foreRange = new AnalogInput(Cpu.AnalogChannel.ANALOG_5, 100, 0, -1);
            backRange = new AnalogInput(Cpu.AnalogChannel.ANALOG_4, 100, 0, -1);
        }

        public void Start()
        {
            // enable interrupts on BT input
            hc06.DataReceived += hc06_DataReceived;

            //start monitoring for button presses
            button.OnInterrupt += new NativeEventHandler(button_OnInterrupt);

            //turn signal threads setup
            leftBlink = new Thread(leftBlinkLogic);
            leftBlink.Start();
            Thread.Sleep(100);
            leftBlink.Suspend();
            rightBlink = new Thread(rightBlinkLogic);
            rightBlink.Start();
            Thread.Sleep(100);
            rightBlink.Suspend();
            Thread.Sleep(200);
        }
        private void hc06_DataReceived(object sender, SerialDataReceivedEventArgs e)
        {
            Debug.Print("Data Received");
            if (cmdListenState)
            {
                //read all input
                int count = 0;
                do
                {
                    try
                    {
                        count = hc06.Read(cmdBuffer, 0, cmdBuffer.Length);  // should read on byte at a time.
                    }
                    catch (Exception) { }        // just eat exceptions 
                    if (count > 0)
                    {
                            cmd = (char)cmdBuffer[count-1];
                            executeCmd(cmd);   //execute command immediately
                    }
                } while (count > 0);

            }
            else 
            {
                executeCmd('S');     //double check that the device is not left with the motors running somehow.
            }
        }

        private void button_OnInterrupt(uint data1, uint data2, DateTime time)
        {
            cmdListenState = !cmdListenState;

            if (cmdListenState)
            {
                // setup timer for rangeFinders
                rangeReader = new Timer(new TimerCallback(rangeTimerHandler), null, 0, 100); 
            }
            else
            {
                rangeReader.Dispose();              // drop the timer as we are not processing commansds so we shouldn't be moving
            }

            led.Write(cmdListenState);              // the onboard LED indicates whether Bert is accepting commands
        }


        private void executeCmd(char cmd)
        {
            /*         
            EA  A2  A1  Motor 1         EB  B2  B1  Motor 2 
            0   X   X   Shaft free      0   X   X   Shaft free 
            1   0   0   Shaft locked    1   0   0   Shaft locked 
            1   0   1   Clockwise       1   0   1   Clockwise 
            1   1   0   Anti-clockwise  1   1   0   Anti-clockwise 
            1   1   1   Shaft locked    1   1   1   Shaft locked  
            */
            leftBlink.Suspend();
            rightBlink.Suspend();
            turnRight.Write(false);
            turnLeft.Write(false);
            switch (cmd)
            {
                case 'F':
                    {
                        Debug.Print("Forward");
                        A1.Write(false);    //left forward = false/true
                        A2.Write(true);
                        B1.Write(true);     // right forward = true/false
                        B2.Write(false);
                        break;
                    }
                case 'L':
                    {
                        Debug.Print("Left");
                        leftBlink.Resume();
                        A1.Write(false);    //left forward = false/true
                        A2.Write(true);
                        B1.Write(false);     //right backward = false/true
                        B2.Write(true);
                        break;
                    }
                case 'R':
                    {
                        Debug.Print("Right");
                        rightBlink.Resume();
                        A1.Write(true);     //left backward = true/false
                        A2.Write(false);
                        B1.Write(true);    //right forward = true/false
                        B2.Write(false);
                        break;
                    }
                case 'B':
                    {
                        Debug.Print("Back");
                        A1.Write(true);     //left backward = true/false
                        A2.Write(false);
                        B1.Write(false);    //right backward = false/true
                        B2.Write(true);
                        break;
                    }
                case 'S':
                    {
                        Debug.Print("Stop");
                        A1.Write(true);
                        A2.Write(true);
                        B1.Write(true);
                        B2.Write(true);
                        break;
                    }
                default:
                    {
                        Debug.Print("Undefined input");
                        break;
                    }
            }
        }
        private void rangeTimerHandler(object o)
        {
            bool appStateHold = cmdListenState;
            // normally the device will be listening
            double reading = foreRange.Read();
            if (reading < 20 || reading > 50)
            {
                cmdListenState = false;  // we are not listening for commands during this
                executeCmd('B');
                Thread.Sleep(500);
                executeCmd('L');
                Thread.Sleep(1000);
                executeCmd('S');
                cmdListenState = appStateHold;   //restore listening state
            }
            reading = backRange.Read();
            if (reading > 30)
            {
                cmdListenState = false;   // we are not listening for commands during this
                executeCmd('F');
                Thread.Sleep(500);
                executeCmd('R');
                Thread.Sleep(1000);
                executeCmd('S');
                cmdListenState = appStateHold;   //restore listening state

            }
        }
        private void leftBlinkLogic()
        {
            while (true)
            {
                turnLeft.Write(true);
                Thread.Sleep(300);
                turnLeft.Write(false);
                Thread.Sleep(300);
            }
        }
        private void rightBlinkLogic()
        {
            while (true)
            {
                turnRight.Write(true);
                Thread.Sleep(300);
                turnRight.Write(false);
                Thread.Sleep(300);
            }
        }
    }
}

<Page
    x:Class="PhoneLogic.MainPage"
    xmlns="http://schemas.microsoft.com/winfx/2006/xaml/presentation"
    xmlns:x="http://schemas.microsoft.com/winfx/2006/xaml"
    xmlns:local="using:GyroTest"
    xmlns:d="http://schemas.microsoft.com/expression/blend/2008"
    xmlns:mc="http://schemas.openxmlformats.org/markup-compatibility/2006"
    mc:Ignorable="d"
    Background="{ThemeResource ApplicationPageBackgroundThemeBrush}">
    <Page.Resources>
        <!-- TODO: Delete this line if the key AppName is declared in App.xaml -->
        <x:String x:Key="AppName">Robot Driver</x:String>
        <SolidColorBrush x:Key="OnBrush" Color="Red"/>
        <SolidColorBrush x:Key="OffBrush" Color="Black"/>
    </Page.Resources>

    <Grid Background="{StaticResource ApplicationPageBackgroundThemeBrush}">
        <Grid.ChildrenTransitions>
            <TransitionCollection>
                <EntranceThemeTransition/>
            </TransitionCollection>
        </Grid.ChildrenTransitions>
        <Grid.RowDefinitions>
            <RowDefinition Height="40"/>
            <RowDefinition Height="Auto"/>
            <RowDefinition Height="Auto"/>
            <RowDefinition Height="Auto"/>
            <RowDefinition Height="Auto"/>
            <RowDefinition Height="Auto"/>
            <RowDefinition Height="*"/>
        </Grid.RowDefinitions>
        <Grid.ColumnDefinitions>
            <ColumnDefinition Width="50"/>
            <ColumnDefinition Width="Auto"/>
        </Grid.ColumnDefinitions >
        <TextBlock 
            x:Name="pageTitle" 
            Style="{StaticResource HeaderTextBlockStyle}" 
            Grid.Column="1" Grid.Row="1"
                Margin="0,0,30,20" 
            Text="{StaticResource AppName}"
            />
        <StackPanel 
            Orientation="Vertical" 
            Grid.Column="1" 
            Grid.Row="2" 
            Margin="0,0,0,10">
            <TextBlock 
                Style="{StaticResource BaseTextBlockStyle}" 
                Text="Use buttons to connect and disconnect"/>
            <TextBlock 
                Name="StatusText" 
                Style="{StaticResource BaseTextBlockStyle}" 
                Text="" FontSize="14" 
                Foreground="Silver"/>
        </StackPanel>
        <StackPanel Orientation="Horizontal" Grid.Column="1" Grid.Row="3">
            <Button Name="EnumerateButton" 
                    Click="EnumerateButton_Click">
                Enumerate
            </Button>
            <Button 
                Name="DisconnectButton" 
                IsEnabled="False" 
                Click="DisconnectButton_Click">
                Disconnect
            </Button>
        </StackPanel>
        <ListBox Name="ServicesList" Visibility="Collapsed" Grid.Column="1" Grid.Row="4" Width="200" Height="150" Tapped="ServicesList_Tapped" HorizontalAlignment="Left" VerticalAlignment="Top">
            <ListBox.ItemTemplate>
                <DataTemplate>
                    <TextBlock Text="{Binding Name}"/>
                </DataTemplate>
            </ListBox.ItemTemplate>
        </ListBox>
        <TextBlock 
            x:Name="txtForward" 
            Style="{StaticResource HeaderTextBlockStyle}" 
            Grid.Column="1" Grid.Row="4"
                Margin="120,20,160,10" 
            Text=" F"
            />
        <TextBlock 
            x:Name="txtLeft" 
            Style="{StaticResource HeaderTextBlockStyle}" 
            Grid.Column="1" Grid.Row="5"
                Margin="60,10,230,10" 
            Text=" L"
            />
        <TextBlock 
            x:Name="txtRight" 
            Style="{StaticResource HeaderTextBlockStyle}" 
            Grid.Column="1" Grid.Row="5"
                Margin="180,10,100,10" 
            Text=" R"
            />
        <TextBlock 
            x:Name="txtBack" 
            Style="{StaticResource HeaderTextBlockStyle}" 
            Grid.Column="1" Grid.Row="6"
                Margin="120,10,160,80
            " 
            Text=" B"
            />
    </Grid>
</Page>

using System;
using System.Text;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Runtime.InteropServices.WindowsRuntime;
using Windows.Foundation;
using Windows.Foundation.Collections;
using Windows.UI.Popups;
using Windows.UI.Xaml;
using Windows.UI.Xaml.Controls;
using Windows.UI.Xaml.Controls.Primitives;
using Windows.UI.Xaml.Data;
using Windows.UI.Xaml.Input;
using Windows.UI.Xaml.Media;
using Windows.UI.Xaml.Navigation;
using Windows.Devices.Sensors;
using Windows.Storage.Streams;
using Windows.Networking.Sockets;
using Windows.Devices.Enumeration;
using Windows.Devices.Bluetooth.Rfcomm;

// The Blank Page item template is documented at http://go.microsoft.com/fwlink/?LinkId=391641

namespace PhoneLogic
{
    /// <summary>
    /// An empty page that can be used on its own or navigated to within a Frame.
    /// </summary>
    public sealed partial class MainPage : Page
    {
        Accelerometer accelerometer;
        private RfcommDeviceService rfcommService;
        private StreamSocket socket;
        private DataWriter writer;
        private char cmdSent = ' ';
        public MainPage()
        {
            this.InitializeComponent();

            this.NavigationCacheMode = NavigationCacheMode.Required;

            accelerometer = Accelerometer.GetDefault();
            uint desiredReportInterval;

            App.Current.Suspending += App_Suspending;

            if (accelerometer != null)
            {

                uint minReportInterval = accelerometer.MinimumReportInterval;
                desiredReportInterval = minReportInterval > 250 ? minReportInterval : 250;
                // slowing this way down to prevent flooding with commands
                accelerometer.ReportInterval = desiredReportInterval;
                //add event for accelerometer readings
                accelerometer.ReadingChanged += new TypedEventHandler<Accelerometer, AccelerometerReadingChangedEventArgs>(ReadingChanged);
            }
            else
            {
                MessageDialog ms = new MessageDialog("No accelerometer Found");
                ms.ShowAsync();
            }
        }
        private void App_Suspending(object sender, Windows.ApplicationModel.SuspendingEventArgs e)
        {
            Disconnect();
        }
        private async void EnumerateButton_Click(object sender, RoutedEventArgs e)
        {
            EnumerateButton.IsEnabled = false;

            try
            {
                var serviceInfoCollection =
                    await DeviceInformation.FindAllAsync(
                        RfcommDeviceService.GetDeviceSelector(RfcommServiceId.SerialPort));

                ServicesList.Items.Clear();

                if (serviceInfoCollection.Count > 0)
                {
                    foreach (var serviceInfo in serviceInfoCollection)
                    {
                        ServicesList.Items.Add(serviceInfo);
                    }
                    ServicesList.Visibility = Visibility.Visible;
                }
            }
            catch (Exception ex)
            {
                NotifyStatus(ex.Message);
            }

            EnumerateButton.IsEnabled = true;
        }

        private async void ServicesList_Tapped(object sender, Windows.UI.Xaml.Input.TappedRoutedEventArgs e)
        {
            ServicesList.IsEnabled = false;

            var serviceInfo = (DeviceInformation)ServicesList.SelectedItem;
            try
            {
                // Initialize the target Bluetooth RFCOMM device service
                rfcommService = await RfcommDeviceService.FromIdAsync(serviceInfo.Id);

                if (rfcommService != null)
                {
                    // Create a socket and connect to the target 
                    socket = new StreamSocket();
                    await socket.ConnectAsync(
                        rfcommService.ConnectionHostName,
                        rfcommService.ConnectionServiceName,
                        SocketProtectionLevel.BluetoothEncryptionAllowNullAuthentication);

                    writer = new DataWriter(socket.OutputStream);

                    EnumerateButton.IsEnabled = false;
                    ServicesList.Visibility = Visibility.Collapsed;
                    DisconnectButton.IsEnabled = true;
                }
            }
            catch (Exception ex)
            {
                NotifyStatus(ex.Message);
            }

            ServicesList.IsEnabled = true;
        }

        /// <summary>
        /// Disconnect from the currently connected radio.
        /// </summary>
        private void DisconnectButton_Click(object sender, RoutedEventArgs e)
        {
            Disconnect();
        }

        /// <summary>
        /// Cleanup Bluetooth resources
        /// </summary>
        private void Disconnect()
        {
            if (writer != null)
            {
                writer.DetachStream();
                writer = null;
            }

            if (socket != null)
            {
                socket.Dispose();
                socket = null;
            }

            if (rfcommService != null)
            {
                rfcommService = null;
            }

            EnumerateButton.IsEnabled = true;
            ServicesList.Visibility = Visibility.Collapsed;
            DisconnectButton.IsEnabled = false;
        }

        async void NotifyStatus(string status)
        {
            await Dispatcher.RunAsync(Windows.UI.Core.CoreDispatcherPriority.Normal, () =>
            {
                StatusText.Text = status;
            });
        }

        private async void ReadingChanged(Accelerometer sender, AccelerometerReadingChangedEventArgs args)
        {
            byte[] packet;
            await Dispatcher.RunAsync(Windows.UI.Core.CoreDispatcherPriority.Normal, () =>
            {
                if (DisconnectButton.IsEnabled == true)
                {
                    AccelerometerReading reading = args.Reading;
                    if (reading.AccelerationX > .4)  //these are arbitrary levels set to what seemed easy
                    {
                        // Send Right
                        if (cmdSent != 'R')
                        {
                            txtRight.Text = "On";
                            txtLeft.Text = " L";
                            txtForward.Text = " F";
                            txtBack.Text = " B";
                            packet = Encoding.UTF8.GetBytes("R");
                            cmdSent = 'R';
                            SendData(packet);
                        }
                    }
                    else if (reading.AccelerationX < -.4)
                    {
                        //send left
                        if (cmdSent != 'L')
                        {
                            txtRight.Text = " R";
                            txtLeft.Text = "On";
                            txtForward.Text = " F";
                            txtBack.Text = " B";
                            packet = Encoding.UTF8.GetBytes("L");
                            cmdSent = 'L';
                            SendData(packet);
                        }
                    }
                    else if (reading.AccelerationY < -.25)
                    {
                        //send back
                        if (cmdSent != 'B')
                        {
                            txtRight.Text = " R";
                            txtLeft.Text = " L";
                            txtForward.Text = " F";
                            txtBack.Text = "On";
                            packet = Encoding.UTF8.GetBytes("B");
                            cmdSent = 'B';
                            SendData(packet);
                        }
                    }
                    else if (reading.AccelerationY > .35)
                    {
                        //send Forward
                        if (cmdSent != 'F')
                        {
                            txtRight.Text = " R";
                            txtLeft.Text = " L";
                            txtForward.Text = "On";
                            txtBack.Text = " B";
                            packet = Encoding.UTF8.GetBytes("F");
                            cmdSent = 'F'; ;
                            SendData(packet);
                        }
                    }
                    else
                    {
                        if (cmdSent != 'S')
                        {
                            txtRight.Text = " R";
                            txtLeft.Text = " L";
                            txtForward.Text = " F";
                            txtBack.Text = " B";
                            packet = Encoding.UTF8.GetBytes("S");
                            cmdSent = 'S';
                            SendData(packet);
                        }
                    }

                }
            });
        }

        private async void SendData(byte[] packet)
        {
            try
            {
                writer.WriteBytes(packet);
                await writer.StoreAsync();
            }
            catch (Exception ex)
            {
                NotifyStatus(ex.Message);
            }
        }

        /// <summary>
        /// Invoked when this page is about to be displayed in a Frame.
        /// </summary>
        /// <param name="e">Event data that describes how this page was reached.
        /// This parameter is typically used to configure the page.</param>
        protected override void OnNavigatedTo(NavigationEventArgs e)
        {
            // TODO: Prepare page for display here.

            // TODO: If your application contains multiple pages, ensure that you are
            // handling the hardware Back button by registering for the
            // Windows.Phone.UI.Input.HardwareButtons.BackPressed event.
            // If you are using the NavigationHelper provided by some templates,
            // this event is handled for you.
        }

    }
}