/// <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);
}
}
void NotificationTimer(ref long timerIntervalInMicroSec,
ref long ignoreEventIfLateBy,
ref bool stopTimer)
{
int timerCount = 0;
long nextNotification = 0;
MicroStopwatch microStopwatch = new MicroStopwatch();
microStopwatch.Start();
while (!stopTimer)
{
long callbackFunctionExecutionTime =
microStopwatch.ElapsedMicroseconds - nextNotification;
long timerIntervalInMicroSecCurrent =
System.Threading.Interlocked.Read(ref timerIntervalInMicroSec);
long ignoreEventIfLateByCurrent =
System.Threading.Interlocked.Read(ref ignoreEventIfLateBy);
nextNotification += timerIntervalInMicroSecCurrent;
timerCount++;
long elapsedMicroseconds = 0;
while ( (elapsedMicroseconds = microStopwatch.ElapsedMicroseconds)
< nextNotification)
{
System.Threading.Thread.SpinWait(10);
}
long timerLateBy = elapsedMicroseconds - nextNotification;
if (timerLateBy >= ignoreEventIfLateByCurrent)
{
continue;
}
MicroTimerEventArgs microTimerEventArgs =
new MicroTimerEventArgs(timerCount,
elapsedMicroseconds,
timerLateBy,
callbackFunctionExecutionTime);
MicroTimerElapsed(this, microTimerEventArgs);
}
microStopwatch.Stop();
}