예제 #1
0
파일: Main.cs 프로젝트: eskira/etroboEV3
        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メインメニューへ戻る
            }
        }
예제 #2
0
파일: EV3body.cs 프로젝트: eskira/etroboEV3
 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);
 }
예제 #3
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メインメニューへ戻る
			}
		}
예제 #4
0
		public static void init(ref EV3body body)
		{
			body.motorLeft = new Motor (MotorPort.OutC);
			body.motorRight = new Motor (MotorPort.OutB);
			body.motorTail = 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);
		}
예제 #5
0
파일: Main.cs 프로젝트: eskira/etroboEV3
        /*
         * 超音波センサによる障害物検知
         * @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);                /* 障害物無し */
            }
        }
예제 #6
0
파일: Main.cs 프로젝트: eskira/etroboEV3
        /*
         * 走行体完全停止用モータの角度制御
         * @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);
            }
        }
예제 #7
0
파일: Main.cs 프로젝트: eskira/etroboEV3
        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);
            }
        }
예제 #8
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);
			}
		}
예제 #9
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; /* 障害物無し */
			}
		}
예제 #10
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);
			}
		}
예제 #11
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);
			}
		}
예제 #12
0
파일: Main.cs 프로젝트: eskira/etroboEV3
        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);
            }
        }