2015 robotic contest arakawa A
/
Nucleo_PS3_Jikken
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
by 
Abe Daigorou
Diff: main.cpp
- Revision:
- 5:428b7ac86810
- Parent:
- 4:bae4e2d103d4
- Child:
- 6:5171a8245b57
--- a/main.cpp Thu Aug 27 05:03:13 2015 +0000
+++ b/main.cpp Mon Sep 14 02:32:41 2015 +0000
@@ -9,18 +9,25 @@
#define btSph 5
#define btCro 6
#define btSqa 7
+#define presUp 0
+#define presRight 1
+#define presDown 2
+#define presLeft 3
#define devideNum 32
#define ED 132
-#define TD 144
-#define ER 130
-#define TR 136
+#define TD 208
+#define EL 134
+#define TL 200
+#define TN 204
Serial pc(SERIAL_TX, SERIAL_RX);
//Serial Dev(D8,D2);//コントローラー
Serial Dev(PC_6,PA_12);//コントローラー
RawSerial Mechanum(D8,D2);//メカナム
I2C i2c(D14,D15);//sda,scl
+//const int airAddr=0x20;
const int airAddr=0xA0;
+const int valveAddr=0xC0;
BusOut dI2c(PC_0,PC_1,PC_2,PC_3);//i2cデバッグ
int Bflag = 0;
@@ -28,12 +35,12 @@
PwmOut Blue(D5),Green(D4),Red(D3);
//PwmOut kRed(A3),kBlue(A1),kGreen(A0);
PwmOut kRed(A3),kBlue(PA_15),kGreen(PB_7);
-#define N 6
+#define N 7
char DATA[N];
void rotate(double setAngle);
void speed();
-void num()//割り込まれたら６回受信
+void num()//割り込まれたら7回受信
{
if(Dev.getc()==114){
//printf("ReceiveCommand\r\n");
@@ -51,6 +58,11 @@
return (DATA[0]>>num)%2;
}
+bool getPress(int num)
+{
+ return (DATA[6]>>num)%2;
+}
+
double pointToDeg(double y,double x,int threshold)//座標から角度を求める関数thresholdはしきい値
{
if(abs((int)(y-128))<threshold)
@@ -121,23 +133,22 @@
void detectPole()
{
bool btFlag=false;
- int vector=0;
+ int vector=TD;
Blue=0.0f;Green=0.0f,Red=0.8f;
while(true)
{
//スイッチが押された時
- if(((int)DATA[1]&12)==12){
- vector=TR;
+ if(((int)DATA[1]&12)==12&&!btFlag){
+ wait(0.001f);
+ Mechanum.putc(64);
+ vector=TN;
btFlag=true;
Blue=0.0f;Green=0.8f;Red=0;
}
- //スイッチが押されていない時
- else if(((int)DATA[1]&12)!=12){
- vector=TD;
- }
//レーザーが反応した時
- if(((!(int)DATA[1]&1==1)&&(!(int)DATA[1]&2==2))&&btFlag){
+ else if(((!(int)DATA[1]&1==1)&&(!(int)DATA[1]&2==2))&&btFlag){
+ pc.printf("raser%d\r\n",(int)DATA[1]);
vector=64;
break;
}
@@ -146,25 +157,39 @@
wait(0.001f);
}
btFlag=false;
- Blue=0;Green=0;Red=0;
+ Blue=0;Green=0;Red=0;
}
-bool shot(char reg, char *data ,int size)
+bool shotToDenziben(char reg, char data ,int size,int addr)
{
- char DATA[2] = {reg,size};
- bool A = i2c.write(airAddr,DATA,2);
- A&= i2c.write(airAddr,data,size);
+ char REG[2] = {reg,size};
+ char DATA[2]={data,0};
+ bool A = i2c.write(addr,REG,2);
+ A|= i2c.write(addr,DATA,size);
return A;
}
+char getshot(char reg)
+{
+ char input=0;
+ char DATA[2] = {reg,1};
+ i2c.write(airAddr,DATA,2);
+ wait(0.005f);
+ i2c.read(airAddr,&input,1);
+ return input;
+}
+
int main() {
pc.baud(230400);
Dev.baud(921600);
Mechanum.baud(230400);
- char shotL[]={0x01,0};
- char shotR[]={0x02,0};
- char shotT[]={0x04,0};
+ char shotL[]={57,0};
+ char shotR[]={58,0};
+ char shotT[]={60,0};
+ char obon[]={56,0};
+ char read[2]={10,0};
+ DigitalIn bt(USER_BUTTON);
Dev.attach(num,Serial::RxIrq);//受信割り込み設定
Green=0;Blue=0;Red=0;
DigitalOut l(PB_2);
@@ -175,37 +200,67 @@
i2c.frequency(400000);
Blue=0.8f;Green=0;
kGreen=0.8f;
+ bool obonFlag=false,pLeftF=false,pRightF=false,btCrF=false;
while(true)
- {
+ {
Blue=0.8f;Green=0;Red=0;
+
+ //ポール検出モード
if(getBt(btTri)&&getBt(btSph)){
detectPole();
//printf("DetectMode\r\n");
}
- if(getBt(btL)){
- kRed=!(shot(0x02,shotL,1));
- wait(0.3f);
+ //射出
+ if(getBt(btL)&&obonFlag){
+ kRed=shotToDenziben(0x02,39,1,airAddr);
+ wait(0.1f);
+ kRed=0;
+ }
+
+ //射出
+ if(getBt(btR)&&obonFlag){
+ 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);
kRed=0;
}
- if(getBt(btR)){
- kRed=!(shot(0x02,shotR,1));
- wait(0.3f);
- //printf("shot:%d\r\n",i2cS=i2c.write(airAddr,cmd,2));
- //i2c.write(airAddr,cmd,2);
+ //射出
+ if(getBt(btTri)&&obonFlag){
+ kRed=shotToDenziben(0x02,15,1,airAddr);
+ wait(0.1f);
kRed=0;
}
- if(getBt(btTri)){
- kRed=!(shot(0x02,shotT,1));
- wait(0.3f);
- kRed=0;
+ //バルブ開放
+ if(getPress(presLeft)&&pLeftF){
+ pLeftF=false;
+ kRed=shotToDenziben(7,1,1,valveAddr);
+ }
+ else if(!getPress(presLeft))
+ pLeftF=true;
+
+ //バルブ開放
+ if(getPress(presRight)&&pRightF){
+ pRightF=false;
+ kRed=shotToDenziben(8,1,1,valveAddr);
}
+ else if(!getPress(presRight))
+ pRightF=true;
+ //バルブ開放
+ if(getBt(btCro)&&btCrF){
+ btCrF=false;
+ kRed=shotToDenziben(9,1,1,valveAddr);
+ }
+ else if(!getBt(btCro))
+ btCrF=true;
+
+
+ //レーザーポインタ
if(getBt(btSqa)&&sqf){
sqf=0;
l=!l;
@@ -214,14 +269,24 @@
else if(!getBt(btSqa))
sqf=1;
- if(getBt(btCro)){
- kRed=!(shot(0x02,0,1));
- wait(0.3f);
- kRed=0;
- }
+ //タッチセンサ
if(((int)DATA[1]&12)==12)
Blue=0.0f;Green=0.8f;Red=0;
+ //お盆回し
+ if(getPress(presUp)){
+ kRed=shotToDenziben(0x02,7,1,airAddr);
+ wait(0.1);
+ obonFlag=true;
+ kRed=0;
+ }
+ else if(getPress(presDown)){
+ kRed=shotToDenziben(0x02,0,1,airAddr);
+ wait(0.1);
+ obonFlag=false;
+ kRed=0;
+ }
+
//deg=pointToDeg((double)DATA[3],(double)DATA[2],30);
deg=devidePoint((double)DATA[3],(double)DATA[2],15,360/devideNum);
//printf("%lf:%d:%d\r\n",deg,(int)DATA[3],(int)DATA[2]);
@@ -230,7 +295,7 @@
//printf("val:%d\r\n",val);
Mechanum.putc(val);
wait(0.03f);
- //pc.printf("DATA[1]%d\r\n",(int)DATA[1]);
+ pc.printf("DATA[1]%d\r\n",(int)DATA[1]);
//printf("deg:%lfX1:%d:bt%ddeg:%lfval:%d\r\n",deg,(int)DATA[1],getBt(btSqa),deg,val);
}
}
\ No newline at end of file