2019NHK_teamA
/
NHK2019robokon_11_19
NHK2019 manual program(回収機構ストール問題解決)
main.cpp
- Committer:
- shina
- Date:
- 2019-06-25
- Revision:
- 0:c7e17c2fd542
- Child:
- 1:99294241f2ba
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); }