春ロボ ロケット団
/
Right_F3RC_mar4_2
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 Encoder CruizCore_R1370P
Diff: main.cpp
- Revision:
- 1:1cb20fa989c2
- Parent:
- 0:9c9994747ea7
- Child:
- 2:8ab2c4ec07e7
--- a/main.cpp Fri Oct 04 12:43:40 2019 +0000
+++ b/main.cpp Fri Oct 04 14:15:18 2019 +0000
@@ -29,45 +29,77 @@
{
double a,b;
if( angle > 1) {
- a = 0.04;
+ a = 0.05;
b = 0;
- RF = a;
- RB = b;
- LF = a;
- LB = b;
- TF = a;
- TB = b;
+ RF = RF + a;
+ RB = RB + b;
+ LF = LF + a;
+ LB = LB + b;
+ TF = TF + a;
+ TB = TB + b;
} else if(angle < -1) {
a = 0;
- b = 0.04;
- RF = a;
- RB = b;
- LF = a;
- LB = b;
- TF = a;
- TB = b;
+ b = 0.05;
+ RF = RF + a;
+ RB = RB + b;
+ LF = LF + a;
+ LB = LB + b;
+ TF = TF + a;
+ TB = TB + b;
}
}
-void go_S(double a)
+void go_S(double a,int x,double b,int count)
{
- RF = 0;
- RB = a;
- LF = a;
- LB = 0;
- TF = 0;
- TB = 0;
+ if((x - count)*(x - count) < 100) {
+ RF = 0;
+ RB = a;
+ LF = a;
+ LB = 0;
+ TF = 0;
+ TB = 0;
+ } else if(x - count >= 10) {
+ RF = 0;
+ RB = a + b/2;
+ LF = a;
+ LB = 0 + b/2;
+ TF = 0 + b;
+ TB = 0;
+ } else if (x - count <= -10) {
+ RF = 0 + b/2;
+ RB = a;
+ LF = a + b/2;
+ LB = 0;
+ TF = 0;
+ TB = 0 + b;
+ }
}
-void go_B(double a)
+void go_B(double a,int x,double b,int count)
{
- RF = a;
- RB = 0;
- LF = 0;
- LB = a;
- TF = 0;
- TB = 0;
+ if((x - count)*(x - count) < 100) {
+ RF = a;
+ RB = 0;
+ LF = 0;
+ LB = a;
+ TF = 0;
+ TB = 0;
+ } else if(x - count >= 10) {
+ RF = a;
+ RB = 0 + b/2;
+ LF = 0;
+ LB = a + b/2;
+ TF = 0 + b;
+ TB = 0;
+ } else if (x - count <= -10) {
+ RF = a + b/2;
+ RB = 0;
+ LF = 0 + b/2;
+ LB = a;
+ TF = 0;
+ TB = 0 + b;
+ }
}
void go_R(double a)
@@ -92,13 +124,14 @@
int main()
{
+ double a,b;
TF.period_ms(1);
TB.period_ms(1);
RF.period_ms(1);
RB.period_ms(1);
LF.period_ms(1);
LB.period_ms(1);
- out1 = 0;
+ out1 = 0;
servo.period_ms(20);
servo.pulsewidth_us(550);
wait(1);
@@ -125,10 +158,23 @@
pc.printf(" angle=%8.4f ",angle);
pc.printf("first");
pc.printf("\r\n");
- go_R(0.1);
+ go_R(0.2);
rotation(angle);
}
+ go_R(0);
rotation(angle);
+ while(count2 < 10000) {
+ count=EC.getCount();
+ count2=EC2.getCount();
+ angle=gyro.getAngle(); //角度の値を受け取る
+ pc.printf(" x=%d ",count);
+ pc.printf(" y=%d ",count2);
+ pc.printf(" angle=%8.4f ",angle);
+ pc.printf("\r\n");
+ go_S(0.4,200,0.1,count);
+ rotation(angle);
+ }
+ go_L(0);
while(count2 < 10800) {
count=EC.getCount();
count2=EC2.getCount();
@@ -137,9 +183,10 @@
pc.printf(" y=%d ",count2);
pc.printf(" angle=%8.4f ",angle);
pc.printf("\r\n");
- go_S(0.1);
+ go_S(0.2,200,0.1,count);
rotation(angle);
}
+ go_L(0);
while(count < 195) {
count=EC.getCount();
count2=EC2.getCount();
@@ -164,7 +211,6 @@
rotation(angle);
}
go_L(0);
- //get (-600,10000)
servo.pulsewidth_us(600);
wait(0.5);
servo.pulsewidth_us(700);
@@ -201,9 +247,10 @@
pc.printf(" y=%d ",count2);
pc.printf(" angle=%8.4f ",angle);
pc.printf("\r\n");
- go_B(0.1);
+ go_B(0.4,200,0.1,count);
rotation(angle);
}
+ go_L(0);
while(count < 195) {
count=EC.getCount();
count2=EC2.getCount();
@@ -215,6 +262,7 @@
go_R(0.1);
rotation(angle);
}
+ go_L(0);
while(count > 205) {
count=EC.getCount();
count2=EC2.getCount();
@@ -226,8 +274,8 @@
go_L(0.1);
rotation(angle);
}
+ go_L(0);
while(angle > -85) {
- double a,b;
a = 0.05;
b = 0;
RF = a;
@@ -238,8 +286,7 @@
TB = b;
angle=gyro.getAngle();
}
- //now (-700,100)
- //EC reset
+ go_L(0);
pc.printf("1\r\n");
while(count2 < 1200) {
count=EC.getCount();
@@ -249,13 +296,17 @@
pc.printf(" y=%d ",count2);
pc.printf(" angle=%8.4f ",angle);
pc.printf("\r\n");
- go_S(0.1);
- }//(0,600)
+ a = 0.1;
+ RF = a;
+ RB = 0;
+ LF = 0;
+ LB = a;
+ TF = 0;
+ TB = 0;
+ }
pc.printf("2\r\n");
- go_S(0);
- go_R(0);
go_L(0);
- wait(15);
+ wait(10);
pc.printf("3\r\n");
while(count2 > 500) {
count=EC.getCount();
@@ -265,12 +316,16 @@
pc.printf(" y=%d ",count2);
pc.printf(" angle=%8.4f ",angle);
pc.printf("\r\n");
- go_B(0.1);
+ a = 0.1;
+ RF = 0;
+ RB = a;
+ LF = a;
+ LB = 0;
+ TF = 0;
+ TB = 0;
}
- go_B(0);
-
+ go_L(0);
pc.printf("4\r\n");
- //now(0,0)
while( angle > 1) {
double a,b;
angle=gyro.getAngle();
@@ -283,6 +338,7 @@
TF = a;
TB = b;
}
+ go_L(0);
while(angle < -1) {
double a,b;
angle=gyro.getAngle();
@@ -295,8 +351,11 @@
TF = a;
TB = b;
}
- //now (0,0)
+ go_L(0);
pc.printf("5\r\n");
+ int old_count,old_count2;
+ old_count=EC.getCount();
+ old_count2=EC2.getCount();
while(count2 < 11800) {
count=EC.getCount();
count2=EC2.getCount();
@@ -305,9 +364,9 @@
pc.printf(" y=%d ",count2);
pc.printf(" angle=%8.4f ",angle);
pc.printf("\r\n");
- go_S(0.1);
+ go_S(0.4,old_count,0.05,count);
rotation(angle);
- }//now(0,10000)
+ }
while(count < 2500) {
count=EC.getCount();
count2=EC2.getCount();
@@ -318,7 +377,9 @@
pc.printf("\r\n");
go_R(0.1);
rotation(angle);
- }//now(-700,11800)
+ }
+ old_count=EC.getCount();
+ old_count2=EC2.getCount();
while(count2 < 13000) {
count=EC.getCount();
count2=EC2.getCount();
@@ -327,12 +388,10 @@
pc.printf(" y=%d ",count2);
pc.printf(" angle=%8.4f ",angle);
pc.printf("\r\n");
- go_S(0.1);
+ go_S(0.4,count,0.05,count);
rotation(angle);
- }//now(-1700,13000)
- //remove battery
- wait(3);
- while(count > 800) {
+ }
+ while(count > 600) {
count=EC.getCount();
count2=EC2.getCount();
angle=gyro.getAngle(); //角度の値を受け取る
@@ -344,9 +403,6 @@
rotation(angle);
}
go_R(0);
- go_S(0);
- go_L(0);
- //now(0,13000)
servo.pulsewidth_us(1100);
wait(0.5);
servo.pulsewidth_us(1050);
@@ -371,4 +427,4 @@
wait(1);
servo.pulsewidth_us(550);
wait(1);
-}//now(-800,13000)
+}