2015 robotic contest arakawa A
/
Nucleo_PS3_Jikken_XBOX
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
Fork of
Nucleo_PS3_Jikken_PS3
by 
2015 robotic contest arakawa A
Revision 20:373fa71ce05b, committed 2015-10-16
- Comitter:
- nodoame
- Date:
- Fri Oct 16 09:32:02 2015 +0000
- Parent:
- 19:cd036cc40a2d
- Child:
- 22:ab619079fb8d
- Commit message:
- ??????????
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Oct 13 07:40:34 2015 +0000
+++ b/main.cpp Fri Oct 16 09:32:02 2015 +0000
@@ -9,19 +9,21 @@
#define btSph 5
#define btCro 6
#define btSqa 7
-#define presUp 0
-#define presRight 1
-#define presDown 2
-#define presLeft 3
+#define btSel 0
+#define presUp 4
+#define presRight 5
+#define presDown 6
+#define presLeft 7
#define devideNum 32
#define ED 132
#define TD 144//208
#define EL 134
#define TL 200
#define TNR 138//204
-#define TNL 135
+#define TNL 150
#define debugFlag 1
#define stopCount 33
+#define shotWait 0.2
Serial pc(SERIAL_TX, SERIAL_RX);
//Serial Dev(D8,D2);//コントローラー
@@ -29,7 +31,8 @@
RawSerial Mechanum(D8,D2);//メカナム
I2C i2c(D14,D15);//sda,scl
//const int airAddr=0x20;
-const int airAddr=0xA0;
+const int airAddr=0xd0;
+const int airReg=10;
const int valveAddr=0xC0;
BusOut dI2c(PC_0,PC_1,PC_2,PC_3);//i2cデバッグ
@@ -75,7 +78,7 @@
return atan2(-(y-128),x-128)/M_PI*180;//y軸反転、/PI*180でRadからDeg変換
}
-double devidePoint(double y,double x,int threshold,float devide)
+int devidePoint(double y,double x,int threshold,float devide)
{
double tempX,tempY,dev;//仮のX,Yと分けた角度
tempX=(x-128);tempY=(y-128);//それぞれ-128
@@ -133,43 +136,60 @@
return val;
}
-void detectPole(int scVector)
+bool detectPole(int scVector)
{
int vector=TD,laser=0;
- bool btFlag=false;
+ bool btFlag=false,canFlag=false,breakFlag=false,stopFlag=false;
Blue=0.0f;Green=0.0f,Red=0.8f;
while(true)
- {
+ {
+ if((getBt(btTri)&&getBt(btSph)&&canFlag)||breakFlag)
+ return breakFlag;
+ else if(!(getBt(btTri)&&getBt(btSph)))
+ canFlag=true;
+
//スイッチが押された時
- if((int)DATA[1]&12==12&&!btFlag){
+ if(((int)DATA[1]&12)==12&&!btFlag){
wait(0.001f);
Mechanum.putc(64);
btFlag=true;
- Blue=0.0f;Green=0.8f;Red=0;
+ Blue=0;Green=0.8;Red=0;
vector=scVector;
}
//片方のレーザーが反応したとき
- if(btFlag){
- if(!(int)DATA[1]&2){
+ if(btFlag&&!laser){
+ if(!(((int)DATA[1]&2)==2)){
laser=1;
+ Red=0.8;
vector=0;
}
- else if((int)DATA[1]&1){
+ else if(!(((int)DATA[1]&1)==1)){
laser=2;
+ Red=0.8;
vector=0;
}
}
//もう片方のレーザーが反応した時
- else if(!(int)DATA[1]&laser&&laser){
- Mechanum.putc(64);
- break;
+ else if(!(((int)DATA[1]&laser)==laser)&&laser&&!stopFlag){
+ Green=0;Red=0.8;Blue=0.8;
+ vector=64;
+ stopFlag=true;
+ wait(0.05);
+ }
+ //ストップ後
+ else if(stopFlag)
+ {
+ vector=162;
+ breakFlag=true;
}
//pc.printf("%d\r\n",DATA[1]);
+ //deg=devidePoint((double)DATA[3],(double)DATA[2],15,360/devideNum);
+ //pc.printf("%d\r\n",scVector);
Mechanum.putc(vector);
- wait(0.001f);
+ wait(0.0126f);
}
btFlag=false;
Blue=0;Green=0;Red=0;
@@ -212,93 +232,52 @@
double gValue=0,bValue=0,rValue=0;
DigitalOut l(PB_2);
int sqf=0;
- double deg;
+ double deg=32;
int val;
i2c.frequency(400000);
setFCLED(0,0.8,0.8);
- bool obonFlag=false,pLeftF=false,pRightF=false,btCrF=false,pUpF=false,pDownF=false,btLF=false,btRF=false,btTrF=false;
+ bool obonFlag=false,pLeftF=false,pRightF=false,btCrF=false,detFinF=false;
while(true)
{
//ポール検出モード
if(getBt(btTri)&&getBt(btSph)){
- if(deg<17)
- detectPole(TNL);
- else
- detectPole(TNR);
- //printf("DetectMode\r\n");
+ if(deg>devideNum-1);
+ else if(deg<17){
+ //pc.printf("R\r\n");
+ detFinF=detectPole(TNR);
+ }
+ else{
+ //pc.printf("L\r\n");
+ detFinF=detectPole(TNL);
+ }
}
//射出
- if(getBt(btL)&&obonFlag&&btLF){
- kRed=shotToDenziben(0x02,39,1,airAddr);
- wait(0.1f);
- }
- else if(!getBt(btL)){
- btLF=true;
- kRed=0;
- }
-
-
- //射出
- if(getBt(btR)&&obonFlag&&btRF){
- kRed=shotToDenziben(0x02,23,1,airAddr);
- wait(0.1f);
- //printf("shot:%d\r\n",i2cS=i2c.write(airAddr,cmd,2));
- //i2c.write(airAddr,cmd,2);
- }
- else if(!getBt(btR)){
- btRF=true;
+ if(getBt(btL)&&obonFlag){
+ kRed=shotToDenziben(airReg,1,1,airAddr);
+ wait(shotWait);
+ kRed=shotToDenziben(airReg,0,1,airAddr);
kRed=0;
}
//射出
- if(getBt(btTri)&&obonFlag&&btTrF){
- kRed=shotToDenziben(0x02,15,1,airAddr);
- wait(0.2f);
- kRed=shotToDenziben(0x02,0,1,airAddr);
- kRed=0;
- }
- else if(!getBt(btTri)){
- btTrF=true;
+ if(getBt(btR)&&obonFlag){
+ kRed=shotToDenziben(airReg,2,1,airAddr);
+ wait(shotWait);
+ kRed=shotToDenziben(airReg,0,1,airAddr);
+ //printf("shot:%d\r\n",i2cS=i2c.write(airAddr,cmd,2));
+ //i2c.write(airAddr,cmd,2);
kRed=0;
}
- //バルブ開放
- if(getPress(presLeft)&&pLeftF){
- pLeftF=false;
- kRed=shotToDenziben(7,1,1,valveAddr);
- wait(0.2);
- }
- else if(!getPress(presLeft)){
+ //射出
+ if(getBt(btTri)&&obonFlag){
+ kRed=shotToDenziben(airReg,4,1,airAddr);
+ wait(shotWait);
+ kRed=shotToDenziben(airReg,0,1,airAddr);
kRed=0;
- pLeftF=true;
- }
-
- //バルブ開放
- if(getPress(presRight)&&pRightF){
- pRightF=false;
- kRed=shotToDenziben(8,1,1,valveAddr);
- wait(0.2);
}
- else if(!getPress(presRight)){
- kRed=0;
- pRightF=true;
- }
-
- //バルブ開放
- if(getBt(btCro)&&btCrF){
- btCrF=false;
- kRed=shotToDenziben(9,1,1,valveAddr);
- wait(0.2);
- }
- else if(!getBt(btCro))
- {
- kRed=0;
- btCrF=true;
- }
-
-
//レーザーポインタ
if(getBt(btSqa)&&sqf){
@@ -309,40 +288,38 @@
else if(!getBt(btSqa))
sqf=1;
- //タッチセンサ
- if(((int)DATA[1]&12)==12){
- bValue=0.0f;gValue=0.8f;rValue=0;
+ //レーザーセンサ
+ if(!((int)DATA[1]&2)==2&&!((int)DATA[1]%1)==1){
+ //Mechanum.putc(162);
+ }
+
+ //お盆回し
+ if(getPress(presUp)){
+ obonFlag=true;
+ rValue=0.8;gValue=0;bValue=0.8;
+ kRed=0;
+ }
+ else if(getPress(presDown)){
+ bValue=0.8f;gValue=0;rValue=0;
+ obonFlag=false;
+ kRed=0;
}
else{
bValue=0.8f;gValue=0;rValue=0;
}
+
- //お盆回し
- if(getPress(presUp)&&pUpF){
- kRed=shotToDenziben(0x02,7,1,airAddr);
- wait(0.2);
- obonFlag=true;
- pUpF=false;
- rValue=0.8;gValue=0;bValue=0.8;
- }
- else if(!getPress(presUp)){
- pUpF=true;
- kRed=0;
+ deg=devidePoint((double)DATA[3],(double)DATA[2],15,360/devideNum);
+ if(getPress(btSel)||(detFinF&°!=devideNum)){
+ detFinF=false;
+ Mechanum.putc(163);
+ wait(0.1);
}
- if(getPress(presDown)&&pDownF&&obonFlag){
- kRed=shotToDenziben(0x02,0,1,airAddr);
- wait(0.2);
- bValue=0.8f;gValue=0;rValue=0;
- pDownF=false;
- obonFlag=false;
+ else {
+ val=valueForMechanum(deg,devideNum,getBt(trigL),getBt(trigR),getBt(btSph));
+ //pc.printf("%lf\r\n",deg);
}
- else if(!getPress(presDown)){
- pDownF=true;
- kRed=0;
- }
-
- deg=devidePoint((double)DATA[3],(double)DATA[2],15,360/devideNum);
- val=valueForMechanum(deg,devideNum,getBt(trigL),getBt(trigR),getBt(btSph));
+
//一定時間通信してないと停止
if(stopCounter>stopCount)
@@ -355,11 +332,15 @@
do{
if(!debugFlag)
break;
+ //pc.printf("%d\r\n",DATA[6]);
+ //if(getPress(btSel))
+ // pc.printf("up\r\n");
//pc.printf("val:%d\r\n",val);
//printf("%lf:%d:%d\r\n",deg,(int)DATA[3],(int)DATA[2]);
//printf("%d\r\n",(int)DATA[0]);
//pc.printf("DATA[1]%d\r\n",(int)(DATA[1]&0x02));
//printf("deg:%lfX1:%d:bt%ddeg:%lfval:%d\r\n",deg,(int)DATA[1],getBt(btSqa),deg,val);
+ //pc.printf("%lf\r\n",deg);
}while(false);
}
}
\ No newline at end of file