void btConnect() { while(true) { if (is_master) { shared_memory->bt_con = fdopen(SIO_PORT_SPP_MASTER_TEST_FILENO, "a+"); if (shared_memory->bt_con != NULL) { setbuf(shared_memory->bt_con, NULL); while (!isConnected()) { //cycle_print((char*)"Connecting..."); spp_master_test_connect(slave_address, pin); sleep(1000); } break; } } else { while (!ev3_bluetooth_is_connected()) { //cycle_print((char*)"Waiting for connection..."); sleep(1000); } shared_memory->bt_con = ev3_serial_open_file(EV3_SERIAL_BT); break; } sleep(1000); } //cycle_print((char*)"Connected."); act_tsk(BT_RECV_TASK); }
/* * call-seq: * SerialPort.new(port) # => SerialPort * * Open serial port. * * Parameters: * +port+ font size. * :default Default serial port * :uart UART * :bt Bluetooth * * Returns SerialPort object. */ static mrb_value mrb_serial_init(mrb_state *mrb, mrb_value self) { struct RClass *ser = mrb_obj_class(mrb, self); mrb_value pmap = mrb_const_get(mrb, mrb_obj_value(ser), mrb_intern_lit(mrb, "PORT")); mrb_sym port; mrb_value portv; FILE *fp; mrb_get_args(mrb, "n", &port); portv = mrb_hash_get(mrb, pmap, mrb_symbol_value(port)); if (mrb_nil_p(portv)) { mrb_raisef(mrb, E_ARGUMENT_ERROR, "invalid port :%S", mrb_sym2str(mrb, port)); } fp = ev3_serial_open_file((serial_port_t)mrb_fixnum(portv)); if (fp == NULL) { mrb_raisef(mrb, E_IO_ERROR, "Serial port cannot open :%S", mrb_sym2str(mrb, port)); } mrb_file_attach(self, fp); return self; }
void main_task(intptr_t unused) { bt = ev3_serial_open_file(EV3_SERIAL_BT); int heartBeatCount = 0; Linetrace* linetrace; Temp::init(); tailControl->SetRefValue(90); wheelControl->Init(); while(1) { if(1250 < heartBeatCount) { linetrace->exec(); } tailControl->Control(); wheelControl->Control(); if(heartBeatCount%250 == 0) { ev3_led_set_color(LED_ORANGE); } else if((heartBeatCount+125)%250 == 0) { ev3_led_set_color(LED_GREEN); } heartBeatCount++; if(heartBeatCount == 1250) { tailControl->SetRefValue(0); linetrace = new Linetrace(wheelControl, calibration); } tslp_tsk(4); } }
void main_task(intptr_t unused) { ev3_lcd_set_font(EV3_FONT_MEDIUM); ev3_font_get_size(EV3_FONT_MEDIUM, &fontw, &fonth); memfile_t memfile; ev3_memfile_load("/test.bmp", &memfile); image_t image; ev3_image_load(&memfile, &image); ev3_lcd_draw_image(&image, 0, fonth * 2); //ev3_sta_cyc(TEST_EV3_CYC1); // Enable TEST_EV3_CYC2 for 5 seconds ev3_sta_cyc(TEST_EV3_CYC2); tslp_tsk(5000); ev3_stp_cyc(TEST_EV3_CYC2); #if 0 // Register button handlers ev3_button_set_on_clicked(BACK_BUTTON, button_clicked_handler, BACK_BUTTON); ev3_button_set_on_clicked(ENTER_BUTTON, button_clicked_handler, ENTER_BUTTON); ev3_button_set_on_clicked(LEFT_BUTTON, button_clicked_handler, LEFT_BUTTON); // Configure sensors ev3_sensor_config(gyro_sensor, GYRO_SENSOR); // Configure motors ev3_motor_config(left_motor, LARGE_MOTOR); ev3_motor_config(right_motor, LARGE_MOTOR); // Start task for self-balancing act_tsk(BALANCE_TASK); // Open Bluetooth file bt = ev3_serial_open_file(EV3_SERIAL_BT); assert(bt != NULL); // Start task for printing message while idle act_tsk(IDLE_TASK); while(1) { uint8_t c = fgetc(bt); sus_tsk(IDLE_TASK); switch(c) { case 'w': if(motor_control_drive < 0) motor_control_drive = 0; else motor_control_drive += 10; fprintf(bt, "motor_control_drive: %d\n", motor_control_drive); break; case 's': if(motor_control_drive > 0) motor_control_drive = 0; else motor_control_drive -= 10; fprintf(bt, "motor_control_drive: %d\n", motor_control_drive); break; case 'a': if(motor_control_steer < 0) motor_control_steer = 0; else motor_control_steer += 10; fprintf(bt, "motor_control_steer: %d\n", motor_control_steer); break; case 'd': if(motor_control_steer > 0) motor_control_steer = 0; else motor_control_steer -= 10; fprintf(bt, "motor_control_steer: %d\n", motor_control_steer); break; case 'h': fprintf(bt, "==========================\n"); fprintf(bt, "Usage:\n"); fprintf(bt, "Press 'w' to speed up\n"); fprintf(bt, "Press 's' to speed down\n"); fprintf(bt, "Press 'a' to turn left\n"); fprintf(bt, "Press 'd' to turn right\n"); fprintf(bt, "Press 'i' for idle task\n"); fprintf(bt, "Press 'h' for this message\n"); fprintf(bt, "==========================\n"); break; case 'i': fprintf(bt, "Idle task started.\n"); rsm_tsk(IDLE_TASK); break; default: fprintf(bt, "Unknown key '%c' pressed.\n", c); } } #endif }
/* メインタスク */ 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) { 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(); }