void main_task(intptr_t unused) {
signed char forward; /* 前後進命令 */
// signed char turn; /* 旋回命令 */
float turn; /* 旋回命令 */
signed char pwm_L, pwm_R; /* 左右モータPWM出力 */
/* Open Bluetooth file */
bt = ev3_serial_open_file(EV3_SERIAL_BT);
fprintf(bt, "main_task start\n");
// assert(bt != NULL);
/* Bluetooth通信タスクの起動 */
act_tsk(BT_TASK);
/* センサー入力ポートの設定 */
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);
/* スタート待機 */
while(1)
{
tail_control(TAIL_ANGLE_STAND_UP); /* 完全停止用角度に制御 */
if (bt_cmd == 1)
{
bt_cmd = 0;
break; /* リモートスタート */
}
if (bt_cmd == 2)
{
ev3_led_set_color(LED_ORANGE);
}
if (bt_cmd == 3)
{
ev3_led_set_color(LED_GREEN);
}
if (ev3_touch_sensor_is_pressed(touch_sensor) == 1)
{
break; /* タッチセンサが押された */
}
tslp_tsk(10); /* 10msecウェイト */
}
fprintf(bt, "start dash\n");
// 滑らかスタートお試し
int i;
i = 0;
for(i = 0; i < 15 ; i++)
{
tail_control(TAIL_ANGLE_STAND_UP + i);
tslp_tsk(5); // 5msecウェイト
}
/*
while(1)
{
tail_control(TAIL_ANGLE_DRIVE);
tslp_tsk(4);
}
*/
// 走行モーターエンコーダーリセット
ev3_motor_reset_counts(left_motor);
ev3_motor_reset_counts(right_motor);
// ジャイロセンサーリセット
ev3_gyro_sensor_reset(gyro_sensor);
balancer.init(GYRO_OFFSET);
// pid制御リセット
pid Pid;
while(1)
{
int32_t motor_ang_l, motor_ang_r;
int gyro, volt;
int16_t sensor_val;
int16_t target_val;
// バックボタンが押されたら終了
if (ev3_button_is_pressed(BACK_BUTTON)) break;
// タッチセンサーが押されたら終了
if (ev3_touch_sensor_is_pressed(touch_sensor) == 1) break;
// バランス走行用角度に制御
tail_control(TAIL_ANGLE_DRIVE);
// 障害物を検知したら停止
if (sonar_alert() == 1)
{
// 障害物を検知したら停止
forward = turn = 0;
}
else
{
// 前進命令
forward = 40;
}
sensor_val = ev3_color_sensor_get_reflect(color_sensor);
target_val = 20;
if(1){
float p, i, d;
p = 0;
i = 0;
d = 0;
diff[0] = diff[1];
diff[1] = sensor_val - target_val; //偏差を取得
integral += (diff[1] + diff[0]) / 2.0 * DELTA_T;
p = KP * diff[1];
i = KI * integral;
d = KD * (diff[1] - diff[0]) / DELTA_T;
if(p + i+ d < -100)
{
turn = -100;
}
else if(p + i + d > 100)
{
turn = 100;
}
else
{
turn = p + i + d;
}
}
// turn = Pid.getTurnVal(sensor_val, target_val);
fprintf(bt, "sensor_val:%d target_val:%d TurnVal:%f \n", sensor_val, target_val, turn);
// 倒立振子制御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();
fprintf(bt, "motor_ang_l:%d motor_ang_r:%d gyro:%d volt:%d\n", motor_ang_l, motor_ang_r, gyro, volt);
// 倒立振子制御APIを呼び出し、倒立走行するための
// 左右モータ出力値を得る
fprintf(bt, "balancer.setCommand: %d : %d\n", forward, turn);
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>
fprintf(bt, "balancer: pwm_L=%d : pwm_R=%d\n", pwm_L, pwm_R);
// 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);
}
// 4msec周期起動
tslp_tsk(4);
}
// モーター停止
ev3_motor_stop(left_motor, false);
ev3_motor_stop(right_motor, false);
// BlueToothタスクを停止
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出力 */
/* 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();
}