File content as of revision 4:58a2afff53a4:

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

#define ps3data ps3s.condata
#define PI 3.14159265359
#define roller_Kp 1.5
#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コン定義
PS3s ps3s(PC_10, PC_11);
//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;
int table;
double looptime;
float timer1 = 0;

//プロトタイプ宣言
//メイン制御関数
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);
//発射関数
void shoot();
//その他諸関数
void changemode();
void printdata();
void outemergency();
void emergency();

//実装部

//メイン関数
int main()
{
    pc.baud(460800);
    emersig = 0;
    Power = 1;
    i2ccheck = 0;
    get_rps.attach_us(&flip, 40000);
    but.rise(shoot);
    pc.printf("\x1b[0m");
    while(true) {
        looptime = mainloop.read();
        mainloop.reset();
        mainloop.start();
        //roller_PID(777,655);
        //i2c.write(0x18, front_roller_data, 1, false);
        //i2c.write(0x20, back_roller_data,  1, false);
        topsonic.start();
        wait(0.01);
        sonicval = topsonic.get_dist_cm();
        ps3s.getdata();
        ps3s.checkdata();
        ctrl();
        //printdata();
        mainloop.stop();
    }
}

//メイン制御関数
void ctrl()
{

    if(ps3data[12]!=0) {//Select
        emergency();//緊急停止
    } else if(ps3data[13]!=0) {//Start
        outemergency();//緊急停止解除
    }

    if(emersig == 0) {
        for(int i = 0; i < 4; i++) {
            send_data[i][0] = 0x83;//ショートブレーキ
        }

        if(ps3data[0]!=0) {
            dmove_val += 1;//上
        } else if(ps3data[1]!=0) {
            dmove_val += 2;//下
        } else if(ps3data[2]!=0) {
            dmove_val += 4;//右
        } else if(ps3data[3]!=0) {
            dmove_val += 8;//左
        } else if(ps3data[8]!=0) {//L1
            dmove_val += 32;//左回転
        } else if(ps3data[10]!=0) {//R1
            dmove_val += 16;//右回転
        }
        if (dmove_val != 0) {
            dmove();//方向キー+L1R1キー
        }

        if(((ps3data[16] <= 51) || (ps3data[16] >= 77)) || ((ps3data[17] <= 51) || (ps3data[17] >= 77))) {
            amove_R();//アナログパッド
        }

        if(ps3data[7]!=0) {//しかく
            //backfif();50cm後退
        } else if(ps3data[6]!=0) {//まる
            shoot();//投擲
        } else if(ps3data[4]!=0) { //さんかく
            semiauto();
        } else if(ps3data[5]!=0) { //しかく
            changemode();
        }
        sendi2c();
    } else {
    }
}

//緊急停止
void emergency()
{
    emersig = 1;
    Powemer = 1;
}

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

//右アナログパッド
void amove_R()
{
    int RXdata = ps3data[16];
    int RYdata = ps3data[17];

    if(RXdata == 64 && RYdata < 64) {
        //垂直上
        send_data[0][0] = abs(RYdata - 53) + 202 - RYdata - 8;
        send_data[1][0] = abs(RYdata - 53) + 202 - RYdata;
        send_data[2][0] = abs(RYdata - 53) + 202 - RYdata - 8;
        send_data[3][0] = abs(RYdata - 53) + 202 - RYdata;

    } else if(RXdata == 64 && RYdata > 64) {
        //垂直下
        send_data[0][0] = abs(RYdata - 127) * 2;
        send_data[1][0] = abs(RYdata - 127) * 2 + 8;
        send_data[2][0] = abs(RYdata - 127) * 2;
        send_data[3][0] = abs(RYdata - 127) * 2 + 8;

    } else if(RXdata > 64 && RYdata == 64) {
        //垂直右
        send_data[0][0] = (RXdata - 75) + 203 - abs(RXdata - 127) - 8;
        send_data[3][0] = (RXdata - 75) + 203 - abs(RXdata - 127) - 8;
        send_data[1][0] = abs(RXdata - 127) * 2;
        send_data[2][0] = abs(RXdata - 127) * 2;

    } else if(RXdata < 64 && RYdata == 64) {
        //垂直左
        send_data[0][0] = RXdata * 2;
        send_data[3][0] = RXdata * 2;
        send_data[1][0] = abs(RXdata - 53) + 202 - RXdata - 8;
        send_data[2][0] = abs(RXdata - 53) + 202 - RXdata - 8;

    } else if (RXdata > 64 && RYdata < 64) {
        //右上
        send_data[0][0] = (RXdata - 74) + 202 - abs(RXdata - 127);
        send_data[3][0] = (RXdata - 74) + 202 - abs(RXdata - 127);
    } else if(RXdata < 64 && RYdata < 64) {
        //左上
        send_data[1][0] = abs(RXdata - 54) + 201 - RXdata;
        send_data[2][0] = abs(RXdata - 54) + 201 - RXdata;
    } else if(RXdata < 64 && RYdata > 64) {
        //左下
        send_data[0][0] = RXdata * 2;
        send_data[3][0] = RXdata * 2;
    } else if(RXdata > 64 && RYdata > 64) {
        //右下
        send_data[1][0] = abs(RXdata - 127) * 2;
        send_data[2][0] = abs(RXdata - 127) * 2;
    }
}

void dmove()
{
    int k = 32;
    while(true) {
        if (dmove_val == 0) {
            break;
        } else {
            if (dmove_val - k >= 0) {
                sncled = 1;
                switch (k) {
                    case 1:
                        send_data[0][0] = 0xF7;
                        send_data[1][0] = 0xFF;
                        send_data[2][0] = 0xF7;
                        send_data[3][0] = 0xFF;
                        break;
                    case 2:
                        send_data[0][0] = 0x00;
                        send_data[1][0] = 0x0A;
                        send_data[2][0] = 0x00;
                        send_data[3][0] = 0x0A;
                        break;
                    case 4:
                        send_data[0][0] = 0xF5;
                        send_data[3][0] = 0xFF;
                        send_data[1][0] = 0x09;
                        send_data[2][0] = 0x00;
                        break;
                    case 8:
                        send_data[0][0] = 0x00;
                        send_data[3][0] = 0x09;
                        send_data[1][0] = 0xFF;
                        send_data[2][0] = 0xF5;
                        break;
                    case 16:
                        send_data[1][0] = 0x00;
                        send_data[3][0] = 0x00;
                        send_data[0][0] = 0xFF;
                        send_data[2][0] = 0xFF;
                        break;
                    case 32:
                        send_data[0][0] = 0x00;
                        send_data[2][0] = 0x00;
                        send_data[1][0] = 0xFF;
                        send_data[3][0] = 0xFF;
                        break;
                    default:
                        break;
                }
                sncled = 0;
                dmove_val -= k;
            } else {
                k /= 2;
            }
        }
    }
}

//ショートブレーキ
void shortb(int i)
{
    i2ccheck = 0;
    send_data[i][0] = 0x83;
}

/*50cm後退
void backfif()
{
    int snc1;
    //int snc2;
    while(true) {
        topsonic.start();
        //undersonic.start();
        snc1 = topsonic.get_dist_cm();
        //snc2 = undersonic.get_dist_cm();
        if (snc1 != 0) { // && (snc2 != 0)) {
            break;
        }
    }

    int snc1buffer;
    //int snc2buffer;
    while(true) {
        topsonic.start();
        //undersonic.start();
        snc1buffer = topsonic.get_dist_cm();
        //snc2buffer = undersonic.get_dist_cm();
        if (snc1buffer == 0) { // || snc2buffer == 0) {
            continue;
        }
        if(snc1buffer >= snc1 + 50) {//|| (snc2buffer >= snc2 + 50)) {
            sncled = 1;
            for(int i = 0; i < 4; i++) {
                send_data[i][0] = 0x83;
            }
            break;
        } else {
            for(int i = 0; i < 4; i++) {
                send_data[i][0] = 0x10;
                i2c.write(address[i], send_data[i], 1);
                pc.printf("Goal:%dcm,Now:%dcm\n\r", snc1 + 50, snc1buffer);
            }
        }
    }
}*/

//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;
    pc.printf("%3d,%3d,%f\n\r",abs(front_roller_rpm), abs(back_roller_rpm), sonicval);
}

//ローラー
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()
{
    shootpet.start();
    while(true) {
        timer1 = shootpet.read();
        if(timer1 > 10) {
            break;
        }
        roller_PID(1000,1000);
        //i2c.write(0x18, front_roller_data, 1, false);
        i2c.write(0x20, back_roller_data,  1, false);
    }
    back_roller_data[0] = 0x80;
    i2c.write(0x20, back_roller_data,1,false);
    shootpet.stop();
    shootpet.reset();
}



//セミオートプレイテスト

void semiauto()
{
    while(true) {
        looptime = mainloop.read();
        pc.printf("%f,%d\n\r", looptime, sonicval);
        mainloop.reset();
        mainloop.start();
        send_data[0][0] = 0xE1;
        send_data[3][0] = 0xEC;
        send_data[1][0] = 0x1D;
        send_data[2][0] = 0x14;
        topsonic.start();
        wait(0.01);
        sonicval = topsonic.get_dist_cm();
        if(sonicval < 100) {
            send_data[0][0] = 0xF7;
            send_data[1][0] = 0xFF;
            send_data[2][0] = 0xF7;
            send_data[3][0] = 0xFF;
            if(sonicval == 75) {
                send_data[0][0] = 0x80;
                send_data[1][0] = 0x80;
                send_data[2][0] = 0x80;
                send_data[3][0] = 0x80;
            }
        }
        sendi2c();
        if(sonicval == 75) {
            table = 0;
            shoot();
        }
        mainloop.stop();
    }
}

void changemode()
{
    mode = !mode;
    if(mode) {
        pc.printf("\x1b[41m");
    } else {
        pc.printf("\x1b[44m");
    }
}


//デバック用データ出力
void printdata()
{
    for (int i = 0; i < 7; i++) {
        pc.printf("%d,",ps3s.Rdata[i]);
    }
    pc.printf("%d\n\r",ps3s.Rdata[7]);
}