void Tactics_update(Tactics* tactics){
  char str[20];

  //それぞれの戦術に飛ぶ 戦術に飛んだら、引数として渡したtacticsのcurrent_stepを見て動作していく
  //各戦術の中でforward, turn値を決め, モーター制御クラスに渡しておくこと
  switch(tactics->tactics_current_name){
  case TEST_tactics1:
    sprintf(str, "%d", 3);
    ev3_lcd_draw_string(str, 0, 0);
    Tactics_TEST1(tactics);
    break;
  case TEST_tactics2:
    sprintf(str, "%d", 4);
    ev3_lcd_draw_string(str, 0, 0);
    Tactics_TEST2(tactics);
    break;

    /*  case STANDARD:
    Tactics_standard(strategy->tactics_main);
    break;
  case U_TURN:
    Tactics_u_turn(stratgy->tactics_main);
    break;
  case R_FIGURE_L:
    Tactics_r_figure(strategy->tactics_main);
    break;
  case R_SHINKANSEN:
    Tactics_r_shinkansen(strategy->tactics_main);
    break;
  case R_PARALLEL_PARKING:
    Tactics_r_parallel_parking(strategy->tactics_main);
    break;
  case L_DUAL_BRIDGE:
    Tactics_l_dual_bridge(strategy->tactics_main);
    break;
  case L_BARCODE:
    Tactics_l_barcode(strategy->tactics_main);
    break;
  case L_SPECIFICATIONS_UNDETERMAINED_AREA:
    Tactics_l_specifications_undetermained_area(strategy->tactics_main);
    break;
  case L_RIGHT_ANGLE_PARKING:
    Tactics_l_right_angle_parking(strategy->tactics_main);
    break;*/
  case FINISHED:
    Tactics_FINISHED(tactics);
    break;
  default:
    break;
  }

  //各戦術内で渡されたforward,turnを元にLine_traceクラスのメソッドでモーターを動かす
  //今はまだ無いので,

}
示例#2
0
文件: app.c 项目: masanori1102/ev3rt
void test_ev3_cychdr(intptr_t idx) {
    ev3_lcd_fill_rect(0, fonth * idx, EV3_LCD_WIDTH, fonth, EV3_LCD_WHITE);
    char buf[100];
    sprintf(buf, "EV3CYC %d count %d", idx, ++count[idx]);
    ev3_lcd_draw_string(buf, 0, fonth * idx);
	SVC_PERROR(tslp_tsk(10));
}
示例#3
0
/*
 *  call-seq:
 *     LCD.draw(x, y, str)  # => nil
 *
 *  Draw the string at specified coordinate of LCD.
 *
 *  Parameters:
 *    +x+     X-coordinate of the string left edge
 *    +y+     Y-coordinate of the string top edge
 *    +str+   The target for drawing.
 *
 *  Returns nil.
 */
static mrb_value
mrb_lcd_draw_string(mrb_state *mrb, mrb_value self)
{
  mrb_value obj, str;
  mrb_int x, y;
  mrb_int fw, fh;
  mrb_int cw, ch;

  mrb_lcd_get_font_size(mrb, self, &fw, &fh);
  cw = EV3_LCD_WIDTH  / fw;
  ch = EV3_LCD_HEIGHT / fh;

  mrb_get_args(mrb, "iio", &x, &y, &obj);

  if (mrb_string_p(obj)) {
    str = obj;
  }
  else {
    str = mrb_funcall(mrb, obj, "to_s", 0);
  }

  ev3_lcd_draw_string(mrb_string_value_cstr(mrb, &str), x*cw, y*ch);

  return mrb_nil_value();
}
示例#4
0
/*
 * Bridge:  lcd_draw_string
 */
void
EV3B_lcd_draw_string( c_t p_string[ESCHER_SYS_MAX_STRING_LEN], const i_t p_x, const i_t p_y )
{

  ev3_lcd_draw_string(p_string, p_x, p_y);

}
示例#5
0
static mrb_value
mrb_mruby_lcd_print(mrb_state *mrb, mrb_value self)
{
	mrb_value s;
	char *msg;
	mrb_get_args(mrb, "S", &s);
	msg = mrb_str_to_cstr(mrb, s);
	ev3_lcd_fill_rect(0, 0, EV3_LCD_WIDTH, EV3_LCD_HEIGHT, EV3_LCD_WHITE);
	ev3_lcd_draw_string(msg, 0, CALIB_FONT_HEIGHT*1);
	return mrb_true_value();
}
示例#6
0
/**
 * ジャイロセンサのオフセット値をキャリブレーションする
 * <戻り値>
 *    false: キャリブレーション未完了
 *    true : キャリブレーション完了
 */
bool Calibration::calibrateGyro(bool startTrigger) {

    int16_t  sensor;
    int16_t  cal;
    bool finish;
    char buf[256];

    finish = false;
    sensor = mGyroSensor.getAnglerVelocity();

    if (mIsStartedGyro == false) {

        sprintf( buf, "gyro = %03d", sensor);           // ジャイロセンサ値を表示
        ev3_lcd_draw_string( buf, 0, 10);

        if (startTrigger == true) {

            mIsStartedGyro = true;
            mSum = 0;
            mCalCount = 0;
        }
    }
    else {

        mSum += sensor;                                 // ジャイロセンサ値を積算
        mCalCount++;

        if (mCalCount == NUM_OF_GYRO_CALIBRATION) {     // 規定回数以上積算

            cal = (1);                                  // 平均値 ★キャリブレーション演習修正箇所
            mGyroSensor.setOffset(cal);

            sprintf( buf, "gyroOffset = %03d", cal);    // ジャイロオフセット値を表示
            ev3_lcd_draw_string( buf, 0, 10);

            finish = true;                              // 次へ
        }
    }
    return finish;
}
示例#7
0
/**
 * 白色の閾値をキャリブレーションする
 * <戻り値>
 *    false: キャリブレーション未完了
 *    true : キャリブレーション完了
 */
bool Calibration::calibrateWhite(bool startTrigger) {

    int8_t  sensor;
    int16_t  cal;
    bool finish;
    char buf[256];

    finish = false;
    sensor = mColorSensor.getBrightness();

    if (mIsStartedWhite == false) {

        sprintf( buf, "white = %03d", sensor);          // 光センサ値を表示
        ev3_lcd_draw_string( buf, 0, 30);

        if (startTrigger == true) {

            mIsStartedWhite = true;
            mSum = 0;
            mCalCount = 0;
        }
    }
    else {

        mSum += sensor;                                 // 光センサ値を積算
        mCalCount++;

        if (mCalCount == NUM_OF_WHITE_CALIBRATION) {    // 規定回数以上積算

            cal = (1);                                  // 平均値 ★キャリブレーション演習修正箇所
            mLineMonitor->setWhiteThreshold(cal);

            sprintf( buf, "whiteTh = %03d", cal);       // 白しきい値を表示
            ev3_lcd_draw_string( buf, 0, 30);

            finish = true;                              // 次へ
        }
    }
    return finish;
}
int16_t RayReflectAdmin_ohs::getClrCvtBright( void )
{
	rgb_raw_t RgbRaw;
	memset( &RgbRaw, 0, sizeof(RgbRaw));

	mColorSensor.getRawColor( RgbRaw );

    /* LCD画面表示 */
#ifdef PRINT
    SCHR   cString[50];
    memset( cString , 0, sizeof(cString));

	sprintf(( char* )cString, "Red_Value[%5d]",RgbRaw.r);
	ev3_lcd_draw_string( cString, 0, 8*4);
	sprintf(( char* )cString, "Red_Value[%5d]",RgbRaw.g);
	ev3_lcd_draw_string( cString, 0, 8*5);
	sprintf(( char* )cString, "Red_Value[%5d]",RgbRaw.b);
	ev3_lcd_draw_string( cString, 0, 8*6);
#endif

	return (int16_t)((uint32_t)( RgbRaw.r + RgbRaw.g + RgbRaw.b )/3);
}
/* ---------------------------------------------------------------------------------------------- */
int16_t RayReflectAdmin_ohs::getClrCvtBright( void )
{
	rgb_raw_t RgbRaw;// RGB値取得用@ev3api.hより
	FLOT fRed    = 0.0F;// カラーセンサ計測値@赤
	FLOT fGre    = 0.0F;// カラーセンサ計測値@緑
	FLOT fBle    = 0.0F;// カラーセンサ計測値@青
	FLOT fBright = 0.0F;// 変換値

	memset( &RgbRaw, 0, sizeof( RgbRaw ));

	mColorSensor.getRawColor( RgbRaw );// RGB値の取得

	/* 反射値の変換 */
	fRed = ( FLOT )RgbRaw.r;
	fGre = ( FLOT )RgbRaw.g;
	fBle = ( FLOT )RgbRaw.b;
	
	//取得値の合成
	fBright = ( fRed * 0.0F ) + ( fGre * 0.5F )+ ( fBle * 0.5F );
	
	/* 出力範囲補正 */
	if( fBright < 0.0F ) { fBright = 0.0F; }// 最低値カット
	if( fBright > REY_MAX_REF ) { fBright = REY_MAX_REF; }// 上限値カット

#ifdef PRINT/* デバッグ@LCD表示 */
    SCHR   cString[50];
    memset( cString , 0, sizeof(cString));

	sprintf(( char* )cString, "Sam_Value[%3d]Red_Value[%3d]",(int)fBright,(int)RgbRaw.r);
	ev3_lcd_draw_string( cString, 0, 8*4);
	sprintf(( char* )cString, "Sam_State[%3d]Gre_Value[%3d]",(int)mState,(int)RgbRaw.g);
	ev3_lcd_draw_string( cString, 0, 8*5);
	sprintf(( char* )cString, "              Ble_Value[%3d]",(int)RgbRaw.b);
	ev3_lcd_draw_string( cString, 0, 8*6);
#endif

	return (int16_t)fBright;
}
//メソッド:void 競技走行する()
void CompetitionRunning::CompetitionRun(){

	float TAIL_ANGLE_DRIVE = 3;

	MotorDrive					motorDrive( m_pDeviceInterface );
	SectionDecisionController	sectionDecisionController( m_pDeviceInterface );
	RunningController			runningController( m_pDeviceInterface, m_pUIGet );

	while (m_nNowSection < 7) {

		ev3_lcd_draw_string("running_start", 0, 60);

		//しっぽをバランス走行用に制御
		motorDrive.TailMotorDrive(TAIL_ANGLE_DRIVE);

		//区間判断コントローラに現区間の番号をもらう
		m_nNowSection = sectionDecisionController.SectionIdentify(m_nNowSection);

		if(m_nNowSection == 7) break;

		//走行コントローラに投げる
		runningController.RunningExecute(m_nNowSection);

		if (m_pUIGet->UIGetter().Button == 'B')
			break;

		m_pDeviceInterface->m_pCClock->sleep(3); /* 4msec周期起動 */

	}

	ev3_lcd_draw_string("running_end", 0, 60);






}
void Tactics_TEST1(Tactics* tactics){

  char str[20];
  int i = 0;
  switch(tactics->current_step) {

  case 0:
    Line_tracer_set_forward(line_tracer, 20);
    Line_tracer_set_turn(line_tracer, 0);
    line_tracer->run_counter = (ev3_motor_get_counts(DRIVE_L) + ev3_motor_get_counts(DRIVE_R))/2;
    sprintf(str, "count:%d", line_tracer->run_counter);
    ev3_lcd_draw_string(str, 0, 40);
    if(line_tracer->run_counter < -300){
      line_tracer->run_counter = 0;
      tactics->current_step++;
    }
    break;

  case 1:
    Line_tracer_set_forward(line_tracer, 100);
    Line_tracer_set_turn(line_tracer, 50);
    line_tracer->run_counter = (ev3_motor_get_counts(DRIVE_L) + ev3_motor_get_counts(DRIVE_R))/2;
    if(line_tracer->run_counter < -3000){
      line_tracer->run_counter = 0;
      tactics->current_step++;
    }
    break;

  case 2:
    tactics->finish_this_tactics = yes;
    break;

  default:
    break;
  }

}
示例#12
0
static void
mrb_lcd_print_line(mrb_state *mrb, mrb_value lcd, mrb_value *str)
{
  mrb_lcd_t *plcd = (mrb_lcd_t*)DATA_PTR(lcd);
  const char *src = mrb_string_value_cstr(mrb, str);
  size_t len = strlen(src);
  char *buf = mrb_malloc(mrb, len+1);
  ev3_font_size *f = &_font_size[plcd->font];
  mrb_int cw;
  mrb_int csr = plcd->cx;
  char *dst;
  mrb_bool lf;

  mrb_assert(plcd->font == EV3_FONT_SMALL || plcd->font == EV3_FONT_MEDIUM);
  mrb_assert(f->w != 0 && f->h != 0);

  cw = plcd->width / f->w;

  memset(buf, 0, len+1);
  dst = buf;
  while (len--) {
    lf = FALSE;
    if (*src == '\n') {
      lf = TRUE;
      src++;
    }
    else {
      *dst++ = *src++;
      csr++;
      if (csr >= cw) {
        lf = TRUE;
      }
    }
    if (lf) {
      *dst = '\0';
      ev3_lcd_draw_string(
        buf,
        plcd->left + plcd->cx * f->w,
        plcd->top  + plcd->cy * f->h
      );

      /* line feed */
#ifdef EV3
      ev3_lcd_fill_rect(
        plcd->left  + csr * f->w,
        plcd->top   + plcd->cy * f->h,
        plcd->width - csr * f->w,
        f->h,
        plcd->color ? 0 : 1
      );
#else
      MRBEV3_PUTS("");
#endif
      plcd->cy = (plcd->cy + 1) % (plcd->height / f->h);
      plcd->cx = csr = 0;

      memset(buf, 0, strlen(src)+1);
      dst = buf;
    }
  }
  *dst = '\0';
  if (dst != buf) {
    ev3_lcd_draw_string(
      buf,
      plcd->left + plcd->cx * f->w,
      plcd->top  + plcd->cy * f->h
    );
  }

  mrb_free(mrb, buf);

  /* update cursor */
  plcd->cx = csr;
}
示例#13
0
/* メインタスク */
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();
}
示例#15
0
void LibSampleClass::draw() {
    static char buf[256];
    sprintf(buf, "Lib Member is 0x%08x.", member);
    ev3_lcd_draw_string(buf, 0, 64);
}
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();
}
示例#17
0
/* メインタスク */
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();
}
//メソッド:void 競技走行する()
void ExtraStageLookUp::ExtraRun()
{

	/* 停止部分 */
	int stopTime = m_pDeviceInterface->m_pCClock->now();
	DV dv_now;
	while(1){
		dv_now = m_pDeviceValueGet->DeviceValueGetter();
		if(m_pDeviceInterface->m_pCClock->now() - stopTime  < 1500){
			dv_now = m_pRunningCalculation->RunningCalculate(dv_now, 0);
			m_pMotorDrive->TailMotorDrive(3);
			ev3_lcd_draw_string("EXrun section0start", 0, 70);
		}
		else if(m_pDeviceInterface->m_pCClock->now() - stopTime  < 2000){
			dv_now.GYRO_OFFSET = -10;
			dv_now = m_pRunningCalculation->RunningCalculate(dv_now, 0);
			m_pMotorDrive->TailMotorDrive(80);
			ev3_lcd_draw_string("EXrun section0 < 3500", 0, 80);
		}
		else{
			break;
		}

		m_pMotorDrive->LRMotorDrive(dv_now.Lmotor_pwm, dv_now.Rmotor_pwm);
		m_pDeviceInterface->m_pCClock->sleep(3); /* 4msec周期起動 */
	}

	ev3_lcd_draw_string("EXrun fin", 0, 90);



	//
	float	fTailAngle = m_fTailAngleStand;	// しっぽ立ち上がり

	int		nLmotor = 20;
	int		nRmotor = 20;
//	int16_t	nDistance = 9999;

	m_uStartTime	= m_pDeviceInterface->m_pCClock->now();	// 開始時間

	//ログ出力
	char*	cLogBuff	= m_pUIGet->GetBlueT()->pcLogBuff;
	sprintf(cLogBuff,"ExtraStageLookUp::ExtraRun go 1: %lu,%d,%d,sizeof(int)=%d\n", GetElapsedTime(),
			nLmotor,
			nRmotor,
			sizeof(int)
			);
	m_pUIGet->WriteLog(cLogBuff);


//BBBBBBBBBBB
// テストデータ取得
//	Spin( 'N', 3600, fTailAngle );
//
//	PauseEt( 3000, fTailAngle);	// ポーズ
//
//	// 5000mm 進む
//	MoveDist( 'N', 5000, fTailAngle, nLmotor, nRmotor );
//	PauseEt(3000, fTailAngle);	// ポーズ
//BBBBBBBBBBB

//	PauseEt(500, fTailAngle);	// ポーズ

//	// 0 しっぽを出しながら止まる
//	MoveDist( 'N', 100, fTailAngle, 5 );

	// ① →

	//ゲートまで近づく
//	GoGate('R', fTailAngle);
	GoGate('N', fTailAngle);
	PauseEt(500, fTailAngle);	// ポーズ

	// ② →

	//走行体をリンボー
	fTailAngle	= Limbo( 'N', fTailAngle);
	PauseEt(500, fTailAngle);	// ポーズ

	// ゲートを潜る為、230mm 進む
	MoveDist( 'N', 230, fTailAngle, 10 );
	PauseEt(500, fTailAngle);	// ポーズ

	// ③ U
	// Uターン 180度回転
	Spin(185, fTailAngle);
	PauseEt(500, fTailAngle);	// ポーズ

	// 戻り
	// ③ ←
	// ゲートを潜る、330mm 戻る
	MoveDist( 'N', 330, fTailAngle, 10 );
	PauseEt(500, fTailAngle);	// ポーズ

	// ④ U
	// 再度Uターン -180度回転
	Spin(-185, fTailAngle);
	PauseEt(500, fTailAngle);	// ポーズ

	// ④ →
	// ゲートを潜る、360mm 進む
	MoveDist( 'N', 360, fTailAngle, 20 );


	// ⑤ →
	// トレースを可能とする為に上体を起こす
	fTailAngle = StandUpTail(fTailAngle);	// しっぽを立てる

	PauseEt(2222, fTailAngle);	// ポーズ

	PauseEt(2222, fTailAngle);	// ポーズ

//
//	// トレースを確実に行う為に故意に右側にはみ出す
//	Spin(-10, fTailAngle);		// -10度回転
//	PauseEt(500, fTailAngle);	// ポーズ
//	MoveDist( 'N', 40, fTailAngle, 10 );	// 3cm進む
//	PauseEt(500, fTailAngle);	// ポーズ

	// ⑥ →
	// ガレージまで走る!!
	// 距離 m_nFinalRunDist	// ■■<最終調整が必要>

	// トレース走行
//	MoveDist( 'R', m_nFinalRunDist, fTailAngle, 10 );
	MoveDist( 'N', m_nFinalRunDist, fTailAngle, 10 );

	MoveDist( 'N', 50, fTailAngle,  5 );	// 急停止すると倒れる場合があるので減速走行

	// ゲートイン
	fTailAngle = StandUpTailFine(fTailAngle);	// しっぽを思い切り立てる
	PauseEt(4000, fTailAngle);	// ポーズ

	// ゲートに届かなかった場合を考慮して、微速前進とポーズを繰り返す
	// ※ゲートイン判定が出た後は、ゲートにぶつかってもお構いなしなはず
	for( int i=0 ; i< 5 ; i++ ){
		MoveDist( 'N', 30, fTailAngle,  5 );	// ゲートまで再度トライ
		PauseEt(4000, fTailAngle);	// ポーズ
	}
	MoveDist( 'N', 30, fTailAngle,  5 );	// ゲートまで再度トライ

//	Ending();

	PauseEt(5000, fTailAngle);	// ポーズ

}
示例#19
0
 void testMethod() {
     static char buf[256];
     sprintf(buf, "Member is 0x%08x.", member);
     ev3_lcd_draw_string(buf, 0, 32);
 }