/** * バランス走行する */ void BalancingWalker::run() { int16_t angle = mGyroSensor.getAnglerVelocity(); // ジャイロセンサ値 int rightWheelEnc = mRightWheel.getCount(); // 右モータ回転角度 int leftWheelEnc = mLeftWheel.getCount(); // 左モータ回転角度 mBalancer->setCommand(mForward, mTurn); int battery = ev3_battery_voltage_mV(); mBalancer->update(angle, rightWheelEnc, leftWheelEnc, battery); // 左右モータに回転を指示する if (detectFall() == false) { mLeftWheel.setPWM(mBalancer->getPwmLeft()); mRightWheel.setPWM(mBalancer->getPwmRight()); } else { mLeftWheel.setPWM(0); mRightWheel.setPWM(0); } }
/* * Bridge: get_battery_voltage */ i_t EV3B_get_battery_voltage() { return ev3_battery_voltage_mV(); }
/* メインタスク */ 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(); }
/* * call-seq: * Battery.mV # => Fixnum * * Get battery battery voltage. * * Returns battery voltage in mV */ static mrb_value mrb_battery_mV(mrb_state *mrb, mrb_value self) { return mrb_fixnum_value(ev3_battery_voltage_mV()); }
/* メインタスク */ 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(); }
int Battery::getValue(){ return ev3_battery_voltage_mV(); }
int CwrBattery::GetmV() // バッテリの電圧を取得する.(mV) { return ev3_battery_voltage_mV(); }