File content as of revision 5:b3bbf96356cf:

/* ------------------------------------------------------------------- */
/* NHKロボコン2018-Bチーム自動ロボット(設計者: 4S 関) */
/* ------------------------------------------------------------------- */
/* このプログラムは上記のロボット専用の制御プログラムである。 */
/* ------------------------------------------------------------------- */
/* 対応機種: NUCLEO-F446RE */
/* ------------------------------------------------------------------- */
/* 製作者: 4D 高久 雄飛, mail: rab1sy23@gmail.com */
/* ------------------------------------------------------------------- */
/* 使用センサ: リミットスイッチ1個, ロータリーエンコーダ: 7個, 超音波センサ: 1個, フォトインタラプタ: 1個 */
/* 他にラインセンサ基板のPIC(16F1938)と通信をしてライントレースをさせている */
/* ------------------------------------------------------------------- */
#include "mbed.h"
#include "math.h"
#include "QEI.h"
#include "PID.h"
#include "hcsr04.h"
//Pi
#define PI  3.14159265359
//PIDGain of rollers
#define roller_Kp 1.5
#define roller_Ki 0.1

InterruptIn shoot(PC_13);
HCSR04 sonic(PB_3, PB_10);

//前ローラー
PID front_roller(roller_Kp, roller_Ki, 0.0, 0.001);
//後ローラー
PID back_roller(roller_Kp, roller_Ki, 0.0, 0.001);
//MDとの通信ポート
I2C i2c(PB_9, PB_8);  //SDA, SCL
//PCとの通信ポート
Serial pc(USBTX, USBRX);    //TX, RX
//12V停止信号ピン
DigitalOut emergency(PA_13);
//前ローラー
QEI front_roller_wheel(PA_0, PA_1, NC, 624);
//後ローラー
QEI back_roller_wheel(PB_5, PB_4, NC, 624);
Ticker get_rps;

int front_roller_rpm;
int back_roller_rpm;

int front_roller_pulse[10];
int back_roller_pulse[10];

int sum_front_roller_pulse;
int sum_back_roller_pulse;

int ave_front_roller_pulse;
int ave_back_roller_pulse;

char send_data[1];

char front_roller_data[1];
char back_roller_data[1];
char cylinder_data[0];

static int i = 0;
//各関数のプロトタイプ宣言
void shot();
void ultrasonic();
//回転数取得関数
void flip();
//ローラーPID
int roller_PID(int front, int back);

//移動平均をPID
void flip() {
    //前回のi番目のデータを消して新たにそこにデータを書き込む(キューのような考え?)
    sum_front_roller_pulse -= front_roller_pulse[i];
    sum_back_roller_pulse  -= back_roller_pulse[i];

    //配列のi番目の箱に取得パルスを代入
    front_roller_pulse[i] = front_roller_wheel.getPulses();
    back_roller_pulse[i]  = back_roller_wheel.getPulses();

    front_roller_wheel.reset();
    back_roller_wheel.reset();

    //i[0]~i[9]までの合計値を代入
    sum_front_roller_pulse += front_roller_pulse[i];
    sum_back_roller_pulse  += back_roller_pulse[i];

    //インクリメント
    i++;
    //iが最大値(9)になったらリセット
    if(i > 9) {
        i = 0;
    }
    //10回分の合計値を10で割り、取得パルスの平均を出す
    ave_front_roller_pulse = sum_front_roller_pulse / 10;
    ave_back_roller_pulse  = sum_back_roller_pulse  / 10;

    //平均値をRPMへ変換
    front_roller_rpm = ave_front_roller_pulse * 25 * 60 / 50;
    back_roller_rpm  = ave_back_roller_pulse  * 25 * 60 / 1200;
    pc.printf("%d %d\n\r", abs(front_roller_rpm), abs(back_roller_rpm));
}
//ローラー
int roller_PID(int front, int back) {
        front_roller.setInputLimits(-2000, 2000);
        back_roller.setInputLimits(-2000, 2000);

        front_roller.setOutputLimits(0x84, 0xFF);
        back_roller.setOutputLimits(0x84, 0xFF);

        front_roller.setMode(AUTO_MODE);
        back_roller.setMode(AUTO_MODE);

        front_roller.setSetPoint(front);
        back_roller.setSetPoint(back);

        front_roller.setProcessValue(abs(front_roller_rpm));
        back_roller.setProcessValue(abs(back_roller_rpm));

        front_roller_data[0] = front_roller.compute();
        back_roller_data[0]  = back_roller.compute();

        return 0;
}
int main(void) {
    //緊急停止用信号ピンをLow
    emergency = 0;
    //回転数取得関数のタイマ割り込み(40ms)
    get_rps.attach_us(&flip, 40000);

    //各MD, エアシリ制御基板へ起動後0.1秒間0x80を送りつける(通信切断防止用)
    send_data[0] = 0x80;
    i2c.write(0x16, send_data, 1);
    i2c.write(0x22, send_data, 1);
    i2c.write(0x40, send_data, 1);
    
    shoot.fall(shot);
    wait(0.1);

    while(1) {
        ultrasonic();
        //ローラーぐるぐる
        roller_PID(200, 200);
        i2c.write(0x20, front_roller_data, 1, false);
        i2c.write(0x22, back_roller_data,  1, false);
        wait_us(20);
    }
}

void shot(){
    cylinder_data[0] = 0x0F;
    i2c.write(0x40, cylinder_data, 1);
    wait(1);
    
    cylinder_data[0] = 0x80;
    i2c.write(0x40, cylinder_data, 1);
    wait(0.5);

}

//超音波センサ用関数
void ultrasonic() {
        //超音波発射
        sonic.start();
        //10ms待機
        wait(0.01);
        //get_dist_cm関数は、計測から得られた距離(cm)を返します。
        int distance = sonic.get_dist_cm();
        //pc.printf("%d[cm]\r\n", distance);
}