2015 robotic contest arakawa A
/
Nucleo_PS3_Jikken_PS3
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_Jikken3
by 
2015 robotic contest arakawa A
Revision 19:cd036cc40a2d, committed 2015-10-13
- Comitter:
- nodoame
- Date:
- Tue Oct 13 07:40:34 2015 +0000
- Parent:
- 18:e3591b0b9e40
- Child:
- 20:373fa71ce05b
- Commit message:
- 20151013; ????
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Oct 08 07:30:25 2015 +0000
+++ b/main.cpp Tue Oct 13 07:40:34 2015 +0000
@@ -9,18 +9,17 @@
#define btSph 5
#define btCro 6
#define btSqa 7
-#define btSel 0
-#define presUp 4
-#define presRight 5
-#define presDown 6
-#define presLeft 7
+#define presUp 0
+#define presRight 1
+#define presDown 2
+#define presLeft 3
#define devideNum 32
#define ED 132
#define TD 144//208
#define EL 134
#define TL 200
#define TNR 138//204
-#define TNL 150
+#define TNL 135
#define debugFlag 1
#define stopCount 33
@@ -30,8 +29,7 @@
RawSerial Mechanum(D8,D2);//メカナム
I2C i2c(D14,D15);//sda,scl
//const int airAddr=0x20;
-const int airAddr=0xd0;
-const int airReg=10;
+const int airAddr=0xA0;
const int valveAddr=0xC0;
BusOut dI2c(PC_0,PC_1,PC_2,PC_3);//i2cデバッグ
@@ -77,7 +75,7 @@
return atan2(-(y-128),x-128)/M_PI*180;//y軸反転、/PI*180でRadからDeg変換
}
-int devidePoint(double y,double x,int threshold,float devide)
+double devidePoint(double y,double x,int threshold,float devide)
{
double tempX,tempY,dev;//仮のX,Yと分けた角度
tempX=(x-128);tempY=(y-128);//それぞれ-128
@@ -135,60 +133,43 @@
return val;
}
-bool detectPole(int scVector)
+void detectPole(int scVector)
{
int vector=TD,laser=0;
- bool btFlag=false,canFlag=false,breakFlag=false,stopFlag=false;
+ bool btFlag=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;Green=0.8;Red=0;
+ Blue=0.0f;Green=0.8f;Red=0;
vector=scVector;
}
//片方のレーザーが反応したとき
- if(btFlag&&!laser){
- if(!(((int)DATA[1]&2)==2)){
+ if(btFlag){
+ if(!(int)DATA[1]&2){
laser=1;
- Red=0.8;
vector=0;
}
- else if(!(((int)DATA[1]&1)==1)){
+ else if((int)DATA[1]&1){
laser=2;
- Red=0.8;
vector=0;
}
}
//もう片方のレーザーが反応した時
- 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;
+ else if(!(int)DATA[1]&laser&&laser){
+ Mechanum.putc(64);
+ break;
}
//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.0126f);
+ wait(0.001f);
}
btFlag=false;
Blue=0;Green=0;Red=0;
@@ -231,52 +212,93 @@
double gValue=0,bValue=0,rValue=0;
DigitalOut l(PB_2);
int sqf=0;
- double deg=32;
+ double deg;
int val;
i2c.frequency(400000);
setFCLED(0,0.8,0.8);
- bool obonFlag=false,pLeftF=false,pRightF=false,btCrF=false,detFinF=false;
+ bool obonFlag=false,pLeftF=false,pRightF=false,btCrF=false,pUpF=false,pDownF=false,btLF=false,btRF=false,btTrF=false;
while(true)
{
//ポール検出モード
if(getBt(btTri)&&getBt(btSph)){
- 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(deg<17)
+ detectPole(TNL);
+ else
+ detectPole(TNR);
+ //printf("DetectMode\r\n");
}
//射出
- if(getBt(btL)&&obonFlag){
- kRed=shotToDenziben(airReg,1,1,airAddr);
- wait(0.5f);
- kRed=shotToDenziben(airReg,0,1,airAddr);
+ 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;
kRed=0;
}
//射出
- if(getBt(btR)&&obonFlag){
- kRed=shotToDenziben(airReg,2,1,airAddr);
- wait(0.5f);
- kRed=shotToDenziben(airReg,0,1,airAddr);
- //printf("shot:%d\r\n",i2cS=i2c.write(airAddr,cmd,2));
- //i2c.write(airAddr,cmd,2);
+ 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;
kRed=0;
}
- //射出
- if(getBt(btTri)&&obonFlag){
- kRed=shotToDenziben(airReg,4,1,airAddr);
- wait(0.5f);
- kRed=shotToDenziben(airReg,0,1,airAddr);
+ //バルブ開放
+ if(getPress(presLeft)&&pLeftF){
+ pLeftF=false;
+ kRed=shotToDenziben(7,1,1,valveAddr);
+ wait(0.2);
+ }
+ else if(!getPress(presLeft)){
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){
@@ -289,36 +311,38 @@
//タッチセンサ
if(((int)DATA[1]&12)==12){
- //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;
+ bValue=0.0f;gValue=0.8f;rValue=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;
+ }
+ 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 if(!getPress(presDown)){
+ pDownF=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);
- }
- else {
- val=valueForMechanum(deg,devideNum,getBt(trigL),getBt(trigR),getBt(btSph));
- //pc.printf("%lf\r\n",deg);
- }
-
+ val=valueForMechanum(deg,devideNum,getBt(trigL),getBt(trigR),getBt(btSph));
//一定時間通信してないと停止
if(stopCounter>stopCount)
@@ -331,15 +355,11 @@
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