AI 자율 주행차량 : Altino (C#)

AI 자율주행 차량 키드

알티노 (Aitino)

 

학습용 자료 

 

아두이노 코드 예제 다운 받는  곳

http://m.saeon.co.kr/board/free/read.html?board_no=7&no=363

 

불루투스로 시리얼 통신포트 연결 (없을때 윈도우에서 포트 추가하기)

 

https://youtu.be/P2hby3uZgJw

 

 

비주얼스튜디오 C#으로 작성한 PC 제어 코드

연결 : PC -- 불루투스 -- 알티노 

https://youtu.be/GgS98chOZB0

AltinoSerialBT.7z

0.06MB

// =========================================================================================
// Altino Robot Car Test module 
// =========================================================================================
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using System.IO.Ports;
using System.IO;
using static AltinoSerial.ALTINO;
// =========================================================================================
// 2023.07.13 : Initial version
// 2023.07.14 : Read & parse Sensor Value
// =========================================================================================
namespace AltinoSerial
{
    public partial class Form1 : Form
    {
        private SerialPort AltinoSerial;
        private int SerialPortNo = 0;
        byte[] SendBufMain = Enumerable.Repeat<byte>(0, 28).ToArray<byte>();
        byte[] SenData56 = Enumerable.Repeat<byte>(0, 56).ToArray<byte>();
        byte[] SenData31A = Enumerable.Repeat<byte>(0, 31).ToArray<byte>();
        byte[] SenData31B = Enumerable.Repeat<byte>(0, 31).ToArray<byte>();
        private ALTINO.AltinoSensor01 atinoSen01 = new ALTINO.AltinoSensor01();
        private ALTINO.AltinoSensor02 atinoSen02 = new ALTINO.AltinoSensor02();
        public static string LogFileName;
        private int SensorInterval = 0;     // Sensor Reading interval (ms)
        private bool fReadSensor = false;
        // -------------------------------------------------------------------------------------
        // main 에서 만든 UI or Control을 다른 Thread가 접근하면 CrossThread Error 발생
        // -------------------------------------------------------------------------------------
        delegate void MyDelegate();        // 델리게이트 선언(크로스 쓰레드 해결)
        // -------------------------------------------------------------------------------------
        public Form1()
        {
            InitializeComponent();
            timerAltino.Tick += timerPortScan_Tick;
            timerAltino.Interval = 10;
            timerAltino.Start();
            LogFileName = "Log" + DateTime.Now.ToString("-yyyyMMdd-HHmmss") + ".txt";
        }
        // -------------------------------------------------------------------------------------
        private void Form1_Load(object sender, EventArgs e)
        {
            AltinoSerial = new SerialPort();
            cBoxSerial.DataSource = SerialPort.GetPortNames();
            string[] serial_list = SerialPort.GetPortNames();
            SerialPortNo = 0;
            foreach (string name in serial_list)
            {
                SerialPortNo++;
            }
        }
        // -------------------------------------------------------------------------------------
        // Relase Data Handler
        // -------------------------------------------------------------------------------------
        private void Form1_FormClosing(object sender, FormClosingEventArgs e)
        {
            if (AltinoSerial.IsOpen)
            {
                AltinoSerial.DataReceived -= new SerialDataReceivedEventHandler(AltinoDataReceived);
                AltinoSerial.Close();
            }
        }
        // -------------------------------------------------------------------------------------
        private void btnPortReload_Click(object sender, EventArgs e)
        {
            cBoxSerial.DataSource = null;
            cBoxSerial.Items.Clear();
            cBoxSerial.DataSource = SerialPort.GetPortNames();
            string[] serial_list = SerialPort.GetPortNames();
            SerialPortNo = 0;
            foreach (string name in serial_list)
            {
                SerialPortNo++;
            }
        }
        // -------------------------------------------------------------------------------------
        private void comboBox1_SelectedIndexChanged(object sender, EventArgs e)
        {

        }
         // -------------------------------------------------------------------------------------
        // Open COM port - prepare to use DataReceived Handler
        // -------------------------------------------------------------------------------------
        private void btnPortOpen_Click(object sender, EventArgs e)
        {
            if (AltinoSerial.IsOpen) // 특정 포트가 이미 열려 있다면 설정이 되지 않기 때문에 우선 닫는다.
            {
                AltinoSerial.Close();
            }
            if (SerialPortNo <= 0)
            {
                MessageBox.Show("열린 COM 포트가 없습니다.");
                return;
            }
            if (!AltinoSerial.IsOpen)  //시리얼포트가 닫혀있을 때만
            {
                AltinoSerial.PortName = cBoxSerial.Text;    // 선택된 combobox 의 이름으로 포트명
                AltinoSerial.BaudRate = 115200;             // 아두이노에서 사용할 전송률
                AltinoSerial.DataBits = 8;
                AltinoSerial.StopBits = StopBits.One;
                AltinoSerial.Parity = Parity.None;
                AltinoSerial.DataReceived += new SerialDataReceivedEventHandler(AltinoDataReceived);
                AltinoSerial.ReceivedBytesThreshold = 31;   // Min 31 & Max 56
                try
                {
                    AltinoSerial.Open();  //시리얼포트 열기
                }
                catch (Exception ex)
                {
                    MessageBox.Show("COM 포트 Open 오류");
                }
            }
            else
            {
                MessageBox.Show("해당포트가 이미 열려 있습니다.");
            }
        }
        // -------------------------------------------------------------------------------------
        // Altino Control
        // -------------------------------------------------------------------------------------
        private void ControlSpeed()
        {
            byte[] Sendbuf = Enumerable.Repeat<byte>(0, 28).ToArray<byte>();
            int left = Convert.ToInt16(tBoxGoLeft.Text);
            int right = Convert.ToInt16(tBoxGoRight.Text);

            int temp = 32768;
            int left_int, right_int;

            if (left < 0) left_int = temp - left;
            else left_int = left;
            if (right < 0) right_int = temp - right;
            else right_int = right;

            Sendbuf[6] = 0;
            Sendbuf[9] = 0;
            Sendbuf[7] = (byte)(right_int / 256);
            Sendbuf[8] = (byte)(right_int % 256);
            Sendbuf[10] = (byte)(left_int / 256);
            Sendbuf[11] = (byte)(left_int % 256);
            SendData(Sendbuf, Convert.ToByte(tbCommand.Text), 0);
            SendBufMain = Sendbuf;          // Save & Keep current Speed
        }
        // -------------------------------------------------------------------------------------
        // Enumerable.Repeat<byte>(0, 28).ToArray<byte>() : 28개 byte를 '0'로 채움
        private void btnGo_Click(object sender, EventArgs e)
        {
            ControlSpeed();
        }
        // -------------------------------------------------------------------------------------
        private void btnSpeedInc_Click(object sender, EventArgs e)
        {
            int left = Convert.ToInt16(tBoxGoLeft.Text);
            int right = Convert.ToInt16(tBoxGoRight.Text);
            int SpeedStep = Convert.ToInt16(tBoxSpeedStep.Text);
            tBoxGoLeft.Text = Convert.ToString(left + SpeedStep);
            tBoxGoRight.Text = Convert.ToString(right + SpeedStep);
            ControlSpeed();
        }
        // -------------------------------------------------------------------------------------
        private void btnSpeedDec_Click(object sender, EventArgs e)
        {
            int left = Convert.ToInt16(tBoxGoLeft.Text);
            int right = Convert.ToInt16(tBoxGoRight.Text);
            int SpeedStep = Convert.ToInt16(tBoxSpeedStep.Text);
            tBoxGoLeft.Text = Convert.ToString(left - SpeedStep);
            tBoxGoRight.Text = Convert.ToString(right - SpeedStep);
            ControlSpeed();
        }
        // -------------------------------------------------------------------------------------
        private void btnStop_Click(object sender, EventArgs e)
        {
            byte[] Sendbuf = Enumerable.Repeat<byte>(0, 28).ToArray<byte>();
            SendBufMain = Sendbuf;          // Save & Keep current Speed
            // Sendbuf[6-11] are already all '0'
            SendData(Sendbuf, Convert.ToByte(tbCommand.Text), 0);
        }
        // -------------------------------------------------------------------------------------
        // Enumerable.Repeat<byte>(0, 28).ToArray<byte>() : 28개 byte를 '0'로 채움
        private void btnSteer_Click(object sender, EventArgs e)
        {
            byte[] Sendbuf = Enumerable.Repeat<byte>(0, 28).ToArray<byte>();
            Sendbuf = SendBufMain;              // Keep current Speed
            int Steer = Convert.ToByte(tBoxStreer.Text);
            if (Steer > 127) Steer = 127;
            if (Steer < -127) Steer = -127;

            if (Steer < 0) Steer = 128 - Steer;

            Sendbuf[5] = Convert.ToByte(Steer);
            Sendbuf[24] = 0;        // Steering Mode =0 상대위치 제어
            Sendbuf[25] = (byte)0;  // Mode = 1 --> (Left)128 0 127(Right): 
            SendData(Sendbuf, Convert.ToByte(tbCommand.Text), 0);
        }
        // -------------------------------------------------------------------------------------
        private void btnSteerLeft_Click(object sender, EventArgs e)
        {
            byte SteerMode = Convert.ToByte(tBoxSteerMode.Text);
            SendSteer(SteerMode, 1);
        }
        // -------------------------------------------------------------------------------------
        private void btnSteerCenter_Click(object sender, EventArgs e)
        {
            byte[] Sendbuf = Enumerable.Repeat<byte>(0, 28).ToArray<byte>();
            Sendbuf = SendBufMain;              // Keep current Speed
            Sendbuf[5] = 2;                     // Steer Center
            Sendbuf[24] = 0;                    // Steering Mode =1 절대위치 제어
            Sendbuf[25] = (byte)0;
            SendData(Sendbuf, Convert.ToByte(tbCommand.Text), 0);
            tBoxStreer.Text = Convert.ToString(2);
        }
        // -------------------------------------------------------------------------------------
        private void btnSteerRight_Click(object sender, EventArgs e)
        {
            byte SteerMode = Convert.ToByte(tBoxSteerMode.Text);
            SendSteer(SteerMode, 3);
        }
        // -------------------------------------------------------------------------------------
        // Sendbuf[24]
        // Steering Mode = 0 (3 stage) : Left -> 1 Center -> 2 Right -> 3
        // Steering Mode = 1 (Abs)     : Left -> 128 + (0~127) Right -> 0~127
        // Steering Mode = 2 (Relative): angle incease by time
        // -------------------------------------------------------------------------------------
        private void SendSteer(byte SteerMode, byte Direction)
        {
            byte[] Sendbuf = Enumerable.Repeat<byte>(0, 28).ToArray<byte>();
            Sendbuf = SendBufMain;                  // Keep current Speed
            int Steer = 0;
            if (SteerMode != 0)
            {
                int SteerStep = Convert.ToByte(tBoxSteerStep.Text);
                Steer = Convert.ToByte(tBoxStreer.Text) + SteerStep;
                if (Steer > 127) Steer = 127;
                if (Steer < -127) Steer = -127;
                if (Steer < 0) Steer = 128 - Steer;
            }
            if (SteerMode == 0) Sendbuf[5] = Direction; // Steer Left
            else Sendbuf[5] = (byte)Steer;
            tBoxStreer.Text = Convert.ToString(Direction);
            Sendbuf[24] = SteerMode;
            Sendbuf[25] = (byte)0;
            SendData(Sendbuf, Convert.ToByte(tbCommand.Text), 0);
        }
        // -------------------------------------------------------------------------------------
        private void btnText_Click(object sender, EventArgs e)
        {
            byte[] Sendbuf = Enumerable.Repeat<byte>(0, 28).ToArray<byte>();
            byte[] ASC2 = Encoding.ASCII.GetBytes(tBoxDisplay.Text);

            int ASCII = ASC2[0];
            if (ASCII > 128) ASCII = ASCII - 128;
            else if (ASCII > 0 && ASCII < 128) ASCII = 128 + ASCII;
            Sendbuf = SendBufMain;                  // Keep current Speed
            Sendbuf[12] = Convert.ToByte(ASCII);
            // Sendbuf[13-20] are already all '0'
            SendData(Sendbuf, Convert.ToByte(tbCommand.Text), 0);
        }
        // -------------------------------------------------------------------------------------
        private void btnLED_Click(object sender, EventArgs e)
        {
            byte[] Sendbuf = Enumerable.Repeat<byte>(0, 28).ToArray<byte>();
            Sendbuf = SendBufMain;                  // Keep current Speed
            Sendbuf[12] = 0;
            Sendbuf[13] = (byte)Convert.ToInt32(tBoxLED0.Text, 16);
            Sendbuf[14] = (byte)Convert.ToInt32(tBoxLED1.Text, 16);
            Sendbuf[15] = (byte)Convert.ToInt32(tBoxLED2.Text, 16);
            Sendbuf[16] = (byte)Convert.ToInt32(tBoxLED3.Text, 16);
            Sendbuf[17] = (byte)Convert.ToInt32(tBoxLED4.Text, 16);
            Sendbuf[18] = (byte)Convert.ToInt32(tBoxLED5.Text, 16);
            Sendbuf[19] = (byte)Convert.ToInt32(tBoxLED6.Text, 16);
            Sendbuf[20] = (byte)Convert.ToInt32(tBoxLED7.Text, 16);
            SendData(Sendbuf, Convert.ToByte(tbCommand.Text), 0);
        }
        // -------------------------------------------------------------------------------------
        private void btnLEDOff_Click(object sender, EventArgs e)
        {
            byte[] Sendbuf = Enumerable.Repeat<byte>(0, 28).ToArray<byte>();
            Sendbuf = SendBufMain;                  // Keep current Speed
            // Sendbuf[13-20] are already all '0'
            for (int i = 12; i <= 20; i++) Sendbuf[i] = 0;
            SendData(Sendbuf, Convert.ToByte(tbCommand.Text), 0);
        }
        // -------------------------------------------------------------------------------------
        // Altino Firmware 4.0 looks having BUG : Speed increase if sound on
        // -------------------------------------------------------------------------------------
        private void btnSound_Click(object sender, EventArgs e)
        {
            byte[] Sendbuf = Enumerable.Repeat<byte>(0, 28).ToArray<byte>();
            Sendbuf = SendBufMain;                  // Keep current Speed
            Sendbuf[22] = Convert.ToByte(tBoxSound.Text);
            SendData(Sendbuf, Convert.ToByte(tbCommand.Text), 0);
        }
        // -------------------------------------------------------------------------------------
        // Enumerable.Repeat<byte>(0, 28).ToArray<byte>() : 28개 byte를 '0'로 채움
        // Send Command ---> 10, 1, 2
        // option : 0 --> clear command buffer, 1 = keep old command
        // -------------------------------------------------------------------------------------
        public void SendData(byte[] Sendbuf1, byte SendCommand, byte option)
        {
            byte[] tx_d = Enumerable.Repeat<byte>(0, 28).ToArray<byte>();
            tx_d[0] = STX;
            tx_d[1] = 28;

            tx_d[3] = 1;
            tx_d[4] = SendCommand;
            for (int i = 5; i < 25; i++)
            {
                tx_d[i] = Sendbuf1[i];
            }
            //tx_d[21] = (byte)(Sendbuf1[21] | 0x01);
            tx_d[21] = (byte)(Sendbuf1[21] | 0x01);     // Turn ON IR sensor
            tx_d[26] = 255;
            tx_d[27] = ETX;

            int u16_tx_check_sum = 0;
            int u16_tx_cnt;
            int u16_cnt = 27;
            u16_tx_check_sum = u16_tx_check_sum + tx_d[1];
            for (u16_tx_cnt = 3; u16_tx_cnt <= u16_cnt; u16_tx_cnt++)
            {
                u16_tx_check_sum = u16_tx_check_sum + tx_d[u16_tx_cnt];
            }
            u16_tx_check_sum = u16_tx_check_sum % 256;
            tx_d[2] = (byte)(u16_tx_check_sum);
            AltinoSerial.Write(tx_d, 0, tx_d.Length);

            rBuffString = BitConverter.ToString(tx_d).Replace("-", " ");
            using (StreamWriter file = new StreamWriter(LogFileName, true))
            {
                file.WriteLine("Send: " + rBuffString);
            }
            btnTextSend.Text = "Command: " + rBuffString;
        }
        // -------------------------------------------------------------------------------------
        //private ALTINO.AltinoSensor10 aParseSensor56(byte[] senData56)
        public const byte STX = 0x02;                       // Altino STX
        public const byte ETX = 0x03;                       // Altino ETX
        private static string rTextParse = "";              // all text to be proceed
        private static string rBuffString = "";             // byte2string 시리얼포트 데이터 수신
        private static byte[] rByteAll = new byte[1024];
        private static byte[] rBuffByte31 = new byte[31];
        private static byte[] rBuffByte56 = new byte[56];
        // -------------------------------------------------------------------------------------
        // // Method 1 : Timer based - NOT used this version, see Method 2
        // -------------------------------------------------------------------------------------
        private void timerPortScan_Tick(object sender, EventArgs e)
        {
            if (!AltinoSerial.IsOpen) return;
            SensorInterval += timerAltino.Interval;
            if (SensorInterval >= Convert.ToInt16(tBoxInterval.Text))
            {
                SensorInterval = 0;
                aSendSenorCommand(10);
            }
            //ReadSerialPort();
        }
        // -------------------------------------------------------------------------------------
        // delay from :: www.bluene.net/blog/550 - Thread.Sleep 사용시 COM 포트 값 읽기 문제있음
        // -------------------------------------------------------------------------------------
        private static DateTime Delay(int MS)
        {
            DateTime ThisMoment = DateTime.Now;
            TimeSpan duration = new TimeSpan(0, 0, 0, 0, MS);
            DateTime AfterWards = ThisMoment.Add(duration);

            while (AfterWards >= ThisMoment)
            {
                System.Windows.Forms.Application.DoEvents();
                ThisMoment = DateTime.Now;
            }
            return DateTime.Now;
        }
        // -------------------------------------------------------------------------------------
        // AVR에서 라인별로 송신해도 C#에서 읽은 시리얼 포트 데이터는 붙어있다.
        // 즉 receiveBuffer 안에는 "\r"로 구분된 여러개의 데이터 포함됨
        // https://wowon.tistory.com/232 : C# 시리얼통신 데이터 잘려서 읽히는 경우
        // -------------------------------------------------------------------------------------
        private void ReadSerialPort()
        {
            if (AltinoSerial.BytesToRead < 31) return;
            if (AltinoSerial.BaseStream.CanRead == false) return;
            int textLen = AltinoSerial.BytesToRead;
            AltinoSerial.BaseStream.Read(rByteAll, 0, textLen);
            for (int i = 0; i < 56; i++) rBuffByte56[i] = rByteAll[i];
            rBuffString = BitConverter.ToString(rBuffByte56).Replace("-", " ");

            string mBlock;
            int endOfLineIndex;
            do
            {   // if No return '-1' from IndexOf
                endOfLineIndex = rTextParse.IndexOf("\n");
                if (endOfLineIndex == 0)
                {
                    rTextParse = rTextParse.Remove(0, 1);     // remove the "\r" at the head
                }
                else if (endOfLineIndex > 0)
                {
                    mBlock = rTextParse.Substring(0, endOfLineIndex);
                    rTextParse = rTextParse.Remove(0, endOfLineIndex);
                }
            } while (endOfLineIndex >= 0);
        }
        // -------------------------------------------------------------------------------------
        // Method 2 : This version is using DataReceived
        // -------------------------------------------------------------------------------------
        private void AltinoDataReceived(object sender, SerialDataReceivedEventArgs e)
        {
            if (AltinoSerial.BytesToRead < 56) return;
            if (AltinoSerial.BaseStream.CanRead == false) return;

            int textLen = AltinoSerial.BytesToRead;
            AltinoSerial.BaseStream.Read(rByteAll, 0, textLen);
            for (int i = 0; i < 56; i++)    rBuffByte56[i] = rByteAll[i];
            rBuffString = BitConverter.ToString(rBuffByte56).Replace("-", " ");
            using (StreamWriter file = new StreamWriter(LogFileName, true))
            {
                file.WriteLine("Recv: " + rBuffString);
            }
            if (this.btnTextRecv.InvokeRequired == true)
            {
                this.btnTextRecv.Invoke((MethodInvoker)delegate
                {
                    this.btnTextRecv.Text = "Altino: " + rBuffString.ToString();
                });
            }
            else 
            {
                btnTextRecv.Text = rBuffString;
            }            
            if (rBuffByte56[0] != STX || rBuffByte56[55] != ETX || rBuffByte56[1] != 56) return;
            if (ALTINO.CheckSum56(rBuffByte56) != rBuffByte56[2]) return; // Check sum error
            ALTINO.AltinoSensor10 _atinoSen10 = new ALTINO.AltinoSensor10();
            _atinoSen10 = ALTINO.aParseSensor56(rBuffByte56);
            UpdateAltinoSensor(_atinoSen10);
        }
        // -------------------------------------------------------------------------------------
        private void UpdateAltinoSensor(ALTINO.AltinoSensor10 _atinoSen10)
        {
            MyDelegate delgateUI = delegate ()                  // Delegation function begine
            {
                btnTextIR1.Text = _atinoSen10.IR[0].ToString();
                btnTextIR2.Text = _atinoSen10.IR[1].ToString();
                btnTextIR3.Text = _atinoSen10.IR[2].ToString();
                btnTextIR4.Text = _atinoSen10.IR[3].ToString();
                btnTextIR5.Text = _atinoSen10.IR[4].ToString();
                btnTextIR6.Text = _atinoSen10.IR[5].ToString();
                btnTextGyroX.Text = _atinoSen10.Gyro[0].ToString();
                btnTextGyroY.Text = _atinoSen10.Gyro[1].ToString();
                btnTextGyroZ.Text = _atinoSen10.Gyro[2].ToString();
                btnTextAccGX.Text = _atinoSen10.AccG[0].ToString();
                btnTextAccGY.Text = _atinoSen10.AccG[1].ToString();
                btnTextAccGZ.Text = _atinoSen10.AccG[2].ToString();
                btnTextMagX.Text = _atinoSen10.Mag[0].ToString();
                btnTextMagY.Text = _atinoSen10.Mag[1].ToString();
                btnTextMagZ.Text = _atinoSen10.Mag[2].ToString();
                btnTextCDS.Text = _atinoSen10.CDS.ToString();
                btnTextBatt.Text = _atinoSen10.Batt.ToString();
                btnTextSteerTQ.Text = _atinoSen10.SteerMoTQ.ToString();
                btnTextSteerVal.Text = _atinoSen10.SteerVal.ToString();
                btnTextMotoTQR.Text = _atinoSen10.MoTQR.ToString();
                btnTextMotoTQL.Text = _atinoSen10.MoTQL.ToString();
                btnTextTemp.Text = _atinoSen10.Temp.ToString();
                btnTextMTemp.Text = _atinoSen10.MTemp.ToString();   
            };                                                  // Delegation function end
            this.Invoke(delgateUI);
        }
        // -------------------------------------------------------------------------------------
        // Send read Sensor command, then Atino responds sensor data
        // However Altino responds sensor data to any command
        // -------------------------------------------------------------------------------------
        private void aSendSenorCommand(int command)
        {
            if (fReadSensor != true) return;
            byte[] Sendbuf = Enumerable.Repeat<byte>(0, 28).ToArray<byte>();
            Sendbuf = SendBufMain;                  // Keep current Speed
            SendData(Sendbuf, 10, 0);
        }
        private void btnReadSensor_Click(object sender, EventArgs e)
        {
            fReadSensor = !fReadSensor;
            if (fReadSensor) btnReadSensor.Text = "Stop Reading";
            else btnReadSensor.Text = "Read Sensor";
            aSendSenorCommand(10);
        }

        private void btnPortClose_Click(object sender, EventArgs e)
        {
            if (AltinoSerial.IsOpen)
            {
                AltinoSerial.DataReceived -= new SerialDataReceivedEventHandler(AltinoDataReceived);
                AltinoSerial.Close();
            }
        }
        // -------------------------------------------------------------------------------------
    }
    // =========================================================================================
}

// =============================================================================================
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using System.IO.Ports;
using System.Runtime.InteropServices;
using System.Reflection;
// =============================================================================================
namespace AltinoSerial
{
    class ALTINO                                // Use "ALTINO." to access thind in this class
    {
        public class AltinoSensor01             // Altino --> PC command 1 [31 Bytes]
        {
            public long[] IR = new long[6];     // IR 6ea [7-18]
            public long MoTQR;                  // Motor TQ - Right [19-20]
            public long MoTQL;                  // Motor TQ - Left [21-22]
            public long Temp;                   // Temp [23-24]
            public long CDS;                    // CDS [25-26]
            public long Remote;                 // Remote control [27]
        };
        public class AltinoSensor02             // Altino --> PC command 2 [31 Bytes]
        {
            public long[] AccG = new long[3];   // Acceleration Sensor 3ea X,Y,Z [7-12]
            public long[] Mag = new long[3];    // Magnetic Field Sensor 3ea X,Y,Z [13-18]
            public long SteerVar;               // Steer Register Value [19-20]
            public long SteerMoTQ;              // Steer Motor TQ [21-22]
            public long Batt;                   // Battery [23-24]
            public long MTemp;                  // Temp of Magnetic sensor [25]
        };
        public class AltinoSensor10             // Altino --> PC command 10 [56 Bytes]
        {
            public long[] IR = new long[6];     // IR 6ea [7-18]
            public long[] AccG = new long[3];   // Acceleration Sensor 3ea  X,Y,Z [25-30]
            public long[] Mag = new long[3];    // Magnetic Field Sensor 3ea X,Y,Z [31-36]
            public long[] Gyro = new long[3];   // Gyro 3 axis [37-42]
            public long MoTQR;                  // Motor TQ - Right [19-20]
            public long MoTQL;                  // Motor TQ - Left [21-22]
            public long SteerMoTQ;              // Steer Motor TQ [23-24]
            public long CDS;                    // CDS [43-44]
            public long SteerVal;               // Steer Register Value [45-46]
            public long Batt;                   // Battery [47-48]
            public long Temp;                   // Temp [49-50]
            public long Remote;                 // Remote control [51]
            public long MTemp;                  // Temp of Magnetic sensor [52]
        };
        // -------------------------------------------------------------------------------------
        public static AltinoSensor10 aParseSensor56(byte[] senData56)
        {
            AltinoSensor10 atinoSen10 = new AltinoSensor10();

            atinoSen10.IR[0] = senData56[7] * 256 + senData56[8];
            atinoSen10.IR[1] = senData56[9] * 256 + senData56[10];
            atinoSen10.IR[2] = senData56[11] * 256 + senData56[12];
            atinoSen10.IR[3] = senData56[13] * 256 + senData56[14];
            atinoSen10.IR[4] = senData56[15] * 256 + senData56[16];
            atinoSen10.IR[5] = senData56[17] * 256 + senData56[18];

            atinoSen10.MoTQR = senData56[19] * 256 + senData56[20];
            atinoSen10.MoTQL = senData56[21] * 256 + senData56[22];
            atinoSen10.Temp = senData56[49] * 256 + senData56[50];

            atinoSen10.CDS = senData56[43] * 256 + senData56[44];

            atinoSen10.AccG[0] = (long)senData56[25] * 256 + (long)senData56[26];
            atinoSen10.AccG[1] = (long)senData56[27] * 256 + (long)senData56[28];
            atinoSen10.AccG[2] = (long)senData56[29] * 256 + (long)senData56[30];

            long temp = 0;

            temp = atinoSen10.AccG[0] / 16;
            atinoSen10.AccG[0] = temp;
            if (temp > 2048) atinoSen10.AccG[0] = atinoSen10.AccG[0] - 4096;

            temp = atinoSen10.AccG[1] / 16;
            atinoSen10.AccG[1] = temp;
            if (temp > 2048) atinoSen10.AccG[1] = atinoSen10.AccG[1] - 4096;

            temp = atinoSen10.AccG[2] / 16;
            atinoSen10.AccG[2] = temp;
            if (temp > 2048) atinoSen10.AccG[2] = atinoSen10.AccG[2] - 4096;

            atinoSen10.Mag[0] = (long)senData56[31] * 256 + (long)senData56[32];
            atinoSen10.Mag[1] = (long)senData56[33] * 256 + (long)senData56[34];
            atinoSen10.Mag[2] = (long)senData56[35] * 256 + (long)senData56[36];

            atinoSen10.SteerVal = senData56[45] * 256 + senData56[46];
            atinoSen10.SteerMoTQ = senData56[23] * 256 + senData56[24];
            atinoSen10.Batt = senData56[47] * 256 + senData56[48];
            atinoSen10.Remote = senData56[51] * 256 + senData56[52];

            atinoSen10.Gyro[0] = (long)senData56[37] * 256 + (long)senData56[38];
            atinoSen10.Gyro[1] = (long)senData56[39] * 256 + (long)senData56[40];
            atinoSen10.Gyro[2] = (long)senData56[41] * 256 + (long)senData56[42];

            if (atinoSen10.AccG[0] > 32768) atinoSen10.AccG[0] = atinoSen10.AccG[0] - 65535;
            if (atinoSen10.AccG[1] > 32768) atinoSen10.AccG[1] = atinoSen10.AccG[1] - 65535;
            if (atinoSen10.AccG[2] > 32768) atinoSen10.AccG[2] = atinoSen10.AccG[2] - 65535;
            if (atinoSen10.Mag[0] > 32768) atinoSen10.Mag[0] = atinoSen10.Mag[0] - 65535;
            if (atinoSen10.Mag[1] > 32768) atinoSen10.Mag[1] = atinoSen10.Mag[1] - 65535;
            if (atinoSen10.Mag[2] > 32768) atinoSen10.Mag[2] = atinoSen10.Mag[2] - 65535;
            if (atinoSen10.Gyro[0] > 32768) atinoSen10.Gyro[0] = atinoSen10.Gyro[0] - 65535;
            if (atinoSen10.Gyro[1] > 32768) atinoSen10.Gyro[1] = atinoSen10.Gyro[1] - 65535;
            if (atinoSen10.Gyro[2] > 32768) atinoSen10.Gyro[2] = atinoSen10.Gyro[2] - 65535;

            return atinoSen10;
        }
        // -------------------------------------------------------------------------------------
        // 다른 곳에서 호출해서 사용하려면 "public static" 로 선언해야함
        // -------------------------------------------------------------------------------------
        public static byte CheckSum56(byte[] rx_d56)
        {
            byte rxCheckSum = 0;

            rxCheckSum = rx_d56[0];
            rxCheckSum += rx_d56[1];
            for (int i = 3; i < 56; i++) rxCheckSum += rx_d56[i];
            rxCheckSum = (byte)(rxCheckSum % 256);
            return rxCheckSum;
        }
        // -------------------------------------------------------------------------------------
        public static byte CheckSum31(byte[] rx_d31)
        {
            byte rxCheckSum = 0;

            rxCheckSum = rx_d31[0];
            rxCheckSum += rx_d31[1];
            for (int i = 3; i < 31; i++) rxCheckSum += rx_d31[i];
            rxCheckSum = (byte)(rxCheckSum % 256);
            return rxCheckSum;
        }
        // -------------------------------------------------------------------------------------
        public static int[] tIndex = { 0, 50, 100, 150, 200, 250, 300, 350, 400, 450, 500, 550, 600, 650, 700, 750, 800, 850, 900, 950, 1024 };
        public static int[] phyTemp = { 104, 95, 86, 77, 68, 60, 51, 44, 36, 30, 25, 21, 17, 11, 2, -6, -14, -21, -28, -35, -40 };
        public static int[] IRindex = { 40, 70, 120, 250, 950};
        public static int[] phyIR = { 150, 70, 50, 30, 10 };        // distance = mm
        // -------------------------------------------------------------------------------------
        public static long Convert2Physical(int[] tNdx, int[] PhyVal, long Val2Find)
        {
            int i = 0;
            if (Val2Find > tNdx[tNdx.Length - 1]) return PhyVal[tNdx.Length - 1];
            if (Val2Find < tNdx[0]) return PhyVal[0];
            while (i < tNdx.Length && Val2Find > tNdx[i]) i++;
            double x0 = tNdx[i - 1];
            double x1 = tNdx[i];
            double y0 = PhyVal[i - 1];
            double y1 = PhyVal[i];
            double RealValue = y0 + (Val2Find - x0) / (x1 - x0) * (y1 - y0);
            return (long) RealValue;
        }
        // -------------------------------------------------------------------------------------
        // leftMost = 130(45도), rightMost = 930(45도), CenterValue = 530
        // -------------------------------------------------------------------------------------
        public static String SteerAngle(long SteerVal)
        {
            long steerAngle = 0;
            long CenterValue = 530;
            if (SteerVal == CenterValue) return ("C00");
            if (SteerVal > 930) SteerVal = 930;
            if (SteerVal < 130) SteerVal = 130;
            steerAngle = (CenterValue - SteerVal) * 45 / 400;
            if(steerAngle > 0) return ("L"+ steerAngle.ToString());
            else return ("R" + (steerAngle*-1).ToString());
        }
        // -------------------------------------------------------------------------------------
        // -------------------------------------------------------------------------------------
        // =====================================================================================
    }
}