2019NHK_teamA
/
NHK2019robokon_11_19
NHK2019 manual program(回収機構ストール問題解決)
main.cpp
- Committer:
- yosino_adati
- Date:
- 2019-09-05
- Revision:
- 2:e462c8257384
- Parent:
- 1:99294241f2ba
- Child:
- 3:8c4e42cba9cb
File content as of revision 2:e462c8257384:
///////////////////////////////////// /*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=0x00; 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) { 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 & 0x01)) { data5=0x00; } else if(get_data_rs232 & 0x01) { data5=0x10; } } else if(triangle==0&&cross==1&&ue1==0&&shita==0&&(-32<Ly<32)) { if(!(get_data_rs232 & 0x04)) { data5=0xff; } else if(get_data_rs232 & 0x04) { data5=0x10; } //左 } else if(triangle==0&&cross==0&&ue1==1&&shita==0&&(-32<Ly<32)) { if(!(get_data_rs232 & 0x02)) { data6=0xff; } else if(get_data_rs232 & 0x02) { data6=0x10; } } else if(triangle==0&&cross==0&&ue1==0&&shita==1&&(-32<Ly<32)) { if(!(get_data_rs232 & 0x08)) { data6=0x00; } else if(get_data_rs232 & 0x08) { 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 & 0x01) { data5=0x10; } else { data5=0x00; } if(get_data_rs232 & 0x02) { data6=0x10; } else { data6=0xff; } } else if(triangle==0&&cross==0&&ue1==0&&shita==0&&Ly<-32) { if(get_data_rs232 & 0x04) { data5=0x10; } else { data5=0xff; } if(get_data_rs232 & 0x08) { data6=0x10; } else { 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 & 0x10)) { 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 & 0x20)) { 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 & 0x80)&&(data11==0x00)) { data11=0x10; send_data(0x30,data11); } else if((get_data_rs232 & 0x40)&&(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); }