2015_robocon_bteam
2015 全国ロボコン 福島B プログラム
輪花繚乱 福島Ｂチームのプログラム
お断り
- 糞みたいなコードだけど生暖かい目で見守ってください
- mbed初心者ゆえコミットの機能を使いこなせていないがご愛嬌
- 謎のコメント、意味のない変数等満載だけど、これもご愛嬌
- ライブラリに頼ってばっかなので実質たいしたことやってないです(;´･ω･)
概要
- 1.オドメトリでマシンの位置情報を入手
- 2.入手したマシン位置情報に応じてマシンに命令を送る
- ～以後繰り返す～
- おしまい
マシンの自己位置計算部（自明な宣言は省略）
#include "mbed.h"
#define RATE 0.01
#define PI 3.14159265359
#define ppr 400.0 //Pulse per revolution(encoder)
QEI Move_r_sense(ENCOD_MOVE_R_A, ENCOD_MOVE_R_B, NC, 400, QEI::X4_ENCODING);
QEI Move_l_sense(ENCOD_MOVE_L_A, ENCOD_MOVE_L_B, NC, 400, QEI::X4_ENCODING);
Ticker Com;
void mesure_state()
{
Pulses_move_r = ( double )Move_r_sense.getPulses();
Pulses_move_l = ( double )Move_l_sense.getPulses();
dlr = ( ( ( Pulses_move_r - PrefPulses_move_r ) / ppr ) * 2.0 * PI ) * r_wheel;
dll = ( ( ( Pulses_move_l - PrefPulses_move_l ) / ppr ) * 2.0 * PI ) * r_wheel;
PrefPulses_move_r = Pulses_move_r;
PrefPulses_move_l = Pulses_move_l;
vr = dlr / RATE;
vl = dll / RATE;
if(flagf) velocity = ( vr + vl ) / 2.0;
else velocity = -( vr + vl ) / 2.0;
dsita = ( dlr - dll ) / ( 2.0 * d );
dx = ( ( dlr + dll ) / 2.0 ) * cos( sita + dsita / 2.0 );
dy = ( ( dlr + dll ) / 2.0 ) * sin( sita + dsita / 2.0 );
sita += dsita;
x += dx;
y += dy;
}
int main() {
Com.attach(&mesure_state,RATE);
while(1) {
}
}