C:/Documents and Settings/student/My Documents/robo_magellan/visual_studio_(system_controller)/system_controller/NavigationController/NavigationStrategy.cs

Go to the documentation of this file.

using System;
using System.Collections.Generic;
using System.Threading;
using System.Linq;
using System.Text;
using RS232Port;

namespace NavigationController {
        public class NavigationStrategy : SerialStrategy {

                public event updateStatusEventHandler updateResetEvent;

                NavigationData _data;
                public NavigationData Data {
                        get { return _data; }
                }

                public NavigationStrategy(string portName, double countsPerMeter) {
                        _data = new NavigationData(countsPerMeter);
                        serialPortName = portName;
                        initializePort();
                        _watchdogTimer.Start();
                }

                private void initializePort() {
                        _rs232 = new RS232MainForm();
                        _rs232.updateErrorEvent += updateError;
                        _rs232.receiveDataEvent += receiveData;
                        _rs232.setSilentModeEvent += updateSilentMode;
                        _rs232.Show();
                        _rs232.Hide();
                        _rs232.Port.ReceivedBytesThreshold = 1;
                        _rs232.DebugMode = RS232MainForm.debugTerminalMode.silent;
                        _rs232.Port.BaudRate = 19200;
                        _rs232.Text = "Navigation Controller Serial Port";
                        _rs232.Port.PortName = serialPortName;
                        _rs232.updatePortSettings();

                        _rs232.RecieveDelayMs = 60;

                        _status = openPort();

                        resetNavigationController();
                }

                public void resetNavigationController() {
                        if (_rs232.Port.IsOpen) {
                                _rs232.Port.DtrEnable = true;
                                Thread.Sleep(100);
                                _rs232.Port.DtrEnable = false;
                        }
                        if(updateResetEvent != null)
                                updateResetEvent.Invoke();
                }

                public void writeToNavigationController(NavigationCommand command) {
                        _rs232.transmit(command.command.ToString());
                }

                private void receiveData() {
                        parsSerialData(_rs232.SerialData);
                        updateUi();
                        updateData();
                        _watchdogTimer.Reset();
                        _watchdogTimer.Start();
                }

                //part of the strategy class is the parsing algorithm 
                //for the navigation controller's serial data.
                public void parsSerialData(string buffer) {
                        try {
                                _data.NavigationBuffer = buffer;
                                string s = "";
                                char c = '0';
                                //buffer = buffer.Remove(0, buffer.IndexOf(";") + 1);
                                buffer = buffer.Remove(buffer.LastIndexOf(";") + 1
                                        , buffer.Length - (buffer.LastIndexOf(";") + 1));
                                while (buffer.Length > 0) {
                                        s = buffer.Substring(buffer.IndexOf(":") - 1, buffer.IndexOf(";"));
                                        c = s[0];
                                        s = s.Remove(0, 2);
                                        switch (c) {
                                                case '0':
                                                        //update the status
                                                        if (s == "nav_init") {
                                                                _data.setStatus(s);
                                                                updateStatus();
                                                        }
                                                        _rs232.Port.DiscardInBuffer();
                                                        break;
                                                case 'a':
                                                        //parse the accelerometer x axis value
                                                        if (int.TryParse(s, out _outInt))
                                                                _data.XAcc = _outInt;
                                                        break;
                                                case 'b':
                                                        //parse y axis value
                                                        if (int.TryParse(s, out _outInt))
                                                                _data.YAcc = _outInt;
                                                        break;
                                                case 'c':
                                                        //parse z axis value
                                                        if (int.TryParse(s, out _outInt))
                                                                _data.ZAcc = _outInt;
                                                        break;
                                                case 'd':
                                                        if (long.TryParse(s, out _outLong)) {
                                                                //instantainous velocity, or as close as possible
                                                                _data.DeltaEncoderCounts = (short)
                                                                        (_outLong - _data.EncoderCounts);
                                                                //total encoder counts
                                                                _data.EncoderCounts = _outLong;
                                                        }
                                                        break;
                                                case 'e':
                                                        //set the steering pulse duration
                                                        if (int.TryParse(s, out _outInt))
                                                                        _data.SteeringPulseDuration = _outInt;
                                                        break;
                                                case 'f':
                                                        //set the throttle pulse duration
                                                        if (int.TryParse(s, out _outInt))
                                                                _data.ThrottlePulseDuration = _outInt;
                                                        break;


                                                case 'n':
                                                        //set the run mode 0, 1, 2
                                                        if (int.TryParse(s, out _outInt))
                                                                _data.RunMode = _outInt;
                                                        break;
                                                case 'o':
                                                        //set the dead man flag
                                                        _data.DeadMan = s == "1";
                                                        break;
                                                default:
                                                        break;
                                        }
                                        buffer = buffer.Substring(buffer.IndexOf(";") + 1
                                                , buffer.Length - (buffer.IndexOf(";") + 1));
                                }
                        } catch (Exception e) {
                                updateError(e);
                                _error = e.Message + "\r\n" + e.StackTrace;
                        }
                }

                public void setSteeringSetpoint(int steering) {
                        if (steering > 127) steering = 127;
                        else if (steering < -127) steering = -127;
                        sendSerialCommand('l', (byte)steering);
                        _data.SteeringSetpoint = steering;
                }

                public void setThrottleSetpoint(int throttle) {
                        sendSerialCommand('m', (byte)throttle);
                        _data.ThrottleSetpoint = throttle;
                }

                public void setEncoderCounts(int counts) {

                }

                public void clearEncoderCounts() {
                        sendSerialCommand('k', 0);
                }

                public void setRunMode(int mode) {
                        sendSerialCommand('n', (byte)mode);
                }

                public void setDeadMan(int flag) {
                        sendSerialCommand('o', (byte)flag);
                }
        }
}

---