2019NHK_teamA
NHK 2019 manual program 11_18（送風シーケンスバグ解決ver）
Diff: main.cpp
- Revision:
- 5:29971215ef50
- Parent:
- 4:4d9c0999a90a
- Child:
- 7:c423a6579c3c
--- a/main.cpp Mon Oct 21 07:51:27 2019 +0000
+++ b/main.cpp Sat Oct 26 04:30:54 2019 +0000
@@ -30,6 +30,7 @@
DigitalOut pwm(D4);
DigitalOut stop(D10);
DigitalOut led1(LED1);
+//DigitalOut aaaaa(D9);
Timer timer;
Timer timer2;
@@ -50,20 +51,20 @@
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;
+bool right1;
+bool right2;
+bool left1;
+bool left2;
+bool select;
+bool start;
+bool circle;
+bool cross;
+bool triangle;
+bool square;
+bool ue1;
+bool shita;
+bool migi1;
+bool hidari1;
int tushin_check;
int old_select=0;
int i=1;
@@ -90,6 +91,9 @@
int s=5;
bool x=0;
bool y=0;
+bool kaisyu_arm=0;
+char get_data_ps3[8];
+
//関数プロトタイプ宣言
@@ -112,11 +116,18 @@
//メイン関数
int main()
-{
+{ //pc.printf("hi\n");
led=1;
pwm=1;
+ i2c.frequency(100000);
+ slave.baud(115200);
+ pc.baud(115200);
initialization();
+
+ //pc.printf("ha\n");
while(true) {
+ //pc.printf("hello\n");
+ //aaaaa=1;
emergency();
get_data();
mode_change();
@@ -142,8 +153,8 @@
send_data(0x40,data10);
//send_data(0x30,data11);
-
-
+ //aaaaa=0;
+ //pc.printf("world\n");
}
}
@@ -194,7 +205,7 @@
send_data(0x18,data_servo);
data_servo=0x04;
send_data(0x18,data_servo);*/
- timer2.reset();
+ //timer2.reset();
}
@@ -202,6 +213,9 @@
//データ読み込み
void get_data()
{
+ int w;
+ //int switch_judge=0;
+ /*
Ry=ps3.getRightJoystickYaxis();
Rx=ps3.getRightJoystickXaxis();
Ly=ps3.getLeftJoystickYaxis();
@@ -219,8 +233,75 @@
shita=ps3.getButtonState(sita);
hidari1=ps3.getButtonState(hidari);
migi1=ps3.getButtonState(migi);
+ */
+ for(w=0;w<8;w++){
+ get_data_ps3[w]=ps3.PS3Data[w];
+ }
+ // get_data_ps3[0]=(*(+(button>>4)) >> (button & 0x0f)) & 1;
+
+ switch(get_data_ps3[2] & 0x0c){
+ case 0x0c:
+ select=true;
+ migi1=false;
+ hidari1=false;
+ break;
+ case 0x04:
+ select=false;
+ migi1=true;
+ hidari1=false;
+ break;
+ case 0x08:
+ select=false;
+ migi1=false;
+ hidari1=true;
+ break;
+ default:
+ select=false;
+ migi1=false;
+ hidari1=false;
+ }
+
+ switch(get_data_ps3[2] & 0x03){
+ case 0x03:
+ start=true;
+ ue1=false;
+ shita=false;
+ break;
+ case 0x01:
+ start=false;
+ ue1=true;
+ shita=false;
+ break;
+ case 0x02:
+ start=false;
+ ue1=false;
+ shita=true;
+ break;
+ default:
+ start=false;
+ ue1=false;
+ shita=false;
+ }
+
+ triangle=bool(get_data_ps3[2] & 0x10);
+ cross=bool(get_data_ps3[2] & 0x20);
+ circle=bool(get_data_ps3[2] & 0x40);
+ square=bool(get_data_ps3[1] & 0x01);
+ left1=bool(get_data_ps3[1] & 0x02);
+ left2=bool(get_data_ps3[1] & 0x04);
+ right1=bool(get_data_ps3[1] & 0x08);
+ right2=bool(get_data_ps3[1] & 0x10);
+ Ry=(int)get_data_ps3[6]*-1+64;
+ Rx=(int)get_data_ps3[5]-64;
+
+
+ //pc.printf("a\n");
+ if(slave.readable()==1){
+ //pc.printf("c\n");
get_data_rs232=slave.getc();
-
+ //pc.printf("d\n");
+ }
+ //pc.printf("b\n");
if(get_data_rs232) {
@@ -231,6 +312,7 @@
//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);
+ //pc.printf("%d,%d,%d\n",x,y,p);
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 {
@@ -491,6 +573,7 @@
data_servo=0x01;
send_data(0x18,data_servo);
o=2;
+ kaisyu_arm=1;
} else if(o==2) {
data_servo=0x02;
send_data(0x18,data_servo);
@@ -514,8 +597,9 @@
}
}
}
- /*
- if(triangle!=triangle){
+
+ if(q==0){
+ if(triangle!=old_triangle){
old_triangle=triangle;
if(triangle==1){
if(y==0){
@@ -528,7 +612,8 @@
y=0;
}
}
- } */
+ }
+ }
}
@@ -645,12 +730,14 @@
//回収機構switch文version
void kaisyu(){
if(q==0){
+ if(kaisyu_arm!=0){
if(old_square!=square){
old_square=square;
if(square==1){
if(x==0){
p=1;
x=1;
+ timer2.start();
}else if(x==1){
p=4;
x=0;
@@ -661,10 +748,11 @@
switch(p){
case 1:
+ y=0;
data_servo=0x07;
send_data(0x18,data_servo);
- timer2.start();
- if(timer2.read()>1.2f){
+ //timer2.start();
+ if(timer2.read()>1.5f){
timer2.stop();
timer2.reset();
p=2;
@@ -682,6 +770,7 @@
break;
case 3:
+ /*
if(triangle!=old_triangle){
old_triangle=triangle;
if(triangle==1){
@@ -695,7 +784,7 @@
y=0;
}
}
- }
+ }*/
break;
@@ -714,6 +803,7 @@
break;
}
+ }
}
}
@@ -789,6 +879,7 @@
old_migi1=migi1;
if(migi1==1&&select==0){
r=1;
+ timer2.start();
}
}
@@ -796,10 +887,11 @@
case 1:
data_servo=0x07;
send_data(0x18,data_servo);
- timer2.start();
+ //timer2.start();
if(timer2.read()>1.5f){
timer2.stop();
timer2.reset();
+ timer2.start();
r=2;
}
break;
@@ -807,7 +899,7 @@
case 2:
data7=0x00;
send_data(0x24,data7);
- timer2.start();
+ //timer2.start();
if(timer2.read()>0.3f){
timer2.stop();
timer2.reset();
@@ -820,7 +912,6 @@
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);
@@ -831,7 +922,6 @@
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);
@@ -842,7 +932,6 @@
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);