春ロボ ロケット団
version which is easier to read
Dependencies: mbed KondoServoLibrary Encoder
Diff: User.cpp
- Revision:
- 24:674b011da45b
- Parent:
- 23:0c6bc29c8f86
- Child:
- 25:4eb2f2795120
--- a/User.cpp Fri Mar 13 08:23:05 2020 +0000
+++ b/User.cpp Thu Mar 19 06:18:37 2020 +0000
@@ -31,18 +31,20 @@
Ec1multi EC(p16,p17,RESOLUTION);//投E
InterruptIn X(p15);
+DigitalIn touchSensor(p14);//タッチセンサー
+
DigitalOut out1(p19);//エアシリンダー（つかむ方）
-DigitalOut out2(p20);//エアシリンダー（投擲）
+DigitalOut out2(p20);//エアシリンダー（投擲））
DigitalOut out3(p18);//エアシリンダー（ボックス）
-int id = 0;
-int id2 = 1;
+int id = 1;//ballbreak;
+int id2 = 0;//box
double SERVO2DEG = 270.0 / (11500 - 3500);
-double first_ball = (6800 - 3500) * SERVO2DEG;
-double first_box = (X - 3500) * SERVO2DEG; //X haven't be decided
-double grab_ball = (3600 - 3500) * SERVO2DEG;
-double pass = (5000 - 3500) * SERVO2DEG;
-double grab_box = (3550 - 3500) * SERVO2DEG;
+double first_ball = (5000 - 3500) * SERVO2DEG;
+double first_box = (5000 - 3500) * SERVO2DEG; //X haven't be decided
+double grab_ball = (3500 - 3500) * SERVO2DEG;
+double pass = (8000 - 3500) * SERVO2DEG;
+double grab_box = (3500 - 3500) * SERVO2DEG;
void cal()
{
@@ -61,9 +63,17 @@
void UserLoopSetting()
{
+ f.period_us(50);
+ b.period_us(50);
+ touchSensor.mode(PullUp);
}
void UserLoop(char n,const u8* data)
{
+ static int i=0;
+ if(i==0) {
+ wait(2);
+ i++;
+ }
u16 ButtonState;
if(n==0) { //有線Ps3USB.cpp
RSX = ((ps3report*)data)->RightStickX;
@@ -116,8 +126,10 @@
move = 10;
} else if((ButtonState >> BUTTONR1)&1 == 1) { //R1
move = 11;
+ } else if((ButtonState >> BUTTONSTART)&1 == 1 ) {//対応するボタンを書く(今回SELLECTボタン
+ move = 12;
} else if((ButtonState >> BUTTONSELECT)&1 == 1 ) {//対応するボタンを書く(今回SELLECTボタン
- move = 1;
+ move = 13;
} else {
move = 5;
}
@@ -125,24 +137,29 @@
//shoot
if((ButtonState >> BUTTONCIRCLE)&1 == 1) {//O
shoot = 1;
+ printf("shoot 1\r\n");
led1 = 1;
} else if((ButtonState >> BUTTONCROSS)&1 == 1) {//X
ball = 1;
+ printf("ball 1\r\n");
led2 = 1;
} else if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//△
box = 1;
+ printf("box 1\r\n");
led3 = 1;
} else if((ButtonState >> BUTTONSQUARE)&1 == 1) {//□
ball = 2;
+ printf("ball 2\r\n");
led4 = 1;
} else if((ButtonState >> BUTTONR2)&1 == 1) {//R2
box = 2;
+ printf("box 2\r\n");
led4 = 1;
}
show_angle();
//CAN通信用プログラム
if(shoot==1)shot();
- if(shoot>=2)catch_ball();
+ catch_ball();
can[0] = move;
controller.write(CANMessage(1,can,1));
printf("%d\r\n",can[0]);
@@ -175,36 +192,75 @@
}
void shot()
{
+ out1=1;
+ int breaknum=0;
X.rise(&Xcount);
- f.period_us(50);
- b.period_us(50);
double a=0,r=0.4,v=0;
int i=0,count;//rで半径指定 a*r=v
ticker.attach(&cal,0.05);
while(1) {
- printf("set");
- if(X_count ==1) {
+ printf("%d\r\n",touchSensor.read());
+ f=0;
+ b=0.05;
+ if(touchSensor==0) {//受け渡し
+ f=0;
+ b=0;
+ printf("%d",EC.getCount());
EC.reset();
- X_count =0;
+ printf("%d",EC.getCount());
+ wait(2);
+ out1=0;
+ wait(1);
+ out2=0;
+ wait(1);
+ printf("passed\r\n");
+ wait(1);
+ while(1) {
+ f=0.1;
+ b=0;
+ printf("moving to remove servo\r\n");
+ if(EC.getCount()>200) {
+ printf("servo move\r\n");
+ f=0;
+ b=0;
+ servo.set_degree(id, first_ball);
+ breaknum=1;
+ break;
+ }
+
+ }
+ if(X_count ==1) {
+ EC.reset();
+ X_count =0;
+ }
}
- break;
+ if(breaknum==1)break;
}
+ EC.reset();
+ wait(1);
+ X_count=0;
+ double m=0;
//角度リセット
while(1) {
- printf("%.3f %.3f %d %d\r\n",a,v,X_count,i);
- a=EC.getOmega();
+ //printf("%.3f %d %d\r\n",v,X_count,i);
+ //a=EC.getOmega();
count=EC.getCount();
i =count%500;
- v=a*r;
- f=0.45;//速度一定
+ //v=a*r;
+ if(m<1.0) {
+ m=m+0.05;
+ }
+ if(m>1) {
+ m=1;
+ }
+ f=0.45*m;//速度一定
b=0.0;
- led1=1;
- led2=0;
- led3=0;
- led4=1;
- if(X_count>3) {
- if(i < -380 && i > -390) {//離す角度を決める
- out2=1; //ボールを離す
+ if(X_count>2) {
+ if(i > 345 && i < 355) {//離す角度を決める
+ out1=1; //ボールを離す
+ f=0;
+ b=0;
+ wait(1);
printf("shot\r\n");
led1=1;
led2=1;
@@ -214,7 +270,7 @@
}
}
}
- printf("%.3f %.3f %d %d stop\r\n",a,v,X_count,i);
+ printf("%.3f %.3f %d\r\n",a,v,i);
X.rise(&Xcount);
a=EC.getOmega();
count=EC.getCount();
@@ -225,20 +281,26 @@
led2=1;
led3=1;
led4=0;
-
+ shoot=0;
}
-
void catch_ball()
{
-
switch (ball) {
case 1: //ball servo
if(ball_count ==0) {
- servo.set_degree(id, first_ball);
+ printf("pass\r\n");
+ servo.set_degree(id,pass);
wait(0.5);
ball_count =1;
ball = 0;
} else if(ball_count ==1) {
+ printf("glab\r\n");
+ servo.set_degree(id, first_ball);
+ wait(0.5);
+ ball_count = 2;
+ ball =0;
+ } else if(ball_count ==2) {
+ printf("first");
servo.set_degree(id, grab_ball);
wait(0.5);
ball_count = 0;
@@ -253,6 +315,7 @@
out1=0;
ball=0;
}
+ break;
}
}
@@ -261,16 +324,19 @@
switch(box) {
case 1:// box servo
if(box_count ==0) {
+ printf("Box first");
servo.set_degree(id2, first_box);
wait(0.5);
box_count =1;
- box = 1;
+ box = 0;
} else if(box_count ==1) {
+ printf("Box grab");
servo.set_degree(id2, grab_box);
wait(0.5);
box_count = 0;
box =0;
}
+ break;
case 2: //box air
if (out3==0) {
out3=1;
@@ -279,5 +345,6 @@
out3=0;
box = 0;
}
+ break;
}
}
\ No newline at end of file