コード例 #1
0
static bool buttonIsPressed(StopWatchButton id) {
	bool pressed = false;
	switch (id) {
		case StartButton:
		pressed = ev3_touch_sensor_is_pressed(EV3_PORT_1);
		break;
		case ResetButton:
		pressed = ev3_button_is_pressed(LEFT_BUTTON);
		break;
		case UpButton:
		pressed = ev3_button_is_pressed(UP_BUTTON);
		break;
		case DownButton:
		pressed = ev3_button_is_pressed(DOWN_BUTTON);
		break;
		case LeftButton:
		pressed = ev3_button_is_pressed(LEFT_BUTTON);
		break;
		case RightButton:
		pressed = ev3_button_is_pressed(RIGHT_BUTTON);
		break;
		default:
		break;
	}
	return pressed;
}
コード例 #2
0
ファイル: EV3B_bridge.c プロジェクト: cortlandstarrett/models
/*
 * Bridge:  is_button_pressed
 */
bool
  EV3B_is_button_pressed( const button_t p_button )
{
  /* Replace/Insert the following instructions with your implementation code.  */
  /* RETURN TRUE */
  return ev3_button_is_pressed(p_button) ? TRUE : FALSE;

}
コード例 #3
0
ファイル: button.c プロジェクト: yuuu/mruby-toppers-ev3rt
/*
 *  call-seq:
 *     btn.pressed?  # => true/false
 *
 *  Get button status.
 *
 *  Returns button status. (true:pressed, false:released)
 */
static mrb_value
mrb_btn_pressed(mrb_state *mrb, mrb_value self)
{
  int32_t pressed = FALSE;
  mrb_value key;

  key = mrb_iv_get(mrb, self, mrb_intern_lit(mrb, "@key"));
  if (!mrb_nil_p(key)) {
    pressed = ev3_button_is_pressed((int16_t)mrb_fixnum(key));
  }

  return mrb_bool_value(pressed);
}
コード例 #4
0
ファイル: PidWalker.cpp プロジェクト: Morichan/ETrobocon-2016
/*
 * 加速機能
 * 最初は遅めにして、だんだんスピードを上げる
 * 一つ目の引数はスタートするときのスピード
 * 二つ目の引数までのスピードに上げる
 */
void PidWalker::accelerate(int8_t startForward, int8_t _forward) {
    int32_t count = 0;

    /*
     * もしstartForward == 0なら、現在の速度から更に加速
     */
    if(startForward != 0) {
        forward = startForward;

        // 最初のスピードより最後のスピードのほうが遅いときは0からに強制
        if(forward > _forward || forward < 0) {
            forward = 0;
        }

        // 最終的なスピードがマイナスの時は、初期値と同じにする(加速しない)
        if(_forward < 0) {
            _forward = forward;
        }
    }

    while(1) {
        if(ev3_button_is_pressed(BACK_BUTTON)) {
            break;
        }
        if(forward >= _forward) {
            break;
        }

        brightness = colorSensor.getBrightness();

        pid.calculate(brightness);
        turn = (int8_t)pid.get_output();

        walker.run(forward, turn);

        /*
         * 20msec周期でforwardを増やす
         */
        count++;
        if(count % 5 == 0) {
            forward++;
        }
        clock.sleep(4); /* 4msec周期起動 */
    }
}
コード例 #5
0
ファイル: PidWalker.cpp プロジェクト: Morichan/ETrobocon-2016
/*
 * ブレーキ機能
 * だんだんスピードを下げる
 * 一つ目の引数は初期スピード、0なら現在のスピード
 * 二つ目の引数までのスピードに下げる
 */
void PidWalker::brake(int8_t startForward, int8_t _forward) {
    int32_t count = 0;

    if(startForward > 0) {
        forward = startForward;
    }

    if(forward < _forward) {
        forward = _forward;
    }

    while(1) {
        if(ev3_button_is_pressed(BACK_BUTTON)) {
            break;
        }
        if(forward <= _forward) {
            break;
        }

        brightness = colorSensor.getBrightness();

        pid.calculate(brightness);
        turn = (int8_t)pid.get_output();

        walker.run(forward, turn);

        /*
         * 12msec周期でforwardを減らす
         */
        count++;
        if(count % 3 == 0) {
            forward--;
        }
        clock.sleep(4); /* 4msec周期起動 */
    }
}
コード例 #6
0
ファイル: UIGet.cpp プロジェクト: MaedaSumire/2016eitnreotb
//メソッド: UI ユーザ操作を取得する() return UI(構造体)
UI UIGet::UIGetter(){
	UI ui;
	ui.Button = 'N';
	ui.btcKey = 0;
	ui.touch = false;

	if(ev3_button_is_pressed(BACK_BUTTON)){
		ui.Button = 'B';
	}
	else if(ev3_button_is_pressed(LEFT_BUTTON)){
		ui.Button = 'L';
	}
	else if(ev3_button_is_pressed(RIGHT_BUTTON)){
		ui.Button = 'R';
	}
	else if(ev3_button_is_pressed(UP_BUTTON)){
		ui.Button = 'U';
	}
	else if(ev3_button_is_pressed(DOWN_BUTTON)){
		ui.Button = 'D';
	}
	else if(ev3_button_is_pressed(ENTER_BUTTON)){
		ui.Button = 'E';
	}
	else if(m_pDeviceInterface->m_pCTouchSensor->isPressed()){
		ui.touch = true;
	}

	if(mBlueT.btcKey >= '0'){
		// キーボード入力	app.c bt_task()にて取得される
		ui.btcKey	= mBlueT.btcKey;	// キー取得
		mBlueT.btcKey	= 0;			// キークリア

		char	sbuff[100];
		sprintf(sbuff,"test!! [%c]  \n",ui.btcKey);
		fputs( sbuff, mBlueT.pBtFile); 		// エコーバック
	}

	return ui;
}
コード例 #7
0
ファイル: app.cpp プロジェクト: koki-hosokawa/frobo
/* メインタスク */
void main_task(intptr_t unused)
{
    signed char forward;      /* 前後進命令 */
    signed char turn;         /* 旋回命令 */
    signed char pwm_L, pwm_R; /* 左右モータPWM出力 */

    /* LCD画面表示 */
    ev3_lcd_fill_rect(0, 0, EV3_LCD_WIDTH, EV3_LCD_HEIGHT, EV3_LCD_WHITE);
    ev3_lcd_draw_string("ミヤウチ", 0, CALIB_FONT_HEIGHT*1);

    /* センサー入力ポートの設定 */
    ev3_sensor_config(sonar_sensor, ULTRASONIC_SENSOR);
    ev3_sensor_config(color_sensor, COLOR_SENSOR);
    ev3_color_sensor_get_reflect(color_sensor); /* 反射率モード */
    ev3_sensor_config(touch_sensor, TOUCH_SENSOR);
    ev3_sensor_config(gyro_sensor, GYRO_SENSOR);
    /* モーター出力ポートの設定 */
    ev3_motor_config(left_motor, LARGE_MOTOR);
    ev3_motor_config(right_motor, LARGE_MOTOR);
    ev3_motor_config(tail_motor, LARGE_MOTOR);
    ev3_motor_reset_counts(tail_motor);
    
     //キャリブレイト 白取得
    printf("Press the touch sensor to measure light intensity of WHITE.\n");
    while(!ev3_touch_sensor_is_pressed(touch_sensor));
    while(ev3_touch_sensor_is_pressed(touch_sensor));
    int white = ev3_color_sensor_get_reflect(color_sensor);
    printf("WHITE light intensity: %d.\n", white);

    //キャリブレイト 黒取得
    printf("Press the touch sensor to measure light intensity of BLACK.\n");
    while(!ev3_touch_sensor_is_pressed(touch_sensor));
    while(ev3_touch_sensor_is_pressed(touch_sensor));
    int black = ev3_color_sensor_get_reflect(color_sensor);
    printf("BLACK light intensity: %d.\n", black);

    //PID制御
    float lasterror = 0, integral = 0;
    float midpoint = (white - black) / 2 + black;

    /* Open Bluetooth file */
    bt = ev3_serial_open_file(EV3_SERIAL_BT);
    assert(bt != NULL);

    /* Bluetooth通信タスクの起動 */
    act_tsk(BT_TASK);

    ev3_led_set_color(LED_ORANGE); /* 初期化完了通知 */

    /* スタート待機 */
    while(1)
    {
    //尻尾さげる
        tail_control(TAIL_ANGLE_STAND_UP); 

        if (bt_cmd == 1)
        {
            //リモートスタート
            break; 
        }

        if (ev3_touch_sensor_is_pressed(touch_sensor) == 1)
        {
            //タッチセンサが押された
            break; 
        }

        tslp_tsk(1);
    }

    /* 走行モーターエンコーダーリセット */
    ev3_motor_reset_counts(left_motor);
    ev3_motor_reset_counts(right_motor);

    /* ジャイロセンサーリセット */
    ev3_gyro_sensor_reset(gyro_sensor);
    balancer.init(GYRO_OFFSET);                // <1>

	/* スタート(LED緑色) */
    ev3_led_set_color(LED_GREEN); 

    while(1)
	{
        int32_t motor_ang_l, motor_ang_r;
        int gyro, volt;

        if (ev3_button_is_pressed(BACK_BUTTON)) break;

        tail_control(TAIL_ANGLE_DRIVE); /* バランス走行用角度に制御 */

		forward = 30;
		
        float error = midpoint - ev3_color_sensor_get_reflect(color_sensor);
        integral = error + integral * 0.5;
        turn = 0.07 * error + 0.3 * integral + 1 * (error - lasterror);
           // float steer = 0.07 * error + 0.3 * integral + 1 * (error - lasterror);
           // ev3_motor_steer(left_motor, right_motor, 10, steer);
        lasterror = error;
        tslp_tsk(1);

        /* 倒立振子制御API に渡すパラメータを取得する */
        motor_ang_l = ev3_motor_get_counts(left_motor);
        motor_ang_r = ev3_motor_get_counts(right_motor);
        gyro = ev3_gyro_sensor_get_rate(gyro_sensor);
        volt = ev3_battery_voltage_mV();

        /* 倒立振子制御APIを呼び出し、倒立走行するための */
        /* 左右モータ出力値を得る */
        balancer.setCommand(forward, turn);   // <1>
        balancer.update(gyro, motor_ang_r, motor_ang_l, volt); // <2>
        pwm_L = balancer.getPwmRight();       // <3>
        pwm_R = balancer.getPwmLeft();        // <3>

        /* EV3ではモーター停止時のブレーキ設定が事前にできないため */
        /* 出力0時に、その都度設定する */
        if (pwm_L == 0)
        {
             ev3_motor_stop(left_motor, true);
        }
        else
        {
            ev3_motor_set_power(left_motor, (int)pwm_L);
        }

        if (pwm_R == 0)
        {
             ev3_motor_stop(right_motor, true);
        }
        else
        {
         ev3_motor_set_power(right_motor, (int)pwm_R);
        }
	

        tslp_tsk(4); /* 4msec周期起動 */
	}
    ev3_motor_stop(left_motor, false);
    ev3_motor_stop(right_motor, false);

    ter_tsk(BT_TASK);
    fclose(bt);

    ext_tsk();
}
void main_task(intptr_t unused) {
	int grey = (BLACK+WHITE)/2;
	int white_side = (BLACK+WHITE*2)/3;
	int black_side = (BLACK*3+WHITE)/4;
	int light = 0;
	int count = 0;
	unsigned int timeCounter = 1;
	int flag = 0;
	float turn_angle = 0.0;
	int distace = 0;
	int dflag = 0;

	pid *my_pid = pid_make(4.0, 0.2, 0.1, grey); //default pid 1.0 0.02 0.0
	pid *white_pid = pid_make(4.0, 0.2, 0.1, white_side);
	pid *black_pid = pid_make(4.0, 0.2, 0.1, black_side);

	turn_angle_structure *tas = turn_angle_constructor();

	ev3_lcd_fill_rect(0, 0, EV3_LCD_WIDTH, EV3_LCD_HEIGHT, EV3_LCD_WHITE);
	ev3_lcd_draw_string("EV3way ETRobcon 2015 KatLab", 0, CALIB_FONT_HEIGHT*1);
	ev3_lcd_draw_string("Ver.20150508", 0, CALIB_FONT_HEIGHT*2);
	ev3_lcd_draw_string("right side", 0, CALIB_FONT_HEIGHT*3);
    // Configure motors
  ev3_motor_config(STEER, LARGE_MOTOR);
  ev3_motor_config(DRIVE_L, LARGE_MOTOR);
  ev3_motor_config(DRIVE_R, LARGE_MOTOR);

    // Configure sensors
  ev3_sensor_config(IR, COLOR_SENSOR);
  ev3_sensor_config(TOUCH, TOUCH_SENSOR);
	ev3_sensor_config(GYRO, GYRO_SENSOR);

  ev3_led_set_color(LED_GREEN);

	ev3_motor_reset_counts(STEER);
	ev3_motor_reset_counts(DRIVE_L);
	ev3_motor_reset_counts(DRIVE_R);


	ev3_motor_set_power(STEER,0);


	ev3_gyro_sensor_reset(GYRO);

	while(!ev3_touch_sensor_is_pressed(TOUCH)){
	  //tslp_tsk(500); /* 500msec wait */
	}
	while(!ev3_button_is_pressed(BACK_BUTTON)){
	  light = ev3_color_sensor_get_reflect(IR);
	  count = ev3_motor_get_counts(STEER);

	  switch(changeStyle){
	  case 0://始めのカーブ差し掛かりまで
	    count = ev3_motor_get_counts(STEER);
	    forward = 55;
	    pid_input(my_pid, ev3_color_sensor_get_reflect(IR));
	    turn = (signed char) pid_get_output(*my_pid);

	    if(turn > 100){
	      turn = 100;
	    }else if(turn < -100){
	      turn = -100;
	    }

	    turn = -turn;

	    if(count<MAX_STEERING_ANGLE && count>-MAX_STEERING_ANGLE)
	      {
		ev3_motor_set_power(STEER, turn/2);
	      }else if(count<=-MAX_STEERING_ANGLE && turn>=0){
	      ev3_motor_set_power(STEER, turn/2);
	    }else if(count>=MAX_STEERING_ANGLE && turn<0){
	      ev3_motor_set_power(STEER, turn/2);
	    }else{
	      ev3_motor_stop(STEER, true);
	    }


	    if((ev3_motor_get_counts(DRIVE_L)+ev3_motor_get_counts(DRIVE_R))/2 < (-7300 + 2400)){
	      ev3_lcd_draw_string("changeStyle 1", 0, 70);
	      changeStyle = 1;
	    }

	    break;
	  case 1: //カーブ白より走行
	    count = ev3_motor_get_counts(STEER);
	    forward = 26;
	    pid_input(white_pid, ev3_color_sensor_get_reflect(IR));
	    turn = (signed char) pid_get_output(*white_pid);

	    if(turn > 0){
	      turn = turn*2;
	    }
	    if(turn > 100){
	      turn = 100;
	    }else if(turn < -100){
	      turn = -100;
	    }

	    turn = -turn;

	    if(count<MAX_STEERING_ANGLE && count>-MAX_STEERING_ANGLE)
	      {
		ev3_motor_set_power(STEER, turn/2);
	      }else if(count<=-MAX_STEERING_ANGLE && turn>=0){
	      ev3_motor_set_power(STEER, turn/2);
	    }else if(count>=MAX_STEERING_ANGLE && turn<0){
	      ev3_motor_set_power(STEER, turn/2);
	    }else{
	      ev3_motor_stop(STEER, true);
	    }


	    if((ev3_motor_get_counts(DRIVE_L)+ev3_motor_get_counts(DRIVE_R))/2 < (-9200 + 2400)){
	      changeStyle = 2;
	    }


	    break;
	  case 2://段差検知まで 黒より走行
	    count = ev3_motor_get_counts(STEER);
	    forward = 30;
	    pid_input(black_pid, ev3_color_sensor_get_reflect(IR));
	    turn = (signed char) pid_get_output(*black_pid);

	    if(turn < 0){
	      turn = turn*2;
	    }
	    if(turn > 100){
	      turn = 100;
	    }else if(turn < -100){
	      turn = -100;
	    }

	    turn = -turn;

	    if(count<MAX_STEERING_ANGLE && count>-MAX_STEERING_ANGLE)
	      {
		ev3_motor_set_power(STEER, turn/2);
	      }else if(count<=-MAX_STEERING_ANGLE && turn>=0){
	      ev3_motor_set_power(STEER, turn/2);
	    }else if(count>=MAX_STEERING_ANGLE && turn<0){
	      ev3_motor_set_power(STEER, turn/2);
	    }else{
	      ev3_motor_stop(STEER, true);
	    }


	    if(stepDetective(12)==1){
	      changeStyle = 3;
	      ev3_lcd_draw_string("changeStyle Now", 0, 70);
	      flag = 1;
	    }

	    break;
	  case 3://二本橋クリアする
	    if(bridge(timeCounter) == 1){
	      changeStyle = 13;
	    }
	    timeCounter++;
	    break;
	  case 4://止まる
	    if(ev3_color_sensor_get_color(IR) == 6 || ev3_color_sensor_get_color(IR) == 3){
	      ev3_motor_set_power(STEER,0);
	      ev3_motor_set_power(DRIVE_L,0);
	      ev3_motor_set_power(DRIVE_R,0);
	    }else{
	      ev3_motor_set_power(STEER,0);
	      ev3_motor_set_power(DRIVE_L,-20);
	      ev3_motor_set_power(DRIVE_R,-20);
	    }
	    break;

	  case 5://バーコード終了後ライントレース
	    count = ev3_motor_get_counts(STEER);
	    forward = 25;
	    pid_input(my_pid, ev3_color_sensor_get_reflect(IR));
	    turn = (signed char) pid_get_output(*my_pid);

	    if(turn > 100){
	      turn = 100;
	    }else if(turn < -100){
	      turn = -100;
	    }

	    turn = -turn;

	    if(count<MAX_STEERING_ANGLE && count>-MAX_STEERING_ANGLE)
	      {
		ev3_motor_set_power(STEER, turn/2);
	      }else if(count<=-MAX_STEERING_ANGLE && turn>=0){
	      ev3_motor_set_power(STEER, turn/2);
	    }else if(count>=MAX_STEERING_ANGLE && turn<0){
	      ev3_motor_set_power(STEER, turn/2);
	    }else{
	      ev3_motor_stop(STEER, true);
	    }

	    if((ev3_motor_get_counts(DRIVE_L)+ev3_motor_get_counts(DRIVE_R))/2 < -300){
	      changeStyle = 6;
	      flag = 1;
	      ev3_motor_reset_counts(DRIVE_L);
	      ev3_motor_reset_counts(DRIVE_R);
	    }
	    break;
	  case 6://まっすぐ走る
	    ev3_motor_set_power(STEER,0);
	    ev3_motor_set_power(DRIVE_L,-40);
	    ev3_motor_set_power(DRIVE_R,-40);
	    if((ev3_motor_get_counts(DRIVE_L)+ev3_motor_get_counts(DRIVE_R))/2 < -750){
	      changeStyle = 7;
	      ev3_motor_set_power(DRIVE_L,0);
	      ev3_motor_set_power(DRIVE_R,0);
	      timeCounter = 0;
	    }
	    break;

	  case 7://前輪を真横にする
	    if(timeCounter <= 9){
	      ev3_motor_rotate(STEER,90,50,true);
	    }else if(timeCounter > 9){
	      changeStyle = 8;
	      ev3_lcd_draw_string("changeStyle Now", 0, 70);
	      timeCounter = 0;
	    }
	    timeCounter++;
	    break;
	  case 8://車体を左向きに
	    turn_angle = turn_angle_detection(tas);
	    if(turn_angle >= -76.0){
	      ev3_motor_set_power(STEER,0);
	      ev3_motor_set_power(DRIVE_L,30);
	      ev3_motor_set_power(DRIVE_R,-20);
	    }else if(turn_angle < -76.0){
	      ev3_motor_set_power(STEER,0);
	      ev3_motor_set_power(DRIVE_L,0);
	      ev3_motor_set_power(DRIVE_R,0);
	      changeStyle = 9;
	    }
	    break;
	  case 9://前輪を戻す
	    ev3_motor_rotate(STEER,-900,50,true);
	    changeStyle = 10;
	    ev3_lcd_draw_string("changeStyle Now", 0, 70);
	    ev3_motor_reset_counts(DRIVE_L);
	    ev3_motor_reset_counts(DRIVE_R);
	    break;

	  case 10://使用未確定エリアに乗る
	    ev3_motor_set_power(DRIVE_L,-50);
	    ev3_motor_set_power(DRIVE_R,-50);
	    if((ev3_motor_get_counts(DRIVE_L)+ev3_motor_get_counts(DRIVE_R))/2 < -500){
	      timeCounter++;
	      ev3_motor_set_power(DRIVE_L,0);
	      ev3_motor_set_power(DRIVE_R,0);
	    }
	    if(timeCounter >= 100){
	      timeCounter=0;
	      changeStyle=11;
	    }
	    break;
	  case 11://ウィリー走行に切り替え
	    if(tail_run(timeCounter) == 1){
	      changeStyle = 12;
	      timeCounter = 0;
	    }else{
	      timeCounter++;
	    }

	    break;

	  case 12://止まる
	    ev3_motor_set_power(STEER,0);
	    ev3_motor_set_power(DRIVE_L,0);
	    ev3_motor_set_power(DRIVE_R,0);
	    break;

	  case 13://ライン復帰
	    ev3_motor_rotate(STEER,940,50,true);
	    changeStyle = 14;
	    ev3_lcd_draw_string("changeStyle Now", 0, 70);
	    timeCounter = 1;
	    break;
	  case 14:
	    if(timeCounter < 37){
	      ev3_motor_set_power(STEER,0);
	      ev3_motor_set_power(DRIVE_L,30);
	      ev3_motor_set_power(DRIVE_R,-30);
	    }else if(timeCounter >= 37){
	      ev3_motor_set_power(STEER,0);
	      ev3_motor_set_power(DRIVE_L,0);
	      ev3_motor_set_power(DRIVE_R,0);
	      changeStyle = 15;
	      timeCounter = 0;
	    }timeCounter++;
	    break;
	  case 15:
	    ev3_motor_rotate(STEER,-1000,50,true);
	    changeStyle = 16;
	    ev3_lcd_draw_string("changeStyle Now", 0, 70);
	    break;
	  case 16:

	    ev3_motor_set_power(STEER,0);
	    if(timeCounter <= 20){
	      ev3_motor_set_power(DRIVE_L,15);
	      ev3_motor_set_power(DRIVE_R,15);
	    }else if(timeCounter > 20 && (timeCounter -20) /60 % 2 == 1){
	      ev3_motor_set_power(DRIVE_L,15);
	      ev3_motor_set_power(DRIVE_R,15);
	    }else if(timeCounter > 20 && (timeCounter - 20) /60 % 2 == 0){
	      ev3_motor_set_power(DRIVE_L,-15);
	      ev3_motor_set_power(DRIVE_R,-15);
	    }
	    timeCounter++;
	    if(ev3_color_sensor_get_color(IR) == 1){
	      changeStyle = 30;
	      ev3_motor_set_power(DRIVE_L,0);
	      ev3_motor_set_power(DRIVE_R,0);
	      timeCounter = 0;
	    }
	    break;
	  case 17:
	    ev3_motor_rotate(STEER,830,50,true);
	    changeStyle = 18;
	    ev3_lcd_draw_string("changeStyle Now", 0, 70);
	    break;
	  case 18:
	    if(ev3_color_sensor_get_reflect(IR) > white_side){
	      ev3_motor_set_power(STEER,0);
	      ev3_motor_set_power(DRIVE_L,-6);
	      ev3_motor_set_power(DRIVE_R,6);
	    }else if(ev3_color_sensor_get_reflect(IR) < white_side){
	      ev3_motor_set_power(STEER,0);
	      ev3_motor_set_power(DRIVE_L,0);
	      ev3_motor_set_power(DRIVE_R,0);
	      changeStyle = 19;
	    }
	    break;
	  case 19:
	    ev3_motor_rotate(STEER,-950,40,true);
	    changeStyle = 20;
	    ev3_lcd_draw_string("changeStyle Now", 0, 70);
	    break;
	  case 20:
	    ev3_motor_set_power(STEER,0);
	    ev3_motor_set_power(DRIVE_L,-17);
	    ev3_motor_set_power(DRIVE_R,-15);
	    if(ev3_color_sensor_get_color(IR) == 1){
	      changeStyle = 21;
	      flag = 0;
	      ev3_motor_reset_counts(DRIVE_L);
	      ev3_motor_reset_counts(DRIVE_R);
	    }
	    break;
	  case 21:
	    count = ev3_motor_get_counts(STEER);
	    forward = 14;
	    pid_input(my_pid, ev3_color_sensor_get_reflect(IR));
	    turn = (signed char) pid_get_output(*my_pid);

	    if(turn > 100){
	      turn = 100;
	    }else if(turn < -100){
	      turn = -100;
	    }


	    if(count<MAX_STEERING_ANGLE && count>-MAX_STEERING_ANGLE)
	      {
		ev3_motor_set_power(STEER, turn/2);
	      }else if(count<=-MAX_STEERING_ANGLE && turn>=0){
	      ev3_motor_set_power(STEER, turn/2);
	    }else if(count>=MAX_STEERING_ANGLE && turn<0){
	      ev3_motor_set_power(STEER, turn/2);
	    }else{
	      ev3_motor_stop(STEER, true);
	    }

	    if((ev3_motor_get_counts(DRIVE_L)+ev3_motor_get_counts(DRIVE_R))/2 < -3000){
	      changeStyle = 5;
	      ev3_motor_reset_counts(DRIVE_L);
	      ev3_motor_reset_counts(DRIVE_R);
	    }

	    break;
	  case 30:
	    ev3_motor_set_power(STEER,0);
	    if(timeCounter <= 65){
	      ev3_motor_set_power(DRIVE_L,-17);
	      ev3_motor_set_power(DRIVE_R,-17);
	    }else if(timeCounter > 65 && timeCounter <= 120){
	      ev3_motor_set_power(DRIVE_L,0);
	      ev3_motor_set_power(DRIVE_R,0);
	    }else if(timeCounter > 120){
	      changeStyle = 17;
	      timeCounter = 0;
	    }			timeCounter++;
	  default:

	    break;
	  }
	  if(flag==0){
	    ev3_motor_steer(DRIVE_L, DRIVE_R, -forward, -turn);
	  }

	  tslp_tsk(10); /* 10msec wait */

	}

	ev3_motor_stop(STEER, false);
	ev3_motor_stop(DRIVE_L, false);
	ev3_motor_stop(DRIVE_R, false);

	ext_tsk();
}
コード例 #9
0
void main_task(intptr_t unused) {
  int grey = (BLACK+WHITE)/2;
  //int light = 0;
  //int count = 0;

  // Configure motors
  ev3_motor_config(STEER, LARGE_MOTOR);
  ev3_motor_config(DRIVE_L, LARGE_MOTOR);
  ev3_motor_config(DRIVE_R, LARGE_MOTOR);

  // Configure sensors
  ev3_sensor_config(IR, COLOR_SENSOR);
  ev3_sensor_config(TOUCH, TOUCH_SENSOR);

  ev3_led_set_color(LED_GREEN);

  ev3_motor_reset_counts(STEER);
  ev3_motor_reset_counts(DRIVE_L);
  ev3_motor_reset_counts(DRIVE_R);

  while(!ev3_touch_sensor_is_pressed(TOUCH));
  tslp_tsk(500); /* 500msec wait */

  char str[20];
  sprintf(str, "%d", 1);
  ev3_lcd_draw_string(str, 0, 0);


  /*
  //戦略の初期化(もろもろやってくれる
  //上のSTRATEGYさえ変えれば、該当の戦略に対する戦術がセットされる
  Strategy* strategy = Strategy_make(STRATEGY);

  //ライントレースクラスのインスタンス
  //各メンバ変数へのset関数を用いて走りをコントロール
  //これ以降もset関数で値を各戦略内で自由に変えることが出来る
  Line_tracer* line_tracer = Line_tracer_make();
  Line_tracer_init_pid(line_tracer, 4.0, 0.2, 0.1, grey);
  Line_tracer_set_current_edge(line_tracer, right);
  Line_tracer_set_target_grey_point(line_tracer, grey);
  Line_tracer_set_forward(line_tracer, 0);
  Line_tracer_set_turn(line_tracer, 0);
  */

  strategy = Strategy_make(STRATEGY);
  line_tracer = Line_tracer_make();
  Line_tracer_init_pid(line_tracer, 4.0, 0.2, 0.1, grey);
  Line_tracer_set_current_edge(line_tracer, right);
  Line_tracer_set_target_grey_point(line_tracer, grey);
  Line_tracer_set_forward(line_tracer, 0);
  Line_tracer_set_turn(line_tracer, 0);

  while(!ev3_button_is_pressed(BACK_BUTTON)) {
    Strategy_update(strategy);
    Line_tracer_output(line_tracer);
    tslp_tsk(10); /* 10msec wait */
    /*
    light = ev3_color_sensor_get_reflect(IR);
    count = ev3_motor_get_counts(STEER);
    if(light>grey){
      if(count<MAX_STEERING_ANGLE){
	ev3_motor_set_power(STEER, 100);
      }else{
	ev3_motor_stop(STEER, true);
      }
      ev3_motor_set_power(DRIVE_L, -DRIVING_POWER);
      ev3_motor_set_power(DRIVE_R,   1);
    }else{
      if(count>-MAX_STEERING_ANGLE){
	ev3_motor_set_power(STEER, -100);
      }else{
	ev3_motor_stop(STEER, true);
      }
      ev3_motor_set_power(DRIVE_L,   1);
      ev3_motor_set_power(DRIVE_R, -DRIVING_POWER);
    */
  }

  ev3_motor_stop(STEER, false);
  ev3_motor_stop(DRIVE_L, false);
  ev3_motor_stop(DRIVE_R, false);

  ext_tsk();
}
コード例 #10
0
ファイル: app.cpp プロジェクト: koki-hosokawa/frobo
/* メインタスク */
void main_task(intptr_t unused)
{
    signed char forward;      /* 前後進命令 */
    signed char turn;         /* 旋回命令 */
    signed char pwm_L, pwm_R; /* 左右モータPWM出力 */
	static int turn = 0;

    /* LCD画面表示 */
    ev3_lcd_fill_rect(0, 0, EV3_LCD_WIDTH, EV3_LCD_HEIGHT, EV3_LCD_WHITE);
    ev3_lcd_draw_string("ETミヤウチ", 0, CALIB_FONT_HEIGHT*1);

    /* センサー入力ポートの設定 */
    ev3_sensor_config(sonar_sensor, ULTRASONIC_SENSOR);
    ev3_sensor_config(color_sensor, COLOR_SENSOR);
    ev3_color_sensor_get_reflect(color_sensor); /* 反射率モード */
    ev3_sensor_config(touch_sensor, TOUCH_SENSOR);
    ev3_sensor_config(gyro_sensor, GYRO_SENSOR);
    /* モーター出力ポートの設定 */
    ev3_motor_config(left_motor, LARGE_MOTOR);
    ev3_motor_config(right_motor, LARGE_MOTOR);
    ev3_motor_config(tail_motor, LARGE_MOTOR);
    ev3_motor_reset_counts(tail_motor);

    /* Open Bluetooth file */
    bt = ev3_serial_open_file(EV3_SERIAL_BT);
    assert(bt != NULL);

    /* Bluetooth通信タスクの起動 */
    act_tsk(BT_TASK);

    ev3_led_set_color(LED_ORANGE); /* 初期化完了通知 */

    /* スタート待機 */
    while(1)
    {
        tail_control(TAIL_ANGLE_STAND_UP); /* 完全停止用角度に制御 */

        if (bt_cmd == 1)
        {
            break; /* リモートスタート */
        }

        if (ev3_touch_sensor_is_pressed(touch_sensor) == 1)
        {
            break; /* タッチセンサが押された */
        }

        tslp_tsk(10); /* 10msecウェイト */
    }

    /* 走行モーターエンコーダーリセット */
    ev3_motor_reset_counts(left_motor);
    ev3_motor_reset_counts(right_motor);

    /* ジャイロセンサーリセット */
    ev3_gyro_sensor_reset(gyro_sensor);
    balancer.init(GYRO_OFFSET);                // <1>

    ev3_led_set_color(LED_GREEN); /* スタート通知 */

    /**
    * Main loop for the self-balance control algorithm
    */
    while(1)
    {
        int32_t motor_ang_l, motor_ang_r;
        int gyro, volt;
    	
    	turn = (ev3_color_sensor_get_reflect(color_sensor) - (LIGHT_WHITE + LIGHT_BLACK)/2) * KP;
        	

        if (ev3_button_is_pressed(BACK_BUTTON)) break;

        tail_control(TAIL_ANGLE_DRIVE); /* バランス走行用角度に制御 */

        if (sonar_alert() == 1) /* 障害物検知 */
        {
            forward = turn = 0; /* 障害物を検知したら停止 */
        }
        else
        {
            forward = 30; /* 前進命令 */
 		if (100 < turn) {
            turn = 100.0;
        } else if (turn < -100) {
            turn = -100.0;
        }
        }

        /* 倒立振子制御API に渡すパラメータを取得する */
        motor_ang_l = ev3_motor_get_counts(left_motor);
        motor_ang_r = ev3_motor_get_counts(right_motor);
        gyro = ev3_gyro_sensor_get_rate(gyro_sensor);
        volt = ev3_battery_voltage_mV();

        /* 倒立振子制御APIを呼び出し、倒立走行するための */
        /* 左右モータ出力値を得る */
        balancer.setCommand(forward, turn);   // <1>
        balancer.update(gyro, motor_ang_r, motor_ang_l, volt); // <2>
        pwm_L = balancer.getPwmRight();       // <3>
        pwm_R = balancer.getPwmLeft();        // <3>

        /* EV3ではモーター停止時のブレーキ設定が事前にできないため */
        /* 出力0時に、その都度設定する */
        if (pwm_L == 0)
        {
             ev3_motor_stop(left_motor, true);
        }
        else
        {
            ev3_motor_set_power(left_motor, (int)pwm_L);
        }

        if (pwm_R == 0)
        {
             ev3_motor_stop(right_motor, true);
        }
        else
        {
            ev3_motor_set_power(right_motor, (int)pwm_R);
        }

        tslp_tsk(4); /* 4msec周期起動 */
    }
    ev3_motor_stop(left_motor, false);
    ev3_motor_stop(right_motor, false);

    ter_tsk(BT_TASK);
    fclose(bt);

    ext_tsk();
}