File content as of revision 0:f99e0ea93e31:

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





//関数プロトタイプ宣言
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 sequence_hakidashi();
//void hakidashi();

//メイン関数
int main(){
    led=1;
    data7=0x80;
    while(true){
        emergency();
        get_data();
        change_pwm();
        change_servo();
        change_data();
        change_rack();
        change_rack_2();
        sequence_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(0x26,data8);
        send_data(0x28,data9);
        send_data(0x40,data10);
        
        }
    }
    
//データ読み込み
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>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){
        data1=0x00;
        data2=0xff;
        data3=0xff;
        data4=0x00;
        }else if(Rx<-30&&i==0){
        data1=0xff;
        data2=0x00;
        data3=0x00;
        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){
        data1=0x3f;
        data2=0xbf;
        data3=0xbf;
        data4=0x3f;
        }else if(Rx<-30&&i==1){
        data1=0xbf;
        data2=0x3f;
        data3=0x3f;
        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;
                            o=0;
                        }else if(o==0){
                            data_servo=0x02;
                            o=1;
                        }
                    }
            }else if(circle!=old_circle){
                    old_circle=circle;
                    //バスタオル挟む機構
                    if(circle==1){
                        if(k==1){
                            data_servo=0x03;
                            k=0;
                        }else if(k==0){
                            data_servo=0x04;
                            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(2);
             data7=0xff;
             send_data(0x24,data7);
             wait(4);
             data7=0x10;
             send_data(0x24,data7);
             data_servo=0x08;
             send_data(0x18,data_servo);
             wait(2);
             data7=0x00;
             send_data(0x24,data7);
             wait(4);
             data7=0x10;
             send_data(0x24,data7);*/
             
             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);

         }
      }
}
/*
void sequence_hakidashi(){
     if(migi1!=old_migi1){
         old_migi1=migi1;
         if(migi1==1){
             data_servo=0x07;
             send_data(0x18,data_servo);
             wait(2);
             data11=0xff;
             while(get_data_rs232!=0x36){
                 send_data(0x30,data11);
                 get_data_rs232=slave.getc();
                 }
              data11=0x10;
              send_data(0x30,data11);
              wait(0.5);
              data11=0x00;
              while(get_data_rs232!=0x38){
                  send_data(0x30,data11);
                  get_data_rs232=slave.getc();
                  }
              data11=0x10;
              send_data(0x30,data11);
              data_servo=0x08;
              send_data(0x18,data_servo);
              wait(2);     
             }
         }
     }
*/  

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