/// <summary>
///
/// </summary>
/// <param name="body"></param>
public void wait_start(EquipmentParameters body)
{
//スタート待ち
var dialogSTART = new InfoDialog("Touch to START", false);
dialogSTART.Show(); // Wait for enter to be pressed
MicroStopwatch stopwatch = new MicroStopwatch();
stopwatch.Start();
int counter = 0;
Thread.Sleep(2000);
while (!body.TouchSensors.IsPressed())
{
TailMoter.tailControl(body, EquipmentParameters.TAIL_ANGLE_STAND_UP); //完全停止用角度に制御
if (btConnectionLibrary.CheckRemoteCommand(EquipmentParameters.REMOTE_COMMAND_START))
break; // PC で 'g' キーが押された
// 電圧を取得
int battery = Brick.GetVoltageMilliVolt();
int reflection = body.LightSensors.Read();
int gyroNow = body.GyroSensors.Read();
int thetaL = body.MpLeft.GetTachoCount();
int theTaR = body.MpRight.GetTachoCount();
SensorData sendData = new SensorData();
sendData.AddAPower(builder, 0);
sendData.AddBPower(builder, 0);
sendData.AddBOdometer(builder, thetaL);
sendData.AddBattery(builder, battery);
sendData.AddCPower (builder, 0);
sendData.AddCOdometer (builder, theTaR);
sendData.AddGyroSensor(builder, gyroNow);
sendData.AddReflectionSensor(builder, reflection);
sendData.AddSonerSensor(builder, 0);
sendData.AddTouchSenser(builder, 0);
sendData.AddTaskProcessTime(builder, stopwatch.ElapsedMicroseconds);
btConnectionLibrary.SensorDataContainer.Enqueue(sendData);
if (++counter >= 100 / 4)
{
btConnectionLibrary.SendQueueData2();
counter = 0;
}
Thread.Sleep(4);
}
while (body.TouchSensors.IsPressed())
{
TailMoter.tailControl(body, EquipmentParameters.TAIL_ANGLE_STAND_UP); //完全停止用角度に制御
Thread.Sleep(4);
}
}
/// <summary>
///
/// </summary>
/// <param name="args"></param>
static void Main(string[] args)
{
var initDialog = new InfoDialog("InitializingDB", false);
initDialog.Show();//Wait for enter to be pressed
// 処理速度の観点からNG
//using System.Diagnostics;
//がソースファイルの一番上に書かれているものとする
// DefaultTraceListenerオブジェクトを取得
// DefaultTraceListener drl;
// drl = (DefaultTraceListener)Trace.Listeners["Default"];
// LogFileNameを変更する
// drl.LogFileName = @"home/root/apps/ET2015/test1.txt";
// drl.LogFileName = @"test1.txt";
initDialog.UpdateMessage("InitializeingParam");
initDialog.Show();//Wait for enter to be pressed
// 構造体の宣言と初期化
// var body = new EV3body();
// EV3body.init(ref body);
var equipParameter = new EquipmentParameters();
var mainControl = new MainLogic();
// ログ取得対象のセンサを追加する
// Bluetooth関係のETロボコン拡張機能を有効にする
Brick.InstallETRoboExt();
var btConnectionLibrary = new Bluetooth();
// リモート接続
btConnectionLibrary.Connect();
mainControl.btConnectionLibrary = btConnectionLibrary;
//キャリブレーション
equipParameter.GyroSensors.Reset();
InfoDialog dialogGyro = new InfoDialog ("Calibrate Gyro", false);
dialogGyro.Show ();//Wait for enter to be pressed
equipParameter.GyroSensors.Read() + 600;
//int gyroOff = gyro.ReadAngularAcceleration ();
// 左・右走行？
// センサーおよびモータに対して初回アクセスをしておく
equipParameter.LightSensors.Read();
equipParameter.DistSensors.Read();
equipParameter.GyroSensors.Read();
equipParameter.MpLeft.SetPower(0);
equipParameter.MpRight.SetPower(0);
equipParameter.TailMoter.SetPower(0);
equipParameter.MpLeft.ResetTacho();
equipParameter.MpRight.ResetTacho();
equipParameter.TailMoter.ResetTacho();
Balancer.init();
// スタート待ち
// TODO: 処理速度
mainControl.wait_start(equipParameter);
try
{
mainControl.run(equipParameter);
}
catch (Exception)
{
var dialogE = new InfoDialog("Exception.", false);
dialogE.Show();//Wait for enter to be pressed
}
equipParameter.MpLeft.Off();
equipParameter.MpRight.Off();
equipParameter.TailMoter.Off();
// ソケットを閉じる
// if (connection != null)
// {
// connection.Close();
// }
Lcd.Instance.Clear();
Lcd.Instance.Update();
if (Debugger.IsAttached)
{
Brick.ExitToMenu(); // MonoBrickメインメニューへ戻る
}
}
/// <summary>
///
/// </summary>
/// <param name="body"></param>
/// <param name="connection"></param>
public void run(EquipmentParameters body)
{
// 電圧を取得
int battery = Brick.GetVoltageMilliVolt();
// int battery = 7500;
// _logger.drl.WriteLine("battery : " + battery +"[mV]");
sbyte forward;
sbyte turn;
int counter = 0;
bool alert = false;
while (!body.TouchSensors.IsPressed())
{
TailMoter.tailControl(body, EquipmentParameters.TAIL_ANGLE_DRIVE); // (計測用)
// if (checkRemoteCommand(connection, EquipmentParameters.REMOTE_COMMAND_STOP))
// break; // PC で 's' キー押されたら走行終了
if (++counter >= 100 / 4)
{
btConnectionLibrary.SendQueueData();
counter = 0;
}
// if (++counter >= 40 / 4)
// {
// alert = sonar_alert(body);
// counter = 0;
// }
int reflection = body.LightSensors.Read();
btConnectionLibrary.SendLogDataAdd(reflection);
if (alert)
{
forward = 0;
turn = 0;
}
else
{
forward = 10;
turn = (reflection >= (EquipmentParameters.LIGHT_BLACK + EquipmentParameters.LIGHT_WHITE) / 2) ? (sbyte)50 : (sbyte)-50;
}
// int gyroNow = -body.GyroSensors.Read();
int gyroNow = body.GyroSensors.Read() + 600;
int thetaL = body.MpLeft.GetTachoCount();
int theTaR = body.MpRight.GetTachoCount();
btConnectionLibrary.SendLogDataAdd(gyroNow);
btConnectionLibrary.SendLogDataAdd(thetaL);
btConnectionLibrary.SendLogDataAdd(theTaR);
sbyte pwmL, pwmR;
Balancer.control(
(float)forward, (float)turn, (float)gyroNow, (float)EquipmentParameters.GYRO_OFFSET, (float)thetaL, (float)theTaR, (float)battery,
out pwmL, out pwmR
);
btConnectionLibrary.SendLogDataAdd(pwmL);
btConnectionLibrary.SendLogDataAdd(pwmR);
// pwmL = 0;
// pwmR = 0;
if (pwmL == 0)
{
body.MpLeft.Brake();
}
else
{
body.MpLeft.SetPower(pwmL);
}
if (pwmR == 0)
{
body.MpRight.Brake();
}
else
{
// body.MpRight.Brake();
body.MpRight.SetPower(pwmR);
}
// バランス制御のみだと3msecで安定
// 尻尾制御と障害物検知を使用する場合2msecで安定
Thread.Sleep(2);
}
}
public void run2(EquipmentParameters body)
{
// 電圧を取得
int battery = Brick.GetVoltageMilliVolt();
// int battery = 7500;
// logger.drl.WriteLine("battery : " + battery +"[mV]");
sbyte forward = 0;
sbyte turn = 0;
int counter = 0;
bool alert = false;
TaskFactory taskFactory = new TaskFactory();
var csource = new CancellationTokenSource();
var task = taskFactory.StartNew(() =>
{
wait_start(body);
// TailMoter.tailControl(body, EquipmentParameters.TAIL_ANGLE_DRIVE); // (計測用)
// if (++counter >= 40 / 4)
// {
// alert = sonar_alert(body);
// counter = 0;
// }
// 超音波センサーからデータ取得する？
int reflection = body.LightSensors.Read();
// int gyroNow = -body.GyroSensors.Read();
int gyroNow = body.GyroSensors.Read() + 600;
// _logger.drl.WriteLine("gyroNow : " + (gyroNow));
int thetaL = body.MpLeft.GetTachoCount();
// _logger.drl.WriteLine("thetaL : " + thetaL);
int theTaR = body.MpRight.GetTachoCount();
// _logger.drl.WriteLine("theTaR : " + theTaR);
sbyte pwmL, pwmR;
Balancer.control(
(float) forward, (float) turn, (float) gyroNow, (float) EquipmentParameters.GYRO_OFFSET,
(float) thetaL, (float) theTaR, (float) battery,
out pwmL, out pwmR
);
// _logger.drl.WriteLine("pwmL : " + pwmL);
// _logger.drl.WriteLine("pwmR : " + pwmR);
// pwmL = 0;
// pwmR = 0;
if (pwmL == 0)
{
body.MpLeft.Brake();
}
else
{
body.MpLeft.SetPower(pwmL);
}
if (pwmR == 0)
{
body.MpRight.Brake();
}
else
{
// body.MpRight.Brake();
body.MpRight.SetPower(pwmR);
}
// バランス制御のみだと3msecで安定
// 尻尾制御と障害物検知を使用する場合2msecで安定
Thread.Sleep(2);
})
.ContinueWith((t) =>
{
if (t.IsFaulted && t.Exception.Flatten().InnerExceptions.Any((e) => e.Message == "何らかのエラー2"))
{
Console.WriteLine("Task1でタスクの取り消しを行います。");
csource.Cancel();
}
Console.WriteLine("Task1 Cancel:{0}", t.IsCanceled);
Console.WriteLine("Task1 Faulted:{0}", t.IsFaulted);
}, csource.Token);
try
{
while (!body.TouchSensors.IsPressed())
{
if (null != btConnectionLibrary && btConnectionLibrary.CheckRemoteCommand(EquipmentParameters.REMOTE_COMMAND_STOP))
{
csource.Cancel(); // PC で 's' キー押されたら走行終了
}
Thread.Sleep(100);
}
}
catch (AggregateException exc)
{
foreach (var innnerExc in exc.InnerExceptions)
{
Console.WriteLine("エラー:{0} {1}", innnerExc.Message, innnerExc.GetType().FullName);
}
}
}
