File content as of revision 2:235db39e0eaa:

/* ------------------------------------------------------------------- */
/* NHKロボコン2018-Bチーム手動ロボット(設計者: 4S 関) */
/* ------------------------------------------------------------------- */
/* このプログラムは上記のロボット専用の制御プログラムである。 */
/* ------------------------------------------------------------------- */
/* 対応機種: NUCLEO-F446RE */
/* ------------------------------------------------------------------- */
/* 製作者: 1-3 武井 智大, mail: taobmcoe@outlook.com */
/* ------------------------------------------------------------------- */
/* 使用センサ:ロータリーエンコーダ: 2個, 超音波センサ: 1個*/
/* ------------------------------------------------------------------- */
#include "mbed.h"
#include "hcsr04.h"
#include "QEI.h"
#include "PID.h"

#define ps3data ps3s.condata
#define PI 3.14159265359
#define roller_Kp 4.0
#define froller_Ki 0.1
#define broller_Ki 0.1
#define roller_Kd 0.05

//定義

/*I2C定義
    0前輪左:0x10
    1前輪右:0x12
    2後輪左:0x14
    3後輪右:0x16
    4投射前:0x18
    5投射後ろ:0x20
    6エアシリンダー:0x40
    正転:0x84~0xFF
    逆転:0x00~0x7C
    ショートブレーキ:0x7D~0x83
*/
I2C i2c(PB_9, PB_8);
//PS3コン定義
//PC定義
Serial pc(USBTX,USBRX);
//ロリコン定義
QEI front_roller_wheel(PC_8, PC_6, NC, 624);
QEI back_roller_wheel(PC_5, PA_12, NC, 624);
//PID定義
PID front_roller(roller_Kp, froller_Ki, roller_Kd, 0.001);
PID back_roller(roller_Kp, broller_Ki, roller_Kd, 0.001);
//超音波センサ定義
HCSR04 topsonic(PB_2,PB_1);
//HCSR04 undersonic(PB_15,PB_14);
//LED定義
DigitalOut Power(PC_12);//Green
DigitalOut i2ccheck(PB_7);//Blue
DigitalOut sncled(PC_2);//Yellow
DigitalOut Powemer(PC_3);//Red
DigitalOut myled(LED1);
//緊急停止定義
DigitalOut emersig(PC_0);
//タイマー定義
Ticker get_rps;//ロリコンからRPMの読み取り
Timer shootpet;
Timer mainloop;

InterruptIn but(USER_BUTTON);

//変数定義
//データ配列
char send_data[4][1];//足回り用データ配列
char send_data2[1];//エアシリンダー用データ配列
int address[4] = {0x10, 0x12, 0x14, 0x16};//i2cアドレス
int err[4] = {1, 1, 1, 1};//I2Cリザルト

int front_roller_rpm;//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 front_roller_data[1];
char back_roller_data[1];
int dmove_val;
static int j = 0;
int sonicval;
int mode = true;

//プロトタイプ宣言
//メイン制御関数
void ctrl();
void semiauto();
//移動関数
void amove_R();
void amave_L();
void dmove();
//void shortb(int i);
void backfif();
void sendi2c();
//回転数取得関数
void flip();
//ローラーPID
int roller_PID(int front, int back);
int sec;
//発射関数
void shoot();
//その他諸関数
void changemode();
void printdata();
void outemergency();
void emergency();
void TimerIsr();
Ticker timer_;

//実装部

//メイン関数
int main()
{
    //float looptime;
    pc.baud(460800);
    emersig = 0;
    Power = 1;
    i2ccheck = 0;
    get_rps.attach_us(&flip, 40000);
    but.rise(shoot);
   /*
    TimerIsr();
    timer_.attach(&TimerIsr, 1);
    */
    while(true){
        //pc.printf("\x1b[0m");
        roller_PID(777,655);
        //roller_PID(1600,800);
        topsonic.start();
        wait(0.01); 
        sonicval = topsonic.get_dist_cm();
        
        pc.printf("%3d,%3d,%d\n\r", abs(front_roller_rpm), abs(back_roller_rpm),sonicval);
        
        i2c.write(0x18, front_roller_data, 1, false);
        i2c.write(0x20, back_roller_data,  1, false);
        }
}
/*
void TimerIsr()
{
    static int k = 0;
    sec = k % 60;   // seconds
    k++;
}
*/

//メイン制御関数
//緊急停止
void emergency()
{
    emersig = 1;
    Powemer = 1;
}

//緊急停止解除
void outemergency()
{
    emersig = 0;
    Powemer = 0;
}

//I2C送信
void sendi2c()
{
    err[0] = i2c.write(address[0], send_data[0], 1);
    err[1] = i2c.write(address[1], send_data[1], 1);
    err[2] = i2c.write(address[2], send_data[2], 1);
    err[3] = i2c.write(address[3], send_data[3], 1);
    if (err[0]!=0 || err[1]!=0 || err[2]!=0 || err[3]!=0) {
        while(10) {
            i2ccheck = 1;
            wait(0.1);

            i2ccheck = 0;
        }
    } else {
        i2ccheck = 1;
    }
}

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

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

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

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

    //インクリメント
    j++;
    //iが最大値(9)になったらリセット
    if(j > 9) {
        j = 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 / 1200;
    back_roller_rpm  = ave_back_roller_pulse  * 25 * 60 / 1200;
}

//ローラー
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;
}

//投擲動作
void shoot()
{
    int time;
    shootpet.start();
    while(true) {
        time = shootpet.read();
        if(time <= 0.5) {
            send_data2[0] = 0x0F;
            i2c.write(0x40, send_data2, 1);
        } else {
            send_data2[0] = 0x80;
            i2c.write(0x40, send_data2, 1);
            shootpet.stop();
            shootpet.reset();
            break;
        }
    }
}