const int REMOTE_COMMAND_STOP = 's'; // 's'
public static void Main()
{
// 構造体の宣言と初期化
var body = new EV3body();
EV3body.init(ref body);
// Bluetooth関係のETロボコン拡張機能を有効にする
Brick.InstallETRoboExt();
// リモート接続
NetworkStream connection = connect();
// センサーおよびモータに対して初回アクセスをしておく
body.color.Read();
body.sonar.Read();
body.gyro.Read();
body.motorL.SetPower(0);
body.motorR.SetPower(0);
body.motorT.SetPower(0);
body.motorL.ResetTacho();
body.motorR.ResetTacho();
body.motorT.ResetTacho();
Balancer.init();
// スタート待ち
wait_start(body, connection);
var dialogRun = new InfoDialog("Running", false);
dialogRun.Show(); //Wait for enter to be pressed
try{
run(body, connection);
}catch (Exception) {
var dialogE = new InfoDialog("Exception.", false);
dialogE.Show(); //Wait for enter to be pressed
}
body.motorL.Off();
body.motorR.Off();
body.motorT.Off();
// ソケットを閉じる
if (connection != null)
{
connection.Close();
}
Lcd.Instance.Clear();
Lcd.Instance.Update();
if (Debugger.IsAttached)
{
Brick.ExitToMenu(); // MonoBrickメインメニューへ戻る
}
}
static void wait_start(EV3body body)
{
//スタート待ち
while (!body.touch.IsPressed())
{
tail_control(body, TAIL_ANGLE_STAND_UP);
Thread.Sleep(4);
}
}
public static void init(ref EV3body body)
{
body.motorL = new Motor(MotorPort.OutC);
body.motorR = new Motor(MotorPort.OutB);
body.motorT = new Motor(MotorPort.OutA);
body.touch = new EV3TouchSensor(SensorPort.In1);
body.sonar = new EV3UltrasonicSensor(SensorPort.In2, UltraSonicMode.Centimeter); // return [mm]
body.color = new EV3ColorSensor(SensorPort.In3, ColorMode.Reflection);
body.gyro = new EV3GyroSensor(SensorPort.In4, GyroMode.AngularVelocity);
}
static void run(EV3body body)
{
// 電圧を取得
int battery = Brick.GetVoltageMilliVolt();
sbyte forward;
sbyte turn;
sbyte oldPwmL = 0, oldPwmR = 0;
while (true)
{
tail_control(body, TAIL_ANGLE_DRIVE); // バランス走行用角度に制御
forward = 50;
turn = (body.color.Read() >= (LIGHT_BLACK + LIGHT_WHITE) / 2) ? (sbyte)50 : (sbyte)-50;
int gyroNow = body.gyro.Read();
int theTaL = body.motorL.GetTachoCount();
int theTaR = body.motorR.GetTachoCount();
// バックラッシュをキャンセル
Balancer.backlash_cancel(oldPwmL, oldPwmR, ref theTaL, ref theTaR);
sbyte pwmL, pwmR;
Balancer.control(
(float)forward, (float)turn, (float)gyroNow, (float)GYRO_OFFSET, (float)theTaL, (float)theTaR, (float)battery,
out pwmL, out pwmR
);
oldPwmL = pwmL;
oldPwmR = pwmR;
if (pwmL == 0)
{
body.motorL.Brake();
}
else
{
body.motorL.SetPower(pwmL);
}
if (pwmR == 0)
{
body.motorR.Brake();
}
else
{
body.motorR.SetPower(pwmR);
}
// バランス制御のみだと3msecで安定
// 尻尾制御と障害物検知を使用する場合2msecで安定
Thread.Sleep(1);
}
}
/*
* 超音波センサによる障害物検知
* @return true(障害物あり)/false(障害物無し)
*/
static bool sonar_alert(EV3body body)
{
int distance = body.sonar.Read();
if ((distance <= SONAR_ALERT_DISTANCE) && (distance >= 0))
{
return(true); /* 障害物を検知 */
}
else
{
return(false); /* 障害物無し */
}
}
const int PWM_ABS_MAX = 60; //完全停止用モータ制御PWM絶対最大値
public static void Main()
{
// 構造体の宣言と初期化
var body = new EV3body();
EV3body.init(ref body);
// Bluetooth関係のETロボコン拡張機能を有効にする
Brick.InstallETRoboExt();
// センサーおよびモータに対して初回アクセスをしておく
body.color.Read();
body.gyro.Read();
body.motorL.SetPower(0);
body.motorR.SetPower(0);
body.motorT.SetPower(0);
body.motorL.ResetTacho();
body.motorR.ResetTacho();
body.motorT.ResetTacho();
Balancer.init();
// スタート待ち
wait_start(body);
try{
run(body);
}catch (Exception) {
var dialogE = new InfoDialog("Exception.");
dialogE.Show(); //Wait for enter to be pressed
}
body.motorL.Off();
body.motorR.Off();
body.motorT.Off();
Lcd.Clear();
Lcd.Update();
if (Debugger.IsAttached)
{
Brick.ExitToMenu(); // MonoBrickメインメニューへ戻る
}
}
/*
* 走行体完全停止用モータの角度制御
* @param angle モータ目標角度[度]
*/
static void tail_control(EV3body body, int angle)
{
float pwm = (float)(angle - body.motorT.GetTachoCount()) * P_GAIN; // 比例制御
// PWM出力飽和処理
if (pwm > PWM_ABS_MAX)
{
pwm = PWM_ABS_MAX;
}
else if (pwm < -PWM_ABS_MAX)
{
pwm = -PWM_ABS_MAX;
}
if ((sbyte)pwm == 0)
{
body.motorT.Brake();
}
else
{
body.motorT.SetPower((sbyte)pwm);
}
}
static void wait_start(EV3body body, NetworkStream connection)
{
//スタート待ち
var dialogSTART = new InfoDialog("Touch to START", false);
dialogSTART.Show(); // Wait for enter to be pressed
while (!body.touch.IsPressed())
{
tail_control(body, TAIL_ANGLE_STAND_UP); //完全停止用角度に制御
if (checkRemoteCommand(connection, REMOTE_COMMAND_START))
{
break; // PC で 'g' キーが押された
}
Thread.Sleep(4);
}
while (body.touch.IsPressed())
{
tail_control(body, TAIL_ANGLE_STAND_UP); //完全停止用角度に制御
Thread.Sleep(4);
}
}
static void run(EV3body body, NetworkStream connection)
{
// 電圧を取得
int battery = Brick.GetVoltageMilliVolt();
sbyte forward;
sbyte turn;
int counter = 0;
bool alert = false;
while (!body.touch.IsPressed())
{
tail_control(body, TAIL_ANGLE_DRIVE); // バランス走行用角度に制御
if (checkRemoteCommand(connection, REMOTE_COMMAND_STOP))
{
break; // PC で 's' キー押されたら走行終了
}
if (++counter >= 40 / 4)
{
alert = sonar_alert(body);
counter = 0;
}
if (alert)
{
forward = 0;
turn = 0;
}
else
{
forward = 50;
turn = (body.color.Read() >= (LIGHT_BLACK + LIGHT_WHITE) / 2) ? (sbyte)50 : (sbyte)-50;
}
int gyroNow = body.gyro.Read();
int thetaL = body.motorL.GetTachoCount();
int theTaR = body.motorR.GetTachoCount();
sbyte pwmL, pwmR;
Balancer.control(
(float)forward, (float)turn, (float)gyroNow, (float)GYRO_OFFSET, (float)thetaL, (float)theTaR, (float)battery,
out pwmL, out pwmR
);
if (pwmL == 0)
{
body.motorL.Brake();
}
else
{
body.motorL.SetPower(pwmL);
}
if (pwmR == 0)
{
body.motorR.Brake();
}
else
{
body.motorR.SetPower(pwmR);
}
// バランス制御のみだと3msecで安定
// 尻尾制御と障害物検知を使用する場合2msecで安定
Thread.Sleep(2);
}
}
