File content as of revision 0:c7e17c2fd542:

/////////////////////////////////////
/*NHK2019*/
/*Aチーム手動機プログラム*/
/*担当者: 品川敦哉*/
/*
アドレス
0x10:右前
0x12:左前
0x14:右後ろ
0x16:左後ろ
0x18:サーボ制御回路
0x20:右ラック
0x22:左ラック
0x24:回収機構
0x26:右ファン
0x28:左ファン
*/
/////////////////////////////////////

//宣言
#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 get_data_rs232=0x80;
int Ry;
int Rx;
int right1;
int left1;
int left2;
int select;
int start;
int circle;
int cross;
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_cross=0;
int l=1;


//関数プロトタイプ宣言
void get_data();
void change_data();
void change_pwm();
void change_servo();
void change_rack();
void change_kaisyu();
void change_fan();
void send_data(char address,char data);
void emergency();
void get_limit_data();

//メイン関数
int main(){
    led=1;
    while(true){
        emergency();
        get_data();
        change_pwm();
        change_servo();
        change_data();
        change_rack();
        change_kaisyu();
        change_fan();
        get_limit_data();
        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);
        }
    }
    
//データ読み込み
void get_data(){
    Ry=ps3.getRightJoystickYaxis();
    Rx=ps3.getRightJoystickXaxis();
    circle=ps3.getButtonState(maru);
    cross=ps3.getButtonState(batu);
    left1=ps3.getButtonState(L1);
    left2=ps3.getButtonState(L2);
    right1=ps3.getButtonState(R1);
    select=ps3.getSELECTState();
    start=ps3.getSTARTState();
    ue1=ps3.getButtonState(ue);
    shita=ps3.getButtonState(sita);
    hidari1=ps3.getButtonState(hidari);
    migi1=ps3.getButtonState(migi);
    pc.printf("%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n",Ry,Rx,left1,left2,right1,select,start,ue1,shita,migi1,hidari1,tushin_check,i);
    if(Ry==0&&Rx==0&&left1==0&&right1==0&&left2==0&&select==0&&start==0&&shita==0&&ue1==0&&migi1==0&&hidari1==0){
        data_check=0;
        }else{
            data_check=1;
            }
    
    }
    
//緊急停止
void emergency(){
    if(start!=old_start){
    old_start=start;
    if(start==1){
        if(j==1){
    //緊急停止
            data_servo=0x00;
            send_data(0x18,data_servo);
            led=0;
            stop=1;
            j=0;
        }else if(j==0){
    //緊急停止解除
            led=1;
            stop=0;
            j=1;
            }
        }
    }  
}

    
//データ変化(メカナム)
void change_data(){
    if(Ry>30&&i==0){
        data1=0xff;
        data2=0x00;
        data3=0xff;
        data4=0x00;
        }else if(Ry<-30&&i==0){
        data1=0x00;
        data2=0xff;
        data3=0x00;
        data4=0xff;
        }else if(Rx>30&&i==0){
        data1=0xff;
        data2=0xff;
        data3=0x00;
        data4=0x00;
        }else if(Rx<-30&&i==0){
        data1=0x00;
        data2=0x00;
        data3=0xff;
        data4=0xff;
        }else if(right1==1&&i==0){
        data1=0x00;
        data2=0x00;
        data3=0x00;
        data4=0x00;
        }else if(left1==1&&i==0){
        data1=0xff;
        data2=0xff;
        data3=0xff;
        data4=0xff;
        }else if(Ry>30&&i==1){
        data1=0xbf;
        data2=0x3f;
        data3=0xbf;
        data4=0x3f;
        }else if(Ry<-30&&i==1){
        data1=0x3f;
        data2=0xbf;
        data3=0x3f;
        data4=0xbf;
        }else if(Rx>30&&i==1){
        data1=0xbf;
        data2=0xbf;
        data3=0x3f;
        data4=0x3f;
        }else if(Rx<-30&&i==1){
        data1=0x3f;
        data2=0x3f;
        data3=0xbf;
        data4=0xbf;
        }else if(right1==1&&i==1){
        data1=0x3f;
        data2=0x3f;
        data3=0x3f;
        data4=0x3f;
        }else if(left1==1&&i==1){
        data1=0xbf;
        data2=0xbf;
        data3=0xbf;
        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(j!=0){
        if(circle!=old_circle){
                    if(circle==1){
                        if(k==1){
                            data_servo=0x01;
                            k=0;
                        }else if(k==0){
                            data_servo=0x02;
                            k=1;
                        }
                    }
            }else if(cross!=old_cross){
                    if(cross==1){
                        if(l==1){
                            data_servo=0x03;
                            l=0;
                        }else if(l==0){
                            data_servo=0x04;
                            l=1;   
                        }
                    }
            }
        }
     }
    
//ラック
void change_rack(){
    if(ue1==1&&shita==0){
        data5=0x00;
        data6=0x00;
        }else if(ue1==0&&shita==1){
            data5=0xff;
            data6=0xff;
            }else{
                data5=0x80;
                data6=0x80;
                }
    }

//回収機構
void change_kaisyu(){
    if(migi1==1&&hidari1==0){
        data7=0xff;
        }else if(migi1==0&&hidari1==1){
            data7=0x00;
            }else{
                data7=0x80;
                }
    }
    
//ファン
void change_fan(){
    if(left2==1){
        data8=0xff;
        data9=0xff;
        }else if(left2==0){
            data8=0x00;
            data9=0x00;
            }
    }
  
//リミットスイッチデータ取得  
void get_limit_data(){
    get_data_rs232=slave.getc();
        if(get_data_rs232==0x20){
              if(ue1==1){
                data5=0x80;
                data6=0x80;
                }
        }else if(get_data_rs232==0x22){
            if(ue1==1){
                data5=0x80;
                }
        }else if(get_data_rs232==0x24){
            if(ue1==1){
                data6=0x80;
                }
        }else if(get_data_rs232==0x26){
            if(shita==1){
                data5=0x80;
                data6=0x80;
                }
        }else if(get_data_rs232==0x28){
            if(shita==1){
                data5=0x80;
                }
        }else if(get_data_rs232==0x30){
            if(shita==1){
                data6=0x80;
                }
        }else if(get_data_rs232==0x32){
            if(migi1==1){
                data7=0x80;
                }
        }else if(get_data_rs232==0x34){
            if(hidari1==1){
                data7=0x80;
                }
        }
    }
    
//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);
    }