File content as of revision 5:29971215ef50:

/////////////////////////////////////
/*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);
//DigitalOut aaaaa(D9);
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;
bool right1;
bool right2;
bool left1;
bool left2;
bool select;
bool start;
bool circle;
bool cross;
bool triangle;
bool square;
bool ue1;
bool shita;
bool migi1;
bool 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;
bool y=0;
bool kaisyu_arm=0;
char get_data_ps3[8];



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



//メイン関数
int main()
{   //pc.printf("hi\n");
    led=1;
    pwm=1;
    i2c.frequency(100000);
    slave.baud(115200);
    pc.baud(115200);
    initialization();

    //pc.printf("ha\n");
    while(true) {
        //pc.printf("hello\n");
        //aaaaa=1;
        emergency();
        get_data();
        mode_change();
        change_pwm();
        change_data();
        change_servo();
        change_rack_2();
        //sequence_kaisyu();
        kaisyu();
        //sequence_kaisyu_2();
        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);

        //aaaaa=0;
        //pc.printf("world\n");

    }
}


//初期化
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()
{
    int w;
    //int switch_judge=0;
    /*
    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);
    */
    for(w=0;w<8;w++){
    get_data_ps3[w]=ps3.PS3Data[w];
    }
   // get_data_ps3[0]=(*(+(button>>4)) >> (button & 0x0f)) & 1;
   
   switch(get_data_ps3[2] & 0x0c){
        case 0x0c:
        select=true;
        migi1=false;
        hidari1=false;
        break;
        case 0x04:
        select=false;
        migi1=true;
        hidari1=false;
        break;
        case 0x08:
        select=false;
        migi1=false;
        hidari1=true;
        break;
        default:
        select=false;
        migi1=false;
        hidari1=false;
        }
        
       switch(get_data_ps3[2] & 0x03){
        case 0x03:
        start=true;
        ue1=false;
        shita=false;
        break;
        case 0x01:
        start=false;
        ue1=true;
        shita=false;
        break;
        case 0x02:
        start=false;
        ue1=false;
        shita=true;
        break;
        default:
        start=false;
        ue1=false;
        shita=false;
        }

   triangle=bool(get_data_ps3[2] & 0x10);
   cross=bool(get_data_ps3[2] & 0x20);
   circle=bool(get_data_ps3[2] & 0x40);
   square=bool(get_data_ps3[1] & 0x01);
   left1=bool(get_data_ps3[1] & 0x02);
   left2=bool(get_data_ps3[1] & 0x04);
   right1=bool(get_data_ps3[1] & 0x08);
   right2=bool(get_data_ps3[1] & 0x10);
    Ry=(int)get_data_ps3[6]*-1+64;
    Rx=(int)get_data_ps3[5]-64;
    

    //pc.printf("a\n");
    if(slave.readable()==1){
        //pc.printf("c\n");
    get_data_rs232=slave.getc();
        //pc.printf("d\n");
    }
    //pc.printf("b\n");


    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);
    //pc.printf("0x%x\n",get_data_rs232);
    //pc.printf("%d\n",x);
    //pc.printf("%d,%d,%d\n",x,y,p);
    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) {
                data_servo=0x01;
                send_data(0x18,data_servo);
                o=2;
                kaisyu_arm=1;
            } else if(o==2) {
                data_servo=0x02;
                send_data(0x18,data_servo);
                o=3;
            }
        }
    } 
    
     if(circle!=old_circle) {
        old_circle=circle;
        //バスタオル挟む機構
        if(circle==1) {
            if(k==1) {
                data_servo=0xf3;
                send_data(0x18,data_servo);
                k=0;
            } else if(k==0) {
                data_servo=0xf4;
                send_data(0x18,data_servo);
                k=1;
            }
        }
    }
    
    if(q==0){
     if(triangle!=old_triangle){
            old_triangle=triangle;
            if(triangle==1){
                if(y==0){
                    data_servo=0x08;
                    send_data(0x18,data_servo);
                    y=1;
                }else if(y==1){
                    data_servo=0x07;
                    send_data(0x18,data_servo);
                    y=0;
                }
            }
        }   
    } 
}



//ラック左右
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);
                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;
              }
            }  
        }
    }
}
*/

//回収機構switch文version
void kaisyu(){
    if(q==0){
        if(kaisyu_arm!=0){
        if(old_square!=square){
            old_square=square;
            if(square==1){
                if(x==0){
                    p=1;
                    x=1;
                    timer2.start();
                }else if(x==1){
                    p=4;
                    x=0;
                }
            }
        }
        
         switch(p){
            
            case 1:
            y=0;
            data_servo=0x07;
            send_data(0x18,data_servo);
            //timer2.start();
            if(timer2.read()>1.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:
            /*
            if(triangle!=old_triangle){
                old_triangle=triangle;
                if(triangle==1){
                    if(y==0){
                        data_servo=0x08;
                        send_data(0x18,data_servo);
                        y=1;
                    }else if(y==1){
                        data_servo=0x07;
                        send_data(0x18,data_servo);
                        y=0;
                    }
                }
            }*/

            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_kaisyu_2(){
    if(q==0){
        if(old_migi1!=migi1){
            if(migi1==1){
                p=1;
            }
        }
       
       switch(p){
            case 1:
            data_servo=0x07;
            send_data(0x18,data_servo);
            timer.start();
            if(timer.read>1.2f){
                p=2;
                timer.stop();
                timer.reset();
            }
            break;
            
            case 2:
            data7=0x00;
            send_data(0x24,data7);
            if(get_data_rs232 & 0x10){
                timer.stop();
                timer.reset();
                data7=0x10;
                send_data(0x24,data7);
                p=3;
            }
            break();
            
            case 3:
            data_servo=0x08;
            send_data(0x18,data_servo);
            timer.start();
            if(timer.read()>1.5f()){
                timer.stop();
                timer.reset();
                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:
            break();
            
            
        } 
        
    }
}
*/



void hakidashi(){
    if(q==0){
        if(old_migi1!=migi1){
            old_migi1=migi1;
            if(migi1==1&&select==0){
                r=1;
                timer2.start();
            }
        }
        
        switch(r){
            case 1:
            data_servo=0x07;
            send_data(0x18,data_servo);
            //timer2.start();
            if(timer2.read()>1.5f){
                timer2.stop();
                timer2.reset();
                timer2.start();
                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);
            if(get_data_rs232 & 0x40){
                data11=0x10;
                send_data(0x30,data11);
                r=4;
            }
            break;
            
            case 4:
            data11=0x00;
            send_data(0x30,data11);
            if(get_data_rs232 & 0x80){
                data11=0x10;
                send_data(0x30,data11);
                r=5;
            }
            break;
            
            case 5:
            data7=0xff;
            send_data(0x24,data7);
            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=0xf3;
            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=0xf4;
            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();
}