Beispiel #1
0
        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メインメニューへ戻る
            }
        }
Beispiel #2
0
 static void wait_start(EV3body body)
 {
     //スタート待ち
     while (!body.touch.IsPressed())
     {
         tail_control(body, TAIL_ANGLE_STAND_UP);
         Thread.Sleep(4);
     }
 }
Beispiel #3
0
 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);
 }
Beispiel #4
0
        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);
            }
        }
Beispiel #5
0
        /*
         * 超音波センサによる障害物検知
         * @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);                /* 障害物無し */
            }
        }
Beispiel #6
0
        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メインメニューへ戻る
            }
        }
Beispiel #7
0
        /*
         * 走行体完全停止用モータの角度制御
         * @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);
            }
        }
Beispiel #8
0
        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);
            }
        }
Beispiel #9
0
        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);
            }
        }