2019NHK_teamA
/
NHK2019robokon_11_19
NHK2019 manual program(回収機構ストール問題解決)
Diff: main.cpp
- Revision:
- 1:99294241f2ba
- Parent:
- 0:c7e17c2fd542
- Child:
- 2:e462c8257384
--- a/main.cpp Tue Jun 25 08:53:27 2019 +0000 +++ b/main.cpp Thu Sep 05 09:40:50 2019 +0000 @@ -1,19 +1,19 @@ ///////////////////////////////////// /*NHK2019*/ -/*Aチーム手動機プログラム*/ -/*担当者: 品川敦哉*/ +//Aチーム手動機プログラム /* アドレス 0x10:右前 -0x12:左前 +0x12:左後ろ 0x14:右後ろ -0x16:左後ろ +0x16:左前 0x18:サーボ制御回路 0x20:右ラック 0x22:左ラック 0x24:回収機構 0x26:右ファン 0x28:左ファン +0x30:吐き出し機構 */ ///////////////////////////////////// @@ -31,6 +31,7 @@ DigitalOut stop(D10); DigitalOut led1(LED1); + //変数 char data1;//右上 char data2;//左上 @@ -42,16 +43,22 @@ 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; @@ -63,25 +70,42 @@ int j=1; int old_circle=0; int k=1; -int old_cross=0; -int l=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_kaisyu(); void change_fan(); void send_data(char address,char data); void emergency(); -void get_limit_data(); +void change_rack_2(); +void sequence_kaisyu(); +//void kaisyu(); +void sequence_hakidashi(); +//void hakidashi(); + + //メイン関数 int main(){ led=1; + initialization(); while(true){ emergency(); get_data(); @@ -89,9 +113,12 @@ change_servo(); change_data(); change_rack(); - change_kaisyu(); + change_rack_2(); + sequence_kaisyu(); + //kaisyu(); + sequence_hakidashi(); + //hakidashi(); change_fan(); - get_limit_data(); send_data(0x10,data1); send_data(0x12,data2); send_data(0x14,data3); @@ -99,29 +126,78 @@ send_data(0x18,data_servo); send_data(0x20,data5); send_data(0x22,data6); - send_data(0x24,data7); + //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); - 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){ + 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; @@ -136,8 +212,6 @@ if(start==1){ if(j==1){ //緊急停止 - data_servo=0x00; - send_data(0x18,data_servo); led=0; stop=1; j=0; @@ -154,65 +228,142 @@ //データ変化(メカナム) void change_data(){ + + if(Ry!=0||Rx!=0){ + square=0; + } + if(Ry>30&&i==0){ - data1=0xff; - data2=0x00; - data3=0xff; - data4=0x00; + 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=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(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=0x3f; + data2=0xbf; data3=0xbf; - data4=0x3f; - }else if(Ry<-30&&i==1){ - data1=0x3f; - data2=0xbf; - data3=0x3f; 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=0x3f; + data1=0xbf; data2=0x3f; - data3=0x3f; + data3=0xbf; data4=0x3f; }else if(left1==1&&i==1){ - data1=0xbf; + data1=0x3f; data2=0xbf; - data3=0xbf; + data3=0x3f; data4=0xbf; }else{ data1=0x80; @@ -222,6 +373,7 @@ } } + //pwm変化 void change_pwm(){ if(select!=old_select){ @@ -240,106 +392,275 @@ //サーボモーター void change_servo(){ - if(j!=0){ - if(circle!=old_circle){ + 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=0x01; + data_servo=0x03; + //send_data(0x18,data_servo); k=0; }else if(k==0){ - data_servo=0x02; - k=1; + data_servo=0x04; + //send_data(0x18,data_servo); + 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; + }/*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(ue1==1&&shita==0){ - data5=0x00; - data6=0x00; - }else if(ue1==0&&shita==1){ - data5=0xff; + + + //右 + 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{ - data5=0x80; - data6=0x80; - } + }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 change_kaisyu(){ + +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){ - data7=0xff; - }else if(migi1==0&&hidari1==1){ - data7=0x00; - }else{ - data7=0x80; - } - } - + 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; } } -//リミットスイッチデータ取得 -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){ @@ -351,4 +672,5 @@ tushin_check=i2c.write(data); i2c.stop(); wait(0.003); - } \ No newline at end of file + } +