2019NHK_teamA
NHK 2019 team A manual machine program. 改良版(回収動作がおかしい)
Dependencies: mbed 2019ROBOKONmanualProgram
Diff: main.cpp
- Revision:
- 1:99294241f2ba
- Parent:
- 0:c7e17c2fd542
- Child:
- 2:830db42bae7e
--- 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
+ }
+