AI 자율주행 차량 키드
알티노 (Aitino)
학습용 자료
아두이노 코드 예제 다운 받는 곳
http://m.saeon.co.kr/board/free/read.html?board_no=7&no=363
불루투스로 시리얼 통신포트 연결 (없을때 윈도우에서 포트 추가하기)
비주얼스튜디오 C#으로 작성한 PC 제어 코드
연결 : PC -- 불루투스 -- 알티노
// =========================================================================================
// 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());
}
// -------------------------------------------------------------------------------------
// -------------------------------------------------------------------------------------
// =====================================================================================
}
}
'C#_.NET' 카테고리의 다른 글
네이버 주식데이터 크롤링 : Web crawler (2) | 2023.05.03 |
---|---|
네이버 증권 데이터 엑셀로 다운 / 그래프 작성 (0) | 2022.07.02 |
Tesseract OCR 4 한글 학습하기 (0) | 2020.02.16 |
네이버 증권정보 엑셀로 저장 - 데이터 처리 보강 (0) | 2020.02.04 |
파인에듀 기자재관리 SW - 업데이트(FineEduAsset) (0) | 2020.01.09 |