File content as of revision 6:2675289c6463:

/////////////////////////////////////
/*NHK2019*/
//Aチーム手動機プログラム
/*
アドレス
0x10:右前
0x12:左後ろ
0x14:右後ろ
0x16:左前
0x18:サーボ制御回路
0x20:右ラック
0x22:左ラック.
0x24:回収機構
0x26:右ファン
0x28:左ファン
0x30:吐き出し機構
*/
/////////////////////////////////////

//宣言
#include "mbed.h"
#include "PS3.h"
I2C i2c(D14,D15);
Serial pc(USBTX,USBRX);
Serial slave(PC_6,PC_7);
PS3 ps3(D8,D2);
DigitalOut led(D5);//電源確認
DigitalOut tsushin(D6);//通信確認
DigitalOut data_check(D7);
DigitalOut pwm(D4);
DigitalOut stop(D10);
DigitalOut led1(LED1);
Timer timer;
Timer timer2;

//変数
char data1;//右上
char data2;//左上
char data3;//右下
char data4;//左下
char data_servo;
char data5;//ラック直動右
char data6;//ラック直動左
char data7;//回収機構
char data8;//右ファン
char data9;//左ファン
char data10;//パトランプ
char data11;//吐き出し機構
char get_data_rs232=0x00;
int Ry;
int Rx;
int Ly;
int right1;
int right2;
int left1;
int left2;
int select;
int start;
int circle;
int cross;
int triangle;
int square;
int ue1;
int shita;
int migi1;
int hidari1;
int tushin_check;
int old_select=0;
int i=1;
int old_start=0;
int j=1;
int old_circle=0;
int k=1;
int old_square=0;
int old_migi1=0;
int old_hidari1=0;
int n=1;
int old_right2=0;
int o=1;
int old_right1=0;
int old_left1=0;
int p=5;
int q=0;
int old_ue1=0;
int old_shita=0;
int old_left2=0;
int old_triangle=0;
int old_cross=0;
int r=6;
int s=5;
bool x=0;


//関数プロトタイプ宣言
void initialization();
void get_data();
void change_data();
void change_pwm();
void change_servo();
//void change_rack();
void change_fan();
void send_data(char address,char data);
void emergency();
void change_rack_2();
void sequence_kaisyu();
//void kaisyu();
//void sequence_kaisyu_short();
//void sequence_hakidashi();
void hakidashi();
void mode_change();



//メイン関数
int main()
{
    led=1;
    pwm=1;
    initialization();
    while(true) {
        emergency();
        get_data();
        mode_change();
        change_pwm();
        change_data();
        change_servo();
        //change_rack();
        change_rack_2();
        sequence_kaisyu();
        //kaisyu();
        //sequence_kaisyu_short();
        //sequence_hakidashi();
        hakidashi();
        change_fan();
        send_data(0x10,data1);
        send_data(0x12,data2);
        send_data(0x14,data3);
        send_data(0x16,data4);
        send_data(0x18,data_servo);
        send_data(0x20,data5);
        send_data(0x22,data6);
        //send_data(0x24,data7);
        send_data(0x26,data8);
        send_data(0x28,data9);
        send_data(0x40,data10);
        //send_data(0x30,data11);




    }
}


//初期化
void initialization()
{
    data1=0x80;
    data2=0x80;
    data3=0x80;
    data4=0x80;
    data5=0x80;
    data6=0x80;
    data7=0x10;
    data8=0x80;
    data9=0x80;
    data10=0x80;
    data11=0x80;
    data_servo=0x00;

    send_data(0x10,data1);
    send_data(0x12,data2);
    send_data(0x14,data3);
    send_data(0x16,data4);
    send_data(0x20,data5);
    send_data(0x22,data6);
    send_data(0x24,data7);
    send_data(0x26,data8);
    send_data(0x28,data9);
    send_data(0x40,data10);
    send_data(0x30,data11);
    send_data(0x18,data_servo);
    

    data10=0x3f;
    send_data(0x40,data10);
    
    if(q==0){
        data10=0xdf;
        send_data(0x40,data10);
    }else if(q==1){
        data10=0xcf;
        send_data(0x40,data10);
    }
    
    /*data_servo=0x03;
    send_data(0x18,data_servo);
    data_servo=0x04;
    send_data(0x18,data_servo);*/
    timer2.reset();
    
}


//データ読み込み
void get_data()
{
    Ry=ps3.getRightJoystickYaxis();
    Rx=ps3.getRightJoystickXaxis();
    Ly=ps3.getLeftJoystickYaxis();
    circle=ps3.getButtonState(maru);
    cross=ps3.getButtonState(batu);
    triangle=ps3.getButtonState(sankaku);
    square=ps3.getButtonState(sikaku);
    left1=ps3.getButtonState(L1);
    left2=ps3.getButtonState(L2);
    right1=ps3.getButtonState(R1);
    right2=ps3.getButtonState(R2);
    select=ps3.getSELECTState();
    start=ps3.getSTARTState();
    ue1=ps3.getButtonState(ue);
    shita=ps3.getButtonState(sita);
    hidari1=ps3.getButtonState(hidari);
    migi1=ps3.getButtonState(migi);
    get_data_rs232=slave.getc();
    


    if(get_data_rs232) {
        led1=1;
    } else {
        led1=0;
    }
    //pc.printf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",Ry,Rx,Ly,left1,left2,right1,right2,select,start,ue1,shita,triangle,square,migi1,hidari1,tushin_check,i);
    if(Ry==0&&Rx==0&&Ly==0&&left1==0&&right1==0&&right2==0&&left2==0&&select==0&&start==0&&shita==0&&triangle==0&&ue1==0&&migi1==0&&hidari1==0&&square==0&&circle==0&&cross==0) {
        data_check=0;
    } else {
        data_check=1;
    }

}

//緊急停止
void emergency()
{
    if(start!=old_start) {
        old_start=start;
        if(start==1) {
            if(j==1) {
                //緊急停止
                led=0;
                stop=1;
                j=0;
            } else if(j==0) {
                //緊急停止解除
                led=1;
                stop=0;
                j=1;
            }
        }
    }
}


//モードチェンジ
void mode_change(){
    if(old_select!=select){
        old_select=select;
        if(select==1){
            if(q==0){
                //送風モード
                q=1;
        /*        data_servo=0x03;
                send_data(0x18,data_servo);
                data_servo=0x04;
                send_data(0x18,data_servo);*/
                data10=0xcf;
                send_data(0x40,data10);
            }else if(q==1){
                //回収モード
                q=0;
                data10=0xdf;
                send_data(0x40,data10);
            }
        }
    }
}

//データ変化(メカナム)
void change_data()
{

    if(Ry!=0||Rx!=0) {
        square=0;
        migi1=0;
    }

    if(Ry>30&&i==0) {
        if(right1==1) {
            data1=0xcf;
            data2=0x00;
            data3=0xcf;
            data4=0x00;
        } else if(left1==1) {
            data1=0x00;
            data2=0xcf;
            data3=0x00;
            data4=0xcf;
        } else {
            data1=0x00;
            data2=0x00;
            data3=0x00;
            data4=0x00;
        }
    } else if(Ry<-30&&i==0) {
            if(right1==1){
                data1=0xdf;
                data2=0xff;
                data3=0xdf;
                data4=0xff;
            }else if(left1==1){
                data1=0xff;
                data2=0xdf;
                data3=0xff;
                data4=0xdf;
            }else{
                data1=0xff;
                data2=0xff;
                data3=0xff;
                data4=0xff;
            }
    } else if(Rx>30&&i==0) {
        if(right1==1) {
            data1=0xff;
            data2=0xdf;
            data3=0xcf;
            data4=0x00;
        } else if(left1==1) {
            data1=0xdf;
            data2=0xff;
            data3=0x00;
            data4=0xcf;
        } else {
            data1=0xff;
            data2=0xff;
            data3=0x00;
            data4=0x00;
        }
    } else if(Rx<-30&&i==0) {
        if(right1==1) {
            data1=0xcf;
            data2=0x00;
            data3=0xff;
            data4=0xdf;
        } else if(left1==1) {
            data1=0x00;
            data2=0xcf;
            data3=0xdf;
            data4=0xff;
        } else {
            data1=0x00;
            data2=0x00;
            data3=0xff;
            data4=0xff;
        }
    } else if(right1==1&&i==0) {
        data1=0xff;
        data2=0x00;
        data3=0xff;
        data4=0x00;
    } else if(left1==1&&i==0) {
        data1=0x00;
        data2=0xff;
        data3=0x00;
        data4=0xff;
    } else if(Ry>30&&i==1) {
        if(right1==1) {
            data1=0x4f;
            data2=0x3f;
            data3=0x4f;
            data4=0x3f;
        } else if(left1==1) {
            data1=0x3f;
            data2=0x4f;
            data3=0x3f;
            data4=0x4f;
        } else {
            data1=0x3f;
            data2=0x3f;
            data3=0x3f;
            data4=0x3f;
        }
    } else if(Ry<-30&&i==1) {
        if(right1==1){
            data1=0x5f;
            data2=0xbf;
            data3=0x5f;
            data4=0xbf;
        }else if(left1==1){
            data1=0xbf;
            data2=0x5f;
            data3=0xbf;
            data4=0x5f;
        }else{
            data1=0xbf;
            data2=0xbf;
            data3=0xbf;
            data4=0xbf;
        }
    } else if(Rx>30&&i==1) {
        if(right1==1) {
            data1=0xbf;
            data2=0x5f;
            data3=0x4f;
            data4=0x3f;
        } else if(left1==1) {
            data1=0x5f;
            data2=0xbf;
            data3=0x3f;
            data4=0x4f;
        } else {
            data1=0xbf;
            data2=0xbf;
            data3=0x3f;
            data4=0x3f;
        }
    } else if(Rx<-30&&i==1) {
        if(right1==1) {
            data1=0x4f;
            data2=0x3f;
            data3=0xbf;
            data4=0x5f;
        } else if(left1==1) {
            data1=0x3f;
            data2=0x4f;
            data3=0x5f;
            data4=0xbf;
        } else {
            data1=0x3f;
            data2=0x3f;
            data3=0xbf;
            data4=0xbf;
        }
    } else if(right1==1&&i==1) {
        data1=0xbf;
        data2=0x3f;
        data3=0xbf;
        data4=0x3f;
    } else if(left1==1&&i==1) {
        data1=0x3f;
        data2=0xbf;
        data3=0x3f;
        data4=0xbf;
    } else {
        data1=0x80;
        data2=0x80;
        data3=0x80;
        data4=0x80;
    }
}


//pwm変化
void change_pwm()
{
    if(cross!=old_cross) {
        old_cross=cross;
        if(cross==1) {
            if(i==1) {
                pwm=0;
                data10=0xbf;
                send_data(0x40,data10);
                i=0;
            } else if(i==0) {
                pwm=1;
                data10=0x3f;
                send_data(0x40,data10);
                i=1;
            }
        }
    }
}

//サーボモーター
void change_servo()
{
    if(right2!=old_right2) {
        old_right2=right2;
        //ハンガー機構&バスタオル機構initialize処理
        if(right2==1) {
            if(o==1) {
                
               // timer.start();
              //  data_servo=0x03;
               // send_data(0x18,data_servo);
                data_servo=0x01;
                //send_data(0x18,data_servo);
                o=2;
            } else if(o==2) {
                data_servo=0x02;
                //send_data(0x18,data_servo);
                o=3;
            }
            //data_servo=0x01;
        }
    } else if(circle!=old_circle) {
        old_circle=circle;
        //バスタオル挟む機構
        if(circle==1/*&&(get_data_rs232 & 0b00000011)*/) {
            if(k==1) {
                data_servo=0xf3;
                //send_data(0x18,data_servo);
                k=0;
            } else if(k==0/*&&(get_data_rs232 & 0b00000011)*/) {
                data_servo=0xf4;
                //send_data(0x18,data_servo);
                k=1;
            }
        }
    }/*else if(hidari1!=old_hidari1){
                    old_hidari1=hidari1;

                    if(hidari1==1&&select==0){
                        if(n==1){
                            data_servo=0x05;
                            n=0;
                        }else if(n==0){
                            data_servo=0x06;
                            n=1;
                        }
                    }
            }*/
            
           /* if(timer.read()>2&&o==2){
                data_servo=0x04;
                send_data(0x18,data_servo);
                timer.stop();
                timer.reset();
                }*/
}

//ラック
/*
void change_rack()
{


    //右
    if(triangle==1&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)) {
        if(!(get_data_rs232 & 0x01)) {
            data5=0x00;
        } else if(get_data_rs232 & 0x01) {
            data5=0x10;
        }
    } else if(triangle==0&&cross==1&&ue1==0&&shita==0&&(-32<Ly<32)) {
        if(!(get_data_rs232 & 0x04)) {
            data5=0xff;
        } else if(get_data_rs232 & 0x04) {
            data5=0x10;
        }
        //左
    } else if(triangle==0&&cross==0&&ue1==1&&shita==0&&(-32<Ly<32)) {
        if(!(get_data_rs232 & 0x02)) {
            data6=0xff;
        } else if(get_data_rs232 & 0x02) {
            data6=0x10;
        }
    } else if(triangle==0&&cross==0&&ue1==0&&shita==1&&(-32<Ly<32)) {
        if(!(get_data_rs232 & 0x08)) {
            data6=0x00;
        } else if(get_data_rs232 & 0x08) {
            data6=0x10;
        }
    } else if(triangle==0&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)) {
        data5=0x80;
        data6=0x80;

    }

}
*/

//ラック左右
void change_rack_2()
{
    /*
    if(triangle==0&&cross==0&&ue1==0&&shita==0&&Ly>32) {
        if(get_data_rs232 & 0x01) {
            data5=0x10;
        } else {
            data5=0x00;
        }
        if(get_data_rs232 & 0x02) {
            data6=0x10;
        } else {
            data6=0xff;
        }
    } else if(triangle==0&&cross==0&&ue1==0&&shita==0&&Ly<-32) {
        if(get_data_rs232 & 0x04) {
            data5=0x10;
        } else {
            data5=0xff;
        }
        if(get_data_rs232 & 0x08) {
            data6=0x10;
        } else {
            data6=0x00;
        }
    } else if(triangle==0&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)) {
        data5=0x80;
        data6=0x80;
    }
    */
    
    if(ue1!=old_ue1){
        old_ue1=ue1;
        if(ue1==1&&start==0){
            data5=0x00;
            data6=0xff;
            
            /*data_servo=0x03;
            send_data(0x18,data_servo);
            data_servo=0x04;
            send_data(0x18,data_servo);*/
        }
    }else if(shita!=old_shita){
        old_shita=shita;
        if(shita==1&&start==0){
            data5=0xff;
            data6=0x00;
        }
    }
    
    if((get_data_rs232 & 0x01)&&data5==0x00){
        data5=0x10;
        send_data(0x20,data5);
    }else if((get_data_rs232 & 0x04)&&data5==0xff){
        data5=0x10;
        send_data(0x20,data5);
    }
    
    if((get_data_rs232 & 0x02)&&data6==0xff){
        data6=0x10;
        send_data(0x22,data6);
    }else if((get_data_rs232 & 0x08)&&data6==0x00){
        data6=0x10;
        send_data(0x22,data6);
    }
    
    
}

//回収機構

void sequence_kaisyu()
{
    if(q==0){
    if(square!=old_square) {
        old_square=square;
        if(square==1&&select==0) {
            if(x==0){
                data_servo=0x07;
                send_data(0x18,data_servo);
                wait(1.2);
                data7=0x00;
                get_data_rs232=get_data_rs232 & 0b11101111;
                while(!(get_data_rs232 & 0x10)){
                    send_data(0x24,data7);
                    get_data_rs232=slave.getc();
                }
                data7=0x10;
                send_data(0x24,data7);
                x=1;
            /*
            data_servo=0x07;
            send_data(0x18,data_servo);
            wait(1.2);
            data7=0x00;
            get_data_rs232=get_data_rs232 & 0b11101111;
            while(!(get_data_rs232 & 0x10)) {
                send_data(0x24,data7);
                get_data_rs232=slave.getc();
            }
            data7=0x10;
            send_data(0x24,data7);
            data_servo=0x08;
            send_data(0x18,data_servo);
            wait(1.2);
            data7=0xff;
            get_data_rs232=get_data_rs232 & 0b11011111;
            while(!(get_data_rs232 & 0x20)) {
                send_data(0x24,data7);
                get_data_rs232=slave.getc();
            }
            data7=0x10;
            send_data(0x24,data7);
            */
            }else if(x==1){
                data_servo=0x08;
                send_data(0x18,data_servo);
                wait(1.2);
                data7=0xff;
                get_data_rs232=get_data_rs232 & 0b11011111;
                while(!(get_data_rs232 & 0x20)){
                    send_data(0x24,data7);
                    get_data_rs232=slave.getc();
                }
                data7=0x10;
                send_data(0x24,data7);
                x=0;
            
            }
        }/*else if(triangle!=old_triangle){
            if(triangle==1&&select==0){
                                
                data_servo=0x07;
                send_data(0x18,data_servo);
                wait(1.2);
                data7=0x00;
                send_data(0x24,data7);
                wait(0.3);
                data7=0x10;
                send_data(0x24,data7);
                data_servo=0x08;
                send_data(0x18,data_servo);
                wait(1.2);
                data7=0xff;
                get_data_rs232=get_data_rs232 & 0b11011111;
                while(!(get_data_rs232 & 0x20)) {
                    send_data(0x24,data7);
                    get_data_rs232=slave.getc();
                }
                data7=0x10;
                send_data(0x24,data7);
            }*/
        }
    }
}


/*
void kaisyu(){
        if(q==0){
        if(old_square!=square){
            old_square=square;
            if(square==1){
                p=1;

                }
            }
         switch(p){
            
            case 1:
            data_servo=0x07;
            send_data(0x24,data_servo);
            timer2.start();
            if(timer2.read()>3.5f){
                timer2.stop();
                timer2.reset();
                p=2;
            }
            break;
            
            case 2:
            data7=0x00;
            send_data(0x24,data7);
            if(get_data_rs232 & 0x10){
                data7=0x10;
                send_data(0x24,data7);
                p=3;
            }
            break;
            
            case 3:
            data_servo=0x08;
            send_data(0x18,data_servo);
            timer2.start();
            if(timer2.read()>1.3f){
                p=4;
            }

            break;
            
            case 4:
            data7=0xff;
            send_data(0x24,data7);
            if(get_data_rs232 & 0x20){
                data7=0x10;
                send_data(0x24,data7);
                p=5;
            }
            break;
            
            default:
                p=5;
            break;
            
            }
         }   
    }
*/

/*
void sequence_hakidashi()
{
    if(q==0){
    if(migi1!=old_migi1) {
        old_migi1=migi1;
        if(migi1==1&&select==0) {
              data_servo=0x07;
              send_data(0x18,data_servo);
              wait(1);
              data7=0x00;
              send_data(0x24,data7);
              wait(0.5);
              data7=0x10;
              send_data(0x24,data7);
              data11=0xff;
              while(!(get_data_rs232 & 0x40)){
                  send_data(0x30,data11);
                  get_data_rs232=slave.getc();
              }
              data11=0x10;
              send_data(0x38,data11);
              data11=0x00;
              while(!(get_data_rs232 & 0x80)){
                  send_data(0x30,data11);
                  get_data_rs232=slave.getc();
              }
              data11=0x10;
              send_data(0x30,data11);
              data7=0xff;
              while(!(get_data_rs232 & 0x20)){
                  send_data(0x24,data7);
                  get_data_rs232=slave.getc();
              }
              data7=0x10;
              send_data(0x24,data7);
 
            }
        }
    }
}
*/

/*
void sequence_kaisyu_short(){
    if(q==0){
        if(old_triangle!=triangle){
            old_triangle=triangle;
            if(triangle==1){
                
                data_servo=0x07;
                send_data(0x18,data_servo);
                wait(1.3);
                data7=0x00;
                send_data(0x24,data7);
                wait(0.3);
                data7=0x10;
                send_data(0x24,data7);
                data_servo=0x08;
                send_data(0x18,data_servo);
                wait(1.3);
                data7=0xff;
                get_data_rs232=get_data_rs232 & 0x;
                while(!(get_data_rs232 & 0x20)) {
                    send_data(0x24,data7);
                    get_data_rs232=slave.getc();
                }
                data7=0x10;
                send_data(0x24,data7);

            }
        }
    }
}*/

void hakidashi(){
    if(q==0){
        if(old_migi1!=migi1){
            old_migi1=migi1;
            if(migi1==1&&select==0){
                r=1;
            }
        }
        
        switch(r){
            case 1:
            data_servo=0x07;
            send_data(0x18,data_servo);
            timer2.start();
            if(timer2.read()>1.5f){
                timer2.stop();
                timer2.reset();
                r=2;
            }
            break;
            
            case 2:
            data7=0x00;
            send_data(0x24,data7);
            timer2.start();
            if(timer2.read()>0.3f){
                timer2.stop();
                timer2.reset();
                data7=0x10;
                send_data(0x24,data7);
                r=3;
            }
            break;
            
            case 3:
            data11=0xff;
            send_data(0x30,data11);
            //get_data_rs232=get_data_rs232 & 0b10111111;
            if(get_data_rs232 & 0x40){
                data11=0x10;
                send_data(0x30,data11);
                r=4;
            }
            break;
            
            case 4:
            data11=0x00;
            send_data(0x30,data11);
            //get_data_rs232=get_data_rs232 & 0b01111111;
            if(get_data_rs232 & 0x80){
                data11=0x10;
                send_data(0x30,data11);
                r=5;
            }
            break;
            
            case 5:
            data7=0xff;
            send_data(0x24,data7);
            //get_data_rs232=get_data_rs232 & 0b11011111;
            if(get_data_rs232 & 0x20){
                data7=0x10;
                send_data(0x24,data7);
                r=6;
            }
            break;
            
            default:
            r=6;
            break;
        }
    }
}



//ファン
void change_fan()
{
    if(q==1){   
    if(left2==1&&(old_triangle==triangle)) {
        data8=0xff;
        data9=0xff;
        data10=0xff;
    } else if(left2==0&&(old_triangle==triangle)) {
        data8=0x00;
        data9=0x00;
        data10=0x00;
    }else if(left2==0&&(old_triangle!=triangle)&&select==0&&(get_data_rs232 & 0b00000011)){
        old_triangle=triangle;
        if(triangle==1){
            data8=0xff;
            data9=0xff;
            data10=0xff;
            data_servo=0x03;
            send_data(0x26,data8);
            send_data(0x28,data9);
            send_data(0x40,data10);
            wait(0.5);
            send_data(0x18,data_servo);
            wait(1);
            data8=0x00;
            data9=0x00;
            data10=0x00;
            send_data(0x26,data8);
            send_data(0x28,data9);
            send_data(0x40,data10);
            wait(0.5);
            data_servo=0x04;
            send_data(0x18,data_servo);
        }
    }
}
    
}

//i2c
void send_data(char address,char data)
{
    tsushin=1;
    tushin_check=0;
    i2c.start();
    i2c.write(address);
    tushin_check=i2c.write(data);
    i2c.stop();
}