春ロボ ロケット団
another
Dependencies: mbed KondoServoLibrary Encoder
Diff: User.cpp
- Revision:
- 26:3adbabf7d770
- Parent:
- 25:4eb2f2795120
- Child:
- 27:7a8cc28eeb42
--- a/User.cpp Thu Mar 19 08:35:23 2020 +0000
+++ b/User.cpp Sat Mar 21 03:15:26 2020 +0000
@@ -41,8 +41,8 @@
int id2 = 1;//box
double SERVO2DEG = 270.0 / (11500 - 3500);
double first_ball = (4500 - 3500) * SERVO2DEG;
-double first_box = (4000 - 3500) * SERVO2DEG; //X haven't be decided
-double grab_ball = (3830 - 3500) * SERVO2DEG;
+double first_box = (6000 - 3500) * SERVO2DEG; //X haven't be decided
+double grab_ball = (3850 - 3500) * SERVO2DEG;
double pass = (7080 - 3500) * SERVO2DEG;
double grab_box = (3500 - 3500) * SERVO2DEG;
@@ -51,8 +51,9 @@
EC.calOmega();
}
-void shot();
+void ballShot(int data);
void catch_ball();
+void catch_box();
void show_angle();
int X_count=0;
@@ -66,12 +67,14 @@
f.period_us(50);
b.period_us(50);
touchSensor.mode(PullUp);
+ X.rise(&Xcount);
+ out1=1;
}
void UserLoop(char n,const u8* data)
{
static int i=0;
if(i==0) {
- wait(2);
+ wait(1);
i++;
}
u16 ButtonState;
@@ -101,8 +104,10 @@
led3=0;
led4=0;
//reset
+
if((ButtonState >> BUTTONUP)&1 == 1 &&(ButtonState >> BUTTONTRIANGEL)&1 == 1 ) {//R2
NVIC_SystemReset();
+ move =14;
printf("reset\r\n");
}
@@ -140,11 +145,19 @@
}
//move
//shoot
- if((ButtonState >> BUTTONCIRCLE)&1 == 1) {//O
- shoot = 1;
- printf("shoot 1\r\n");
+ if((ButtonState >> BUTTONCIRCLE)&1 == 1 && (ButtonState >> BUTTONLEFT)&1 == 1 ) {//O & left shot reset
+ shoot =0;
+ printf("shoot 0\r\n");
+ led2 = 1;
+ } else if((ButtonState >> BUTTONCIRCLE)&1 == 1) {//O
+ shoot = shoot +1;
+ if(shoot > 3) {
+ shoot =0;
+ }
+ printf("shoot %d\r\n",shoot);
+ wait(0.5);
led1 = 1;
- } else if((ButtonState >> BUTTONCROSS)&1 == 1) {//X
+ } else if((ButtonState >> BUTTONCROSS)&1 == 1) {//X
ball = 1;
printf("ball 1\r\n");
led2 = 1;
@@ -162,131 +175,105 @@
led4 = 1;
}
- show_angle();
//CAN通信用プログラム
- if(shoot==1)shot();
+ ballShot(shoot);
catch_ball();
+ catch_box();
can[0] = move;
controller.write(CANMessage(1,can,1));
printf("%d\r\n",can[0]);
}
-void show_angle()
+
+void ballShot(int data)
{
+ static 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);
- if(X_count ==1) {
- printf("set ok");
- EC.reset();
- }
- //角度リセット
- a=EC.getOmega();
- count=EC.getCount();
- i =count%500;
- printf("%.3f %.3f %d %d stop\r\n",a,v,X_count,count);
- f=0;
- b=0;
- led1=0;
- led2=1;
- led3=1;
- led4=0;
-
-}
-void shot()
-{
- int breaknum=0;
- X.rise(&Xcount);
- double a=0,r=0.4,v=0;
int i=0,count;//rで半径指定 a*r=v
ticker.attach(&cal,0.05);
- while(1) {
- out1=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();
- printf("%d",EC.getCount());
- wait(1);
- out2=0;
- out2=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");
+ switch(data) {
+ case 1:
+ if(breaknum==0) {
+ out2=1;
+ out1=1;
+ printf("%d\r\n",touchSensor.read());
+ printf("%d\r\n",EC.getCount());
+ f=0;
+ b=0.09;
+ if(touchSensor==0) {//受け渡し
f=0;
b=0;
- servo.set_degree(id, first_ball);
+ printf("%d",EC.getCount());
+ EC.reset();
+ printf("%d",EC.getCount());
breaknum=1;
- break;
}
-
- }
- if(X_count ==1) {
- EC.reset();
- X_count =0;
}
- }
- if(breaknum==1)break;
- }
- EC.reset();
- wait(1);
- X_count=0;
- double m=0;
- //角度リセット
- while(1) {
- //printf("%.3f %d %d\r\n",v,X_count,i);
- //a=EC.getOmega();
- count=EC.getCount();
- i =count%500;
- //v=a*r;
- if(m<1.0) {
- m=m+0.05;
- }
- if(m>1) {
- m=1;
- }
- f=0.1*m;//速度一定
- b=0.0;
- if(X_count>2) {
- if(i > 345 && i < 355) {//離す角度を決める
- out1=1; //ボールを離す
+ break;
+
+ case 2:
+ if(breaknum ==1) {
+ out1=1;
+ wait(1);
+ out2=0;
+ wait(0.5);
+ breaknum++;
+ }
+ 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;
- wait(1);
- printf("shot\r\n");
- led1=1;
- led2=1;
- led3=1;
- led4=1;
+ servo.set_degree(id, first_ball);
+ EC.reset();
break;
}
- }
+ break;
+ case 3:
+ X_count=0;
+ double m=0;
+ while(1) {
+ count=EC.getCount();
+ i =count%500;
+ if(m<1.0) {
+ m=m+0.05;
+ }
+ if(m>1) {
+ m=1;
+ }
+ f=0.35*m;//速度一定
+ b=0.0;
+ if(X_count>2) {
+ if(i > 345 && i < 355) {//離す角度を決める
+ out1=1; //ボールを離す
+ f=0;
+ b=0;
+ printf("shot\r\n");
+ led1=1;
+ led2=1;
+ led3=1;
+ led4=1;
+ break;
+ }
+ }
+ }
+ X.rise(&Xcount);
+ count=EC.getCount();
+ i =count%500;
+ f=0;
+ b=0;
+ led1=0;
+ led2=1;
+ led3=1;
+ led4=0;
+ shoot=0;
+ breaknum=0;
+ break;
}
- printf("%.3f %.3f %d\r\n",a,v,i);
- X.rise(&Xcount);
- a=EC.getOmega();
- count=EC.getCount();
- i =count%500;
- f=0;
- b=0;
- led1=0;
- led2=1;
- led3=1;
- led4=0;
- shoot=0;
}
+
void catch_ball()
{
switch (ball) {
@@ -315,9 +302,11 @@
if (out1==0) {
out1=1;
ball=0;
+ wait(0.5);
} else if (out1 == 1) {
out1=0;
ball=0;
+ wait(0.5);
}
break;
}
@@ -329,13 +318,13 @@
case 1:// box servo
if(box_count ==0) {
printf("Box first");
- servo.set_degree(id2, first_box);
+ servo.set_degree(id2,first_box);
wait(0.5);
box_count =1;
box = 0;
} else if(box_count ==1) {
printf("Box grab");
- servo.set_degree(id2, grab_box);
+ servo.set_degree(id2,grab_box);
wait(0.5);
box_count = 0;
box =0;
@@ -345,9 +334,11 @@
if (out3==0) {
out3=1;
box = 0;
+ wait(0.5);
} else if (out3 == 1) {
out3=0;
box = 0;
+ wait(0.5);
}
break;
}