File content as of revision 1:99294241f2ba:

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


//変数
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=0x80;
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=2;
int q=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_hakidashi();
//void hakidashi();



//メイン関数
int main(){
    led=1;
    initialization();
    while(true){
        emergency();
        get_data();
        change_pwm();
        change_servo();
        change_data();
        change_rack();
        change_rack_2();
        sequence_kaisyu();
       //kaisyu();
       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;
    
    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);
    
    
    }    

    
//データ読み込み
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==0x20||get_data_rs232==0x22||get_data_rs232==0x24||get_data_rs232==0x32||get_data_rs232==0x34){
        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 change_data(){
    
        if(Ry!=0||Rx!=0){
        square=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){
        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){
        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(select!=old_select){
    old_select=select;
    if(select==1){
    if(i==1){
        pwm=0;
        i=0;
        }else if(i==0){
            pwm=1;
            i=1;
            }
        }
    }
}

//サーボモーター
void change_servo(){
        if(right2!=old_right2){
                    old_right2=right2;
                    //ハンガー機構
                    if(right2==1){
                        if(o==1){
                            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){
                        if(k==1){
                            data_servo=0x03;
                            //send_data(0x18,data_servo);
                            k=0;
                        }else if(k==0){
                            data_servo=0x04;
                            //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;
                        }
                    }
            }*/
     }
    
//ラック
void change_rack(){
        
        
        //右
        if(triangle==1&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)){
            if(get_data_rs232!=0x20&&get_data_rs232!=0x22){
                data5=0x00;
            }else if(get_data_rs232==0x20||get_data_rs232==0x22){
                data5=0x10;
                }
        }else if(triangle==0&&cross==1&&ue1==0&&shita==0&&(-32<Ly<32)){
            if(get_data_rs232!=0x26&&get_data_rs232!=0x28){
                data5=0xff;
            }else if(get_data_rs232==0x26||get_data_rs232==0x28){
                data5=0x10;
                }
        //左
        }else if(triangle==0&&cross==0&&ue1==1&&shita==0&&(-32<Ly<32)){
            if(get_data_rs232!=0x20&&get_data_rs232!=0x24){
                data6=0xff;
            }else if(get_data_rs232==0x20||get_data_rs232==0x24){
                data6=0x10;
                }
        }else if(triangle==0&&cross==0&&ue1==0&&shita==1&&(-32<Ly<32)){
            if(get_data_rs232!=0x26&&get_data_rs232!=0x30){
                data6=0x00;
            }else if(get_data_rs232==0x26||get_data_rs232==0x30){
                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==0x20){
            data5=0x10;
            data6=0x10;
        }else if(get_data_rs232==0x22){
            data5=0x10;
            data6=0xff;
        }else if(get_data_rs232==0x24){
            data5=0x00;
            data6=0x10;
        }else{
            data5=0x00;
            data6=0xff;
        }
    }else if(triangle==0&&cross==0&&ue1==0&&shita==0&&Ly<-32){
        if(get_data_rs232==0x26){
            data5=0x10;
            data6=0x10;
        }else if(get_data_rs232==0x28){
            data5=0x10;
            data6=0x00;
        }else if(get_data_rs232==0x30){
            data5=0xff;
            data6=0x10;
        }else{
            data5=0xff;
            data6=0x00;
        }
    }else if(triangle==0&&cross==0&&ue1==0&&shita==0&&(-32<Ly<32)){
        data5=0x80;
        data6=0x80;
    }
}

//回収機構

void sequence_kaisyu(){
     if(square!=old_square){
         old_square=square;
         if(square==1){
             
             data_servo=0x07;
             send_data(0x18,data_servo);
             wait(1.5);
             data7=0x00;
             while(get_data_rs232!=0x32){
             send_data(0x24,data7);
             get_data_rs232=slave.getc();
             }
             data7=0x10;
             send_data(0x24,data7);
             data_servo=0x08;
             send_data(0x18,data_servo);
             wait(2);
             data7=0xff;
             while(get_data_rs232!=0x34){
             send_data(0x24,data7);
             get_data_rs232=slave.getc();
             }
             data7=0x10;
             send_data(0x24,data7);
             data_servo=0x07;
             send_data(0x18,data_servo);

         }
      }
}



/*void kaisyu(){
    if(square!=old_square){
        if(square==1){
            p=1;
            data_servo=0x07;
            send_data(0x18,data_servo);
            wait(1.5);
            }
        }
        
        //get_data_rs232=slave.getc();
        
     if(p==1&&get_data_rs232_2!=0x32){
         data7=0x00;
         }else if(p==1&&get_data_rs232_2==0x32){
             data7=0x10;
             p=0;
             data_servo=0x08;
             send_data(0x18,data_servo);
             wait(1.5);
         }else if(p==0&&get_data_rs232!=0x34){
             data7=0xff;
         }else if(p==0&&get_data_rs232==0x34){
             data7=0x10;
             p=2;
         }
        
    }
*/


void sequence_hakidashi(){
     if(migi1!=old_migi1){
         old_migi1=migi1;
         if(migi1==1){
           /*  data_servo=0x07;
             send_data(0x18,data_servo);
             wait(1.5);
             data11=0xff;
             while(get_data_rs232!=0x36){
                 send_data(0x30,data11);
                 get_data_rs232=slave.getc();
                 }
              data11=0x10;
              send_data(0x30,data11);*/
              data11=0xff;
              send_data(0x30,data11);
             }
         }else if(hidari1!=old_hidari1){
             if(hidari1==1){
                 data11=0x00;
                 send_data(0x30,data11);
                 }
         }  /*   else if(hidari1==1){
                    if(get_data_rs232!=0x38){
                        data11=0x38;
                        send_data(0x30,data11);
                        }else if(get_data_rs232==0x38){
                            data11=0x10;
                            send_data(0x30,data11;
                        }
        }*/ /*else{
                 data11=0x80;
                 send_data(0x30,data11);
                 }*/
                 
            if(get_data_rs232==0x38&&data11==0x00){
                data11=0x10;
                send_data(0x30,data11);
            }  else if(get_data_rs232==0x36&&data11==0xff){
                data11=0x10;
                send_data(0x30,data11);
                
            }   
     }
  

/*
void hakidashi(){
    if(migi1==1&&hidari1==0){
        data_servo=0x07;
        if(get_data_rs232==0x36){
            data11=0x10;
        }else if(get_data_rs232!=0x38){
            data11=0xff;
            }
    }else if(migi1==0&&hidari1==1){
        data_servo=0x08;
        if(get_data_rs232==0x38){
            data11=0x10;
        }else if(get_data_rs232!=0x38){
            data11=0x00;
        }
    }else{
        data11=0x80;
    }   
}

*/

//ファン
void change_fan(){
    if(left2==1){
        data8=0xff;
        data9=0xff;
        data10=0xff;
        }else if(left2==0){
            data8=0x00;
            data9=0x00;
            data10=0x00;
            }
    }
  


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