2019NHK_teamA
NHK2019 manual program(回収機構ストール問題解決)
Diff: main.cpp
- Revision:
- 4:4d9c0999a90a
- Parent:
- 3:8c4e42cba9cb
- Child:
- 5:29971215ef50
--- a/main.cpp Thu Sep 12 09:03:59 2019 +0000
+++ b/main.cpp Mon Oct 21 07:51:27 2019 +0000
@@ -9,7 +9,7 @@
0x16:左前
0x18:サーボ制御回路
0x20:右ラック
-0x22:左ラック
+0x22:左ラック.
0x24:回収機構
0x26:右ファン
0x28:左ファン
@@ -30,7 +30,8 @@
DigitalOut pwm(D4);
DigitalOut stop(D10);
DigitalOut led1(LED1);
-
+Timer timer;
+Timer timer2;
//変数
char data1;//右上
@@ -78,13 +79,17 @@
int o=1;
int old_right1=0;
int old_left1=0;
-int p=2;
+int p=5;
int q=0;
int old_ue1=0;
int old_shita=0;
int old_left2=0;
int old_triangle=0;
int old_cross=0;
+int r=6;
+int s=5;
+bool x=0;
+bool y=0;
//関数プロトタイプ宣言
@@ -93,15 +98,14 @@
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();
+//void sequence_kaisyu();
+void kaisyu();
+//void sequence_kaisyu_2();
+void hakidashi();
void mode_change();
@@ -117,20 +121,19 @@
get_data();
mode_change();
change_pwm();
+ change_data();
change_servo();
- change_data();
- //change_rack();
change_rack_2();
- sequence_kaisyu();
- //kaisyu();
- sequence_hakidashi();
- //hakidashi();
+ //sequence_kaisyu();
+ kaisyu();
+ //sequence_kaisyu_2();
+ 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(0x18,data_servo);
send_data(0x20,data5);
send_data(0x22,data6);
//send_data(0x24,data7);
@@ -160,6 +163,7 @@
data9=0x80;
data10=0x80;
data11=0x80;
+ data_servo=0x00;
send_data(0x10,data1);
send_data(0x12,data2);
@@ -172,6 +176,8 @@
send_data(0x28,data9);
send_data(0x40,data10);
send_data(0x30,data11);
+ send_data(0x18,data_servo);
+
data10=0x3f;
send_data(0x40,data10);
@@ -184,7 +190,11 @@
send_data(0x40,data10);
}
-
+ /*data_servo=0x03;
+ send_data(0x18,data_servo);
+ data_servo=0x04;
+ send_data(0x18,data_servo);*/
+ timer2.reset();
}
@@ -210,6 +220,7 @@
hidari1=ps3.getButtonState(hidari);
migi1=ps3.getButtonState(migi);
get_data_rs232=slave.getc();
+
if(get_data_rs232) {
@@ -217,7 +228,9 @@
} 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);
+ //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);
+ //pc.printf("0x%x\n",get_data_rs232);
+ //pc.printf("%d\n",x);
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 {
@@ -256,6 +269,10 @@
if(q==0){
//送風モード
q=1;
+ /* data_servo=0x03;
+ send_data(0x18,data_servo);
+ data_servo=0x04;
+ send_data(0x18,data_servo);*/
data10=0xcf;
send_data(0x40,data10);
}else if(q==1){
@@ -468,88 +485,53 @@
{
if(right2!=old_right2) {
old_right2=right2;
- //ハンガー機構
+ //ハンガー機構&バスタオル機構initialize処理
if(right2==1) {
if(o==1) {
data_servo=0x01;
- //send_data(0x18,data_servo);
+ send_data(0x18,data_servo);
o=2;
} else if(o==2) {
data_servo=0x02;
- //send_data(0x18,data_servo);
+ send_data(0x18,data_servo);
o=3;
}
- //data_servo=0x01;
}
- } else if(circle!=old_circle) {
+ }
+
+ if(circle!=old_circle) {
old_circle=circle;
//バスタオル挟む機構
if(circle==1) {
if(k==1) {
- data_servo=0x03;
- //send_data(0x18,data_servo);
+ data_servo=0xf3;
+ send_data(0x18,data_servo);
k=0;
} else if(k==0) {
- data_servo=0x04;
- //send_data(0x18,data_servo);
+ data_servo=0xf4;
+ 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;
- }
- }
- }*/
+ }
+ /*
+ if(triangle!=triangle){
+ old_triangle=triangle;
+ if(triangle==1){
+ if(y==0){
+ data_servo=0x08;
+ send_data(0x18,data_servo);
+ y=1;
+ }else if(y==1){
+ data_servo=0x07;
+ send_data(0x18,data_servo);
+ y=0;
+ }
+ }
+ } */
}
-//ラック
-/*
-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()
@@ -585,13 +567,18 @@
if(ue1!=old_ue1){
old_ue1=ue1;
- if(ue1==1){
+ if(ue1==1&&start==0){
data5=0x00;
data6=0xff;
+
+ /*data_servo=0x03;
+ send_data(0x18,data_servo);
+ data_servo=0x04;
+ send_data(0x18,data_servo);*/
}
}else if(shita!=old_shita){
old_shita=shita;
- if(shita==1){
+ if(shita==1&&start==0){
data5=0xff;
data6=0x00;
}
@@ -617,163 +604,261 @@
}
//回収機構
-
+/*
void sequence_kaisyu()
{
if(q==0){
if(square!=old_square) {
old_square=square;
- if(square==1) {
+ if(square==1&&select==0) {
+ if(x==0){
+ data_servo=0x07;
+ send_data(0x18,data_servo);
+ wait(1.2);
+ data7=0x00;
+ get_data_rs232=get_data_rs232 & 0b11101111;
+ 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(1.2);
+ data7=0xff;
+ get_data_rs232=get_data_rs232 & 0b11011111;
+ while(!(get_data_rs232 & 0x20)){
+ send_data(0x24,data7);
+ get_data_rs232=slave.getc();
+ }
+ data7=0x10;
+ send_data(0x24,data7);
+ x=0;
+ }
+ }
+ }
+ }
+}
+*/
+//回収機構switch文version
+void kaisyu(){
+ if(q==0){
+ if(old_square!=square){
+ old_square=square;
+ if(square==1){
+ if(x==0){
+ p=1;
+ x=1;
+ }else if(x==1){
+ p=4;
+ x=0;
+ }
+ }
+ }
+
+ switch(p){
+
+ case 1:
+ data_servo=0x07;
+ send_data(0x18,data_servo);
+ timer2.start();
+ if(timer2.read()>1.2f){
+ timer2.stop();
+ timer2.reset();
+ p=2;
+ }
+ break;
+
+ case 2:
+ data7=0x00;
+ send_data(0x24,data7);
+ if(get_data_rs232 & 0x10){
+ data7=0x10;
+ send_data(0x24,data7);
+ p=3;
+ }
+ break;
+
+ case 3:
+ if(triangle!=old_triangle){
+ old_triangle=triangle;
+ if(triangle==1){
+ if(y==0){
+ data_servo=0x08;
+ send_data(0x18,data_servo);
+ y=1;
+ }else if(y==1){
+ data_servo=0x07;
+ send_data(0x18,data_servo);
+ y=0;
+ }
+ }
+ }
+
+ break;
+
+ case 4:
+ data7=0xff;
+ send_data(0x24,data7);
+ if(get_data_rs232 & 0x20){
+ data7=0x10;
+ send_data(0x24,data7);
+ p=5;
+ }
+ break;
+
+ default:
+ p=5;
+ break;
+
+ }
+ }
+}
+
+/*
+void sequence_kaisyu_2(){
+ if(q==0){
+ if(old_migi1!=migi1){
+ if(migi1==1){
+ p=1;
+ }
+ }
+
+ switch(p){
+ case 1:
data_servo=0x07;
send_data(0x18,data_servo);
- wait(1.5);
+ timer.start();
+ if(timer.read>1.2f){
+ p=2;
+ timer.stop();
+ timer.reset();
+ }
+ break;
+
+ case 2:
data7=0x00;
- while(!(get_data_rs232 & 0x10)) {
+ send_data(0x24,data7);
+ if(get_data_rs232 & 0x10){
+ timer.stop();
+ timer.reset();
+ data7=0x10;
send_data(0x24,data7);
- get_data_rs232=slave.getc();
+ p=3;
}
- data7=0x10;
- send_data(0x24,data7);
+ break();
+
+ case 3:
data_servo=0x08;
send_data(0x18,data_servo);
- wait(2);
+ timer.start();
+ if(timer.read()>1.5f()){
+ timer.stop();
+ timer.reset();
+ p=4;
+ }
+ break();
+
+ case 4:
data7=0xff;
- while(!(get_data_rs232 & 0x20)) {
+ send_data(0x24,data7);
+ if(get_data_rs232 & 0x20){
+ data7=0x10;
send_data(0x24,data7);
- get_data_rs232=slave.getc();
+ p=5;
+ }
+ break;
+
+ default:
+ break();
+
+
+ }
+
+ }
+}
+*/
+
+
+
+void hakidashi(){
+ if(q==0){
+ if(old_migi1!=migi1){
+ old_migi1=migi1;
+ if(migi1==1&&select==0){
+ r=1;
+ }
+ }
+
+ switch(r){
+ case 1:
+ data_servo=0x07;
+ send_data(0x18,data_servo);
+ timer2.start();
+ if(timer2.read()>1.5f){
+ timer2.stop();
+ timer2.reset();
+ r=2;
}
- data7=0x10;
+ break;
+
+ case 2:
+ data7=0x00;
send_data(0x24,data7);
-
+ timer2.start();
+ if(timer2.read()>0.3f){
+ timer2.stop();
+ timer2.reset();
+ data7=0x10;
+ send_data(0x24,data7);
+ r=3;
+ }
+ break;
+
+ case 3:
+ data11=0xff;
+ send_data(0x30,data11);
+ //get_data_rs232=get_data_rs232 & 0b10111111;
+ if(get_data_rs232 & 0x40){
+ data11=0x10;
+ send_data(0x30,data11);
+ r=4;
}
+ break;
+
+ case 4:
+ data11=0x00;
+ send_data(0x30,data11);
+ //get_data_rs232=get_data_rs232 & 0b01111111;
+ if(get_data_rs232 & 0x80){
+ data11=0x10;
+ send_data(0x30,data11);
+ r=5;
+ }
+ break;
+
+ case 5:
+ data7=0xff;
+ send_data(0x24,data7);
+ //get_data_rs232=get_data_rs232 & 0b11011111;
+ if(get_data_rs232 & 0x20){
+ data7=0x10;
+ send_data(0x24,data7);
+ r=6;
+ }
+ break;
+
+ default:
+ r=6;
+ break;
}
}
}
-/*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(q==0){
- if(migi1!=old_migi1) {
- old_migi1=migi1;
- if(migi1==1&&select==0) {
- data_servo=0x07;
- send_data(0x18,data_servo);
- wait(1);
- data7=0x00;
- send_data(0x24,data7);
- wait(0.5);
- data7=0x10;
- send_data(0x24,data7);
- data11=0xff;
- while(!(get_data_rs232 & 0x40)){
- send_data(0x30,data11);
- get_data_rs232=slave.getc();
- }
- data11=0x10;
- send_data(0x38,data11);
- data11=0x00;
- while(!(get_data_rs232 & 0x80)){
- send_data(0x30,data11);
- get_data_rs232=slave.getc();
- }
- data11=0x10;
- send_data(0x30,data11);
- data7=0xff;
- while(!(get_data_rs232 & 0x20)){
- send_data(0x24,data7);
- get_data_rs232=slave.getc();
- }
- data7=0x10;
- send_data(0x24,data7);
-
- }
- } /*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()
{
@@ -786,17 +871,17 @@
data8=0x00;
data9=0x00;
data10=0x00;
- }else if(left2==0&&(old_triangle!=triangle)&&select==0){
+ }else if(left2==0&&(old_triangle!=triangle)&&select==0&&(get_data_rs232 & 0b00000011)){
old_triangle=triangle;
if(triangle==1){
data8=0xff;
data9=0xff;
data10=0xff;
- data_servo=0x03;
+ data_servo=0xf3;
send_data(0x26,data8);
send_data(0x28,data9);
send_data(0x40,data10);
- wait(1);
+ wait(0.5);
send_data(0x18,data_servo);
wait(1);
data8=0x00;
@@ -806,7 +891,7 @@
send_data(0x28,data9);
send_data(0x40,data10);
wait(0.5);
- data_servo=0x04;
+ data_servo=0xf4;
send_data(0x18,data_servo);
}
}
@@ -814,9 +899,6 @@
}
-
-
-
//i2c
void send_data(char address,char data)
{
@@ -828,3 +910,4 @@
i2c.stop();
}
+