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クラスのメソッドでモーターを動かす
//今はまだ無いので,
}
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));
}
/*
* 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();
}
/*
* 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);
}
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();
}
/**
* ジャイロセンサのオフセット値をキャリブレーションする
* ＜戻り値＞
* 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;
}
/**
* 白色の閾値をキャリブレーションする
* ＜戻り値＞
* 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;
}
}
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;
}
/* メインタスク */
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 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();
}
/* メインタスク */
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); // ポーズ
// // ０ しっぽを出しながら止まる
// 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); // ポーズ
// ③ Ｕ
// Ｕターン 180度回転
Spin(185, fTailAngle);
PauseEt(500, fTailAngle); // ポーズ
// 戻り
// ③ ←
// ゲートを潜る、330mm 戻る
MoveDist( 'N', 330, fTailAngle, 10 );
PauseEt(500, fTailAngle); // ポーズ
// ④ Ｕ
// 再度Ｕターン -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); // ポーズ
}
void testMethod() {
static char buf[256];
sprintf(buf, "Member is 0x%08x.", member);
ev3_lcd_draw_string(buf, 0, 32);
}
