あえて賢かれ

自分用の備忘録

関数でスラロームを実装する

マウス合宿でブログを書くように煽られてしまったので,書きました...

関数でスラロームを実装しようって話です.

なぜ関数で実装する必要があるかというと,スラロームなどをするとパラメータの調整が必須でいちいち加速テーブルなんて作っていられないっとおもうからだと思います.(Miceでは加速テーブルをつかわないので,よくわかってません.)

大回りと斜めの走行も考えると,左右で同じパラメータを使ったとしても

①普通の90度スラローム

②大回り90

③大回り180

④45度入

⑤45度出

⑥135度入

⑦135度出

⑧v90

とたくさんあります.

すべてを調整する必要があるので,まあ地獄.



f:id:staytus:20190709065327p:plain
昨年度作ったターン用の関数群
ターンについて詳しくは,けりさんのブログで.


kerikeri.top


とまあこんな感じでターンはたくさんあるので,角度,重心速度,角加速度,最高(最低)角速度を引数として持つスラローム用の関数を作成しようってなります.

 

スラロームを詳しく知りたい方は以下をご参照.


tenpura.sugo-roku.com



重心速度をvとして,右タイヤの速度をv+Δv,左の速度をv-Δvとすれば,前に進みながら左に旋回しますよね.トレッドの半分をrとすれば,この時の角速度はΔv=rωの関係から求まります.速度を連続的に変化させないと脱調などの問題となるので,Δv(すなわちω)を台形加減速すればいいって話です.

 メイン関数でやるべきことは,

①割り込み許可

②目標角度に達するまで待機

制御周期ごと割り込ませる関数でやるべきことは

①角速度ωを計算する

②角度θを計算する

③右のタイヤの速度をv+rω,左のタイヤの速度をv-rωとする

です.

 
実装のイメージ

//制御周期ごとに割り込ませる関数

void AccelerateMotor_Turn(void) {
	static uint32_t counter = 0;
	static uint16_t process = 0;
	static float 減速開始距離 = 0; 

//条件判断

	if (左旋回の場合) {

		if (現在角 >= 目標角) { 
				process = 終了;

		} else if (counter == 0) {
			process = 初期化;
			counter++;
		} else if (現在角 < 目標角 / 2) {
			if (現在角速度 <最大角速度) {
				process = 加速;
			} else {
				if (process == 加速) {
					減速開始角度 = 目標角 - 現在角;
				}
				process = 維持;
				counter++;
			}
		} else {
			if (現在角 >= 減速開始角度) {
				process = 減速;
				counter++;
			} else {
				process = 維持;
				counter++;
			}
		}

	} else {//右旋回の場合

		if (現在角<= 目標角) {
				process = 終了;
				counter = 0;

		} else if (counter == 0) {
			process = 初期化;
			counter++;

		} else if (現在角 > 目標角 / 2) {
			if (現在角速度 >目標角速度) {
				process = 加速;
				counter++;
			} else {
				if (process == 加速) {
					減速開始角度 = 目標角 - 現在角;
				}
				process = 維持;
				counter++;
			}
		} else {
			if (現在角 <= 減速開始角度) {
				process = 減速;
				counter++;

			} else {
				process = 維持;
				counter++;
			}
		}

	}

//条件に基づく動作

	switch (process) {
	case 初期化:
		減速開始角度=0;
		現在角=0;
		現在角速度=初角速度;
		//速度変更
		Change_Duty(LEFT,
				calculate_Duty_L(重心速度, 現在角速度));
		Change_Duty(RIGHT,
				calculate_Duty_R(重心速度, 現在角速度));
		//角度計算
		現在角 += 現在角速度 * 制御周期;	
		break;
	case 加速:
		//角速度計算
		tar.omega += 角加速度 * 制御周期;
		//速度変更
		Change_Duty(LEFT,
				calculate_Duty_L(重心速度, 現在角速度));
		Change_Duty(RIGHT,
				calculate_Duty_R(重心速度, 現在角速度));
		//角度計算
		現在角 += 現在角速度 * 制御周期;
		break;
	case 減速:
		//角速度計算
		現在角 -= 角加速度 * 制御周期;
		//速度変更
		Change_Duty(LEFT,
				calculate_Duty_L(重心速度, 現在角速度));
		Change_Duty(RIGHT,
				calculate_Duty_R(重心速度, 現在角速度));
		//角度計算
		現在角 += 現在角速度 * 制御周期;

		break;
	case 維持:
		//角度計算
		現在角 += 現在角速度 * 制御周期;
		break;
	case 終了:
		counter = 0;
		process = 0;

		//割り込みでこの関数が呼ばれないようにする
		g_mode_it == STANDBY;
		break;
	default:
		break;
	}

}

//メイン関数で使う関数

void Turn(float 目標角, float 初角速度,
		float 角加速度, float 重心速度, float 最大角速度) {
	//AccelerateMotor_Turnで使う変数(目標角,初角速度,角加速度,重心速度,最大角速度)を変更

	//重心速度は正で,それ以外については左旋回の場合は正,右旋回の場合は負

	・・・

	//割り込みでAccelerateMotor_Turnを呼び出すようにする
	g_mode_it = TURN;
	
	while (g_mode_it == TURN);

}




僕はこんな感じの実装をしています.重心速度が一定であれば,台形加減速のプログラムより簡単です.
他人のプログラムを読んだことがほぼないので,もしかしたらガラパゴス化しているかもしれません.
何かまずそうなことやもっとここを説明しろと思うことがあればご連絡ください.