using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System.IO.Ports;
public class Serial : MonoBehaviour
{
//通信パラメータ
private SerialPort serialPort_ = new SerialPort("COM1", 38400, Parity.Even, 8, StopBits.One);
//電動車椅子の速度・角速度
float linear = 0.2F; //速度[km/h](-5.6~5.6)
float angular = 0.0F; //角速度[km/h]
//電動車椅子の情報
float Perimeter = 1.18F; //タイヤ周長[m]
float Gear_Ratio = 12.64F; //ギア比[-]
float Distance = 0.24F; //電動車椅子の中心から車輪までの距離[m]
float Turning_Radius =0.0F; //電動車椅子の旋回半径[m]
//右のタイヤの回転数・速度
float rpm_right; //電動車椅子の右のタイヤの回転数[rpm]
float velocity_right; //電動車椅子の右のタイヤの速度[km/h]
float Velocity_right; //電動車椅子の右のタイヤの速度[m/s]
//左のタイヤの回転数・速度
float rpm_left; //電動車椅子の左のタイヤの回転数[rpm]
float velocity_left; //電動車椅子の左のタイヤの速度[km/h]
float Velocity_left; //電動車椅子の左のタイヤの速度[m/s]
//電動車椅子の中心の速度・角速度
float Angular = angular/3.6F; //角速度[m/s]
void Start()
{
serialPort_.Open();
//RequestCommand
byte[] RequestCommand = new byte[15]; //配列の作成
//BaseFrame
RequestCommand[0] = 0x01; //ヘッダ
RequestCommand[1] = 0x08; //ソースアドレス
RequestCommand[2] = 0x03; //ディスティネーションアドレス
RequestCommand[3] = 0x01; //通信番号
RequestCommand[4] = 0x68; //コマンド
RequestCommand[5] = 0x08; //データ長
//Data
RequestCommand[6] = 0x01; //モード(アカデミックモード=01h)
RequestCommand[7] = 0x00; //
RequestCommand[8] = 0x00; //
RequestCommand[9] = 0x00; //
RequestCommand[10] = 0x00; //タイムアウト(無効=00h)
RequestCommand[11] = 0x00; //
RequestCommand[12] = 0x00; //
RequestCommand[13] = 0x00; //
//CheckSum
int sum = 0;
for (int i = 1; i < 13; i++)
{
sum += RequestCommand[i];
}
RequestCommand[14] = (byte)sum; //CheckSum(これまでの数値の和の下位1バイト)
//Request Command 送信
int offset_Write = 0;
int count_Write = 15;
serialPort_.Write(RequestCommand, offset_Write, count_Write);
//Request Command 受信
byte[] inBuffer = new byte[15];
int offset_Read = 0;
int count_Read = 15;
int read;
while (count_Read > 0 && (read = serialPort_.Read(inBuffer, offset_Read, count_Read)) > 0) //データが送られている間はinBufferにデータを入れる
{
offset_Read += read;
count_Read -= read;
for (int i = 1; i < 15; i++)
{
print(inBuffer[i]); //受信データの表示
}
}
}
void Update()
{
try
{
Turning_Radius = linear / angular;
//right tire
velocity_right = (Turning_Radius + Distance) * Angular; //m/s
Velocity_right = velocity_right * 3.6F; //km/h
rpm_right = (1000 * Velocity_right * Gear_Ratio) / (60 * Perimeter);
//left tire
velocity_left = (Turning_Radius - Distance) * Angular; //m/s
Velocity_left = velocity_left * 3.6F; //km/h
rpm_left = (1000 * Velocity_Left * Gear_Ratio) / (60 * Perimeter);
}
catch (System.DivideByZeroException)
{
Turning_Radius = 0.0F;
rpm_right = (1000 * linear * Gear_Ratio) / (60 * Perimeter);
rpm_left = (1000 * linear * Gear_Ratio) / (60 * Perimeter);
}
//WriteReadCommand
byte[] WriteReadCommand = new byte[23];
//BaseFrame
WriteReadCommand[0] = 0x01; //ヘッダ
WriteReadCommand[1] = 0x08; //ソースアドレス
WriteReadCommand[2] = 0x03; //ディスティネーションアドレス
WriteReadCommand[3] = 0x01; //通信番号
WriteReadCommand[4] = 0x69; //コマンド
WriteReadCommand[5] = 0x10; //データ長
//Data
WriteReadCommand[6] = 0x02; //ユニット有効指示(モータ回転指示値=02h)
WriteReadCommand[7] = 0x00; //
WriteReadCommand[8] = 0x00; //
WriteReadCommand[9] = 0x00; //
WriteReadCommand[10] = 0x00; //
WriteReadCommand[11] = 0x00; //
WriteReadCommand[12] = (byte)((int)rpm_right & 0x00FF); //右モーター回転指示(下位バイト) (-1000rpm~1000rpm)
WriteReadCommand[13] = (byte)(((int)rpm_right & 0xFF00) >> 8); ////右モーター回転指示(上位バイト) (-1000rpm~1000rpm)
WriteReadCommand[14] = (byte)((int)rpm_left & 0x00FF); ////左モーター回転指示(下位バイト) (-1000rpm~1000rpm)
WriteReadCommand[15] = (byte)(((int)rpm_left & 0xFF00) >> 8); ////左モーター回転指示(上位バイト) (-1000rpm~1000rpm)
WriteReadCommand[16] = 0x00; //
WriteReadCommand[17] = 0x00; //
WriteReadCommand[18] = 0x00; //
WriteReadCommand[19] = 0x00; //
WriteReadCommand[20] = 0x00; //
WriteReadCommand[21] = 0x00; //
//CheckSum
int sum = 0;
for (int i = 1; i < 21; i++)
{
sum += WriteReadCommand[i];
}
WriteReadCommand[22] = (byte)sum; //CheckSum(これまでの数値の和の下位1バイト)
//WriteReadCommand 送信
int offset_Write = 0;
int count_Write = 23;
serialPort_.Write(WriteReadCommand, offset_Write, count_Write);
//WriteReadCommand 受信
byte[] inBuffer = new byte[23];
int offset_Read = 0;
int count_Read = 23;
int read;
while (count_Read > 0 && (read = serialPort_.Read(inBuffer, offset_Read, count_Read)) > 0)
{
offset_Read += read;
count_Read -= read;
for (int i = 1; i < 23; i++)
{
print(inBuffer[i]);
}
//change rpm to velocity
//right
int right_lower = inBuffer[8];
int right_upper = inBuffer[9];
float rpm_Right = (right_upper << 8) + right_lower;
float speed_right = rpm_right * 60 / 1000 * tire / gear;
//left
int left_lower = inBuffer[10];
int left_upper = inBuffer[11];
float rpm_left = (left_upper << 8) + left_lower;
float speed_left = rpm_left * 60 / 1000 * tire / gear;
}
}
}
More than 3 years have passed since last update.
Register as a new user and use Qiita more conveniently
- You get articles that match your needs
- You can efficiently read back useful information
- You can use dark theme