File content as of revision 7:9837f47a2f55:

/* ------------------------------------------------------------------- */
/* 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 2.5
#define froller_Ki 1.0
#define broller_Ki 0.8
#define roller_Kd 0.0

//定義

/*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 shooot;

//変数定義
//データ配列
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];
static int j = 0;
int sonicval;
int mode = true;
int table[3];
int times = 0;
int letshoot = 0;
int endshoot = 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()
{
    Power = 1;
    emersig = 0;
    i2ccheck = 0;
    pc.baud(460800);
    get_rps.attach_us(&flip, 40000);
    pc.printf("\x1b[0m");

    send_data2[0] = 0x80;
    i2c.write(0x10, send_data2, 1);
    i2c.write(0x12, send_data2, 1);
    i2c.write(0x14, send_data2, 1);
    i2c.write(0x16, send_data2, 1);
    i2c.write(0x18, send_data2, 1);
    i2c.write(0x20, send_data2, 1);
    i2c.write(0x40, send_data2, 1);

    while(true) {
        topsonic.start();
        sonicval = topsonic.get_dist_cm();
        ps3s.getdata();
        ps3s.checkdata();
        ctrl();
        sendi2c();
    }
}

//メイン制御関数
void ctrl()
{
    bool dmove_val = false;
    front_roller_data[0] = 0x80;
    back_roller_data[0] = 0x80;
    send_data2[0] = 0x80;
    i2c.write(0x18, front_roller_data, 1, false);
    i2c.write(0x20, back_roller_data, 1, false);
    i2c.write(0x40, send_data2, 1, false);
    for(int i = 0; i < 4; i++) {
        send_data[i][0] = 0x80;//ショートブレーキ
    }

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

    if(ps3data[6] == 1) { //まる
        shoot();
    } else if(ps3data[4]==1) { //さんかく
        //semiauto();
    } else if(ps3data[5]==1) { //ばつ
        //changemode();
    } else if(ps3data[7]==1){//しかく
        endshoot = 0;
        shooot.reset();
        times = 0;
    }
    if(emersig == 0) {

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

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

//緊急停止
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()
{

    if(ps3data[0] == 1) {
        send_data[0][0] = 0xF7;
        send_data[1][0] = 0xFF;
        send_data[2][0] = 0xF7;
        send_data[3][0] = 0xFF;
    } else if(ps3data[1] == 1) {
        send_data[0][0] = 0x00;
        send_data[1][0] = 0x0A;
        send_data[2][0] = 0x00;
        send_data[3][0] = 0x0A;
    } else if(ps3data[2] == 1) {
        send_data[0][0] = 0xF5;
        send_data[3][0] = 0xFF;
        send_data[1][0] = 0x09;
        send_data[2][0] = 0x00;
    } else if(ps3data[3] == 1) {
        send_data[0][0] = 0x00;
        send_data[3][0] = 0x09;
        send_data[1][0] = 0xFF;
        send_data[2][0] = 0xF5;
    } else if(ps3data[10] == 1) {
        send_data[1][0] = 0x00;
        send_data[3][0] = 0x00;
        send_data[0][0] = 0xFF;
        send_data[2][0] = 0xFF;
    } else if(ps3data[8] == 1) {
        send_data[0][0] = 0x00;
        send_data[2][0] = 0x00;
        send_data[1][0] = 0xFF;
        send_data[3][0] = 0xFF;
    }

}

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


//I2C送信
void sendi2c()
{
    if(send_data[0][0] < 0x80) {
        send_data[0][0] += 50;
    } else if(send_data[0][0] > 0x80) {
        send_data[0][0] -= 50;
    }
    if(send_data[1][0] < 0x80) {
        send_data[1][0] += 50;
    } else if(send_data[1][0] > 0x80) {
        send_data[1][0] -= 50;
    }
    if(send_data[2][0] < 0x80) {
        send_data[2][0] += 50;
    } else if(send_data[2][0] > 0x80) {
        send_data[2][0] -= 50;
    }
    if(send_data[3][0] < 0x80) {
        send_data[3][0] += 50;
    } else if(send_data[3][0] > 0x80) {
        send_data[3][0] -= 50;
    }
    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) {
        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,%d,%d\n\r",abs(front_roller_rpm), abs(back_roller_rpm),sonicval,times);
}

//ローラー
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()
{
    letshoot = 0;
    int front = 1650;
    int back = 600;
    while(endshoot == 0 || times < 2) {
        roller_PID(front, back);
        i2c.write(0x18, front_roller_data, 1, false);
        i2c.write(0x20, back_roller_data, 1, false);
        if( ( ( abs(front_roller_rpm) >= (front - 5)) && ( abs(front_roller_rpm) <= front + 5) ) && ( (abs(back_roller_rpm) >= back - 5) && (abs(back_roller_rpm) <= back + 5) ) ) {
            letshoot++;
        } else {
            letshoot = 0;
        }
        if(letshoot == 30 && endshoot == 0) {
            send_data2[0] = 0x0F;
            i2c.write(0x40, send_data2, 1, false);
            endshoot = 1;
            shooot.start();
        }
        if(endshoot == 1){
            times = shooot.read();
        }
    }
    shooot.stop();
}

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

void semiauto()
{
    while(true) {//一回目低固定
        if(mode) {
            send_data[0][0] = 0xE1;
            send_data[3][0] = 0xEC;
            send_data[1][0] = 0x1D;
            send_data[2][0] = 0x14;
        } else if(!mode) {
            send_data[0][0] = 0x00;
            send_data[3][0] = 0x09;
            send_data[1][0] = 0xFF;
            send_data[2][0] = 0xF5;
        }
        topsonic.start();
        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 <= 74) {
                send_data[0][0] = 0x80;
                send_data[1][0] = 0x80;
                send_data[2][0] = 0x80;
                send_data[3][0] = 0x80;
            }
        }
        sendi2c();
        if(sonicval <= 74) {
            shoot();
            for(int i = 0; i < 100; i++) {
                pc.printf("Press Box button if pet stood");
                ps3s.getdata();
                ps3s.checkdata();
                if(ps3data[7] == 1) {
                    table[0] = true;
                }
            }
        }
        if(table[0]) {
            break;
        }
    }

    while(true) { //高固定
        if(mode) {
            send_data[0][0] = 0xE1;
            send_data[3][0] = 0xEC;
            send_data[1][0] = 0x1D;
            send_data[2][0] = 0x14;
        } else if(!mode) {
            send_data[0][0] = 0x00;
            send_data[3][0] = 0x09;
            send_data[1][0] = 0xFF;
            send_data[2][0] = 0xF5;
        }
        topsonic.start();
        sonicval = topsonic.get_dist_cm();
        if(sonicval < 150) {
            send_data[0][0] = 0xF7;
            send_data[1][0] = 0xFF;
            send_data[2][0] = 0xF7;
            send_data[3][0] = 0xFF;
            if(sonicval <= 124) {
                send_data[0][0] = 0x80;
                send_data[1][0] = 0x80;
                send_data[2][0] = 0x80;
                send_data[3][0] = 0x80;
            }
        }
        sendi2c();
        if(sonicval <= 124) {
            shoot();
            for(int i = 0; i < 100; i++) {
                pc.printf("Press Box button if pet stood");

                ps3s.getdata();
                ps3s.checkdata();
                if(ps3data[7] == 1) {
                    table[1] = true;
                }
            }
        }
        if(table[1]) {
            break;
        }
    }
}


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]);
    pc.printf("%d,",int(send_data[0][0]));
    pc.printf("%d,",int(send_data[0][1]));
    pc.printf("%d.",int(send_data[0][2]));
    pc.printf("%d\n\r",int(send_data[0][3]));
}