//構造体変数の更新、回転角度の計算 float turn_angle_detection(turn_angle_structure *tas){ //rotation angle of right wheel tas->old_angle = tas->current_angle; tas->right_motor_old_angle = tas->right_motor_current_angle; tas->right_motor_current_angle = ev3_motor_get_counts(DRIVE_R) - tas->original_right_motor_angle; tas->right_motor_rotation_angle = tas->right_motor_current_angle - tas->right_motor_old_angle; //rotation angle of left wheel tas->left_motor_old_angle = tas->left_motor_current_angle; tas->left_motor_current_angle = ev3_motor_get_counts(DRIVE_L) - tas->original_left_motor_angle; tas->left_motor_rotation_angle = tas->left_motor_current_angle - tas->left_motor_old_angle; //moving distance and turning angle tas->turning_angle = tas->wheel_across/2 * (tas->right_motor_rotation_angle - tas->left_motor_rotation_angle) / tas->between_wheels; //updating of coordinates tas->current_angle = tas->old_angle + tas->turning_angle; return tas->current_angle; }
static mrb_value mrb_mruby_motor_getCount(mrb_state *mrb, mrb_value self) { motor_port_t port = mrb_fixnum(mrb_iv_get(mrb, self, mrb_intern_lit(mrb, "@port"))); mrb_int val = ev3_motor_get_counts(port); return mrb_fixnum_value(val); }
//回転角度検知用の構造体作成 turn_angle_structure *turn_angle_structure_make(void) { turn_angle_structure *new_turn_angle_structure = (turn_angle_structure*)malloc(sizeof(turn_angle_structure)); new_turn_angle_structure->right_motor_current_angle = ev3_motor_get_counts(DRIVE_R); new_turn_angle_structure->left_motor_current_angle = ev3_motor_get_counts(DRIVE_L); new_turn_angle_structure->right_motor_old_angle = 0; new_turn_angle_structure->left_motor_old_angle = 0; new_turn_angle_structure->right_motor_rotation_angle = 0; new_turn_angle_structure->left_motor_rotation_angle = 0; new_turn_angle_structure->wheel_across = 8.0; new_turn_angle_structure->between_wheels = 12.0; new_turn_angle_structure->moving_distance = 0; new_turn_angle_structure->turning_angle = 0; new_turn_angle_structure->old_angle = 0; new_turn_angle_structure->current_angle = 0; new_turn_angle_structure->original_right_motor_angle = 0; new_turn_angle_structure->original_left_motor_angle = 0; return new_turn_angle_structure; }
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; } }
//尻尾制御 static void tail_control(signed int angle) { float pwm = (float)(angle - ev3_motor_get_counts(tail_motor))*P_GAIN; /* PWM出力飽和処理 */ if (pwm > PWM_ABS_MAX) { pwm = PWM_ABS_MAX; } else if (pwm < -PWM_ABS_MAX) { pwm = -PWM_ABS_MAX; } if (pwm == 0) { ev3_motor_stop(tail_motor, true); } else { ev3_motor_set_power(tail_motor, (signed char)pwm); } }
/* メインタスク */ 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(); }
/* * 駆動タスク * 機能: 設定されたスピードとステアリング角になるようにモータを制御する */ void nxtrike_drive_task(intptr_t exinf) { int steer_degree, steer_speed, current_steer_degree; int left_speed, right_speed; float ratio; static int prev_steer_speed = 0; static int prev_left_speed = 0; static int prev_right_speed = 0; while(1){ /* 現在のステアリング角度の取得 */ current_steer_degree = ev3_motor_get_counts(STEER_MOTOR) / 8; /* ステアリング操作速度の設定 */ #if 0 if((nxtrike_steer_degree - current_steer_degree) > 0){ steer_speed = NXTRIKE_STEER_SPEED_MAX; }else{ steer_speed = -NXTRIKE_STEER_SPEED_MAX; } #endif /* ステアリングの回転速度は目標値と現在値の差の3倍 */ steer_speed = (nxtrike_steer_degree - current_steer_degree) * 3; if(steer_speed > 0){ if(steer_speed > NXTRIKE_STEER_SPEED_MAX){ steer_speed = NXTRIKE_STEER_SPEED_MAX; }else if(steer_speed < NXTRIKE_STEER_SPEED_MIN){ steer_speed = NXTRIKE_STEER_SPEED_MIN; } }else if(steer_speed < 0){ if(steer_speed < -NXTRIKE_STEER_SPEED_MAX){ steer_speed = -NXTRIKE_STEER_SPEED_MAX; }else if(steer_speed > -NXTRIKE_STEER_SPEED_MIN){ steer_speed = -NXTRIKE_STEER_SPEED_MIN; } }else{ steer_speed = 0; } /* 後輪速度計算 */ ratio = current_steer_degree * current_steer_degree; if (nxtrike_steer_degree > 0) { left_speed = nxtrike_base_speed * (1 + 0.00012 * ratio); right_speed = nxtrike_base_speed / (1 + 0.00032 * ratio); } else { left_speed = nxtrike_base_speed / (1 + 0.00032 * ratio); right_speed = nxtrike_base_speed * (1 + 0.00012 * ratio); } /* モータ制御 */ if (steer_speed != 0) ev3_motor_set_power(STEER_MOTOR, steer_speed); else ev3_motor_stop(STEER_MOTOR, true); if (left_speed != 0) ev3_motor_set_power(LEFT_MOTOR, -left_speed); else ev3_motor_stop(LEFT_MOTOR, true); if (right_speed != 0) ev3_motor_set_power(RIGHT_MOTOR, -right_speed); else ev3_motor_stop(RIGHT_MOTOR, true); /* 現在の値を保存 */ prev_steer_speed = steer_speed; prev_left_speed = left_speed; prev_right_speed = right_speed; //syslog(LOG_NOTICE, "l = %d, r = %d", left_speed, right_speed); /* タスクを休止 */ slp_tsk(); } ext_tsk(); }
int nxtrike_get_motor_rot(U32 motor_id) { return -ev3_motor_get_counts(motor_id); }
/* メインタスク */ 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(); }