2015_robocon_bteam / Mbed 2 deprecated 2015robot_main_zenkoku

2015 全国ロボコン　福島B　プログラム

Dependencies:   PID QEI mbed

輪花繚乱　福島Ｂチームのプログラム

お断り

  • 糞みたいなコードだけど生暖かい目で見守ってください
  • 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) {
    }
}