ROBOSTEP_LIBRARY
F3RC4班 自動機プログラム by巨泉 速度制御ユニット使用の2輪(活かせなかった)、測定輪エンコーダ、MicroInfinityを用いている XY座標を読むことには成功したが、まともに制御できなかったのでノーカン()
Dependencies: CruizCore_R1370P EC delta enc_1multi mbed
Fork of
F3RC915
by 
春ロボ1班(元F3RC4班+)
Diff: main.cpp
- Revision:
- 7:3a7e49ed1162
- Parent:
- 6:7ec6c3d3a30a
- Child:
- 8:7b368096fed8
--- a/main.cpp Thu Sep 13 04:17:15 2018 +0000
+++ b/main.cpp Sat Sep 15 07:54:13 2018 +0000
@@ -4,10 +4,11 @@
#include "EC.h"
#include "R1370P.h"
#include "enc_1multi.h"
-#define BASIC_SPEED 15 //モーターはこの角速度で駆動させる
+#define BASIC_SPEED 24 //モーターはこの角速度で駆動させる
SpeedControl motorR(PB_13,PA_10,NC,500,0.05,PB_10,PB_1); //right enc migi ue
SpeedControl motorL(PB_5,PB_3,NC,500,0.05,PA_5,PA_7); //left enc hidari sita //ok
+
Ec EC1(PB_4,PA_8,NC,300,0.05); //center enc
Ticker motor_tick; //角速度計算用ticker
Ticker ticker;//for enc
@@ -23,11 +24,11 @@
EC1.CalOmega();
}
-//DigitalIn button(USER_BUTTON);
+DigitalIn button(USER_BUTTON,PullUp);
DigitalIn reset_f(PC_1,PullUp);
DigitalIn reset_a(PA_4,PullUp);
-PwmOut servo(PB_14);//servo
+PwmOut servo(PB_7);//servo
PwmOut motor_f(PC_9);
PwmOut motor_b(PB_9);//arm
DigitalOut denjiben(PC_0);//dennjibenn
@@ -38,14 +39,14 @@
double d_dist=0;
double x;
double y;
-double asari_x=900;
-double asari_y=2500;//asariwo toru tekisetsuna zahyouwo kaeraremasu
-double goal_x=1120;
-double goal_y=1700;
+double asari_x=810;
+double asari_y=2674;//asariwo toru tekisetsuna zahyouwo kaeraremasu
+double goal_x=1020;
+double goal_y=1562;
double start_x=185;
-double start_y=150;
+double start_y=300;
Timer t;
-int i=1;
+int i=0;
int kai=0;//printf kansuu
double target_R=0,target_L=0;
@@ -65,40 +66,40 @@
void susumu_y(double ay,double by,double goaly)//y zahyou wo motiita susumikata subete
{
//pc.printf("ay=%f by=%f y=%f goaly=%f\r\n",ay,by,y,goaly);
- if(y<goaly-50&&ay>=0) {
+ if(y<goaly-100&&ay>=0) {
t.start();
- pc.printf("R=%f L=%f\r\n",target_R,target_L);
- if(t.read_ms()<100) {
+ //pc.printf("R=%f L=%f\r\n",target_R,target_L);
+ if(t.read_ms()<150) {
pc.printf("t=%f",t.read());
tgt(ay/3,by/3);
- } else if(t.read_ms()>=100&&t.read_ms()<200) {
+ } else if(t.read_ms()>=150&&t.read_ms()<300) {
pc.printf("t=%f",t.read());
tgt(ay*2/3,by*2/3);
} else {
tgt(ay,by);
t.stop();
- pc.printf("R=%f L=%f",target_R,target_L);
+ //pc.printf("R=%f L=%f",target_R,target_L);
}
- } else if(y<goaly&&y>=goaly-50&&ay>=0) {
- tgt(ay/3,by/3);
+ } else if(y<goaly&&y>=goaly-100&&ay>=0) {
+ tgt(ay/2,by/2);
- } else if(y>goaly+50&&ay<0) {
+ } else if(y>goaly+100&&ay<0) {
t.start();
- pc.printf("R=%f L=%f\r\n",target_R,target_L);
- if(t.read_ms()<100) {
+ //pc.printf("R=%f L=%f\r\n",target_R,target_L);
+ if(t.read_ms()<150) {
pc.printf("t=%f",t.read());
tgt(ay/3,by/3);
- } else if(t.read_ms()>=100&&t.read_ms()<200) {
+ } else if(t.read_ms()>=150&&t.read_ms()<300) {
pc.printf("t=%f",t.read());
tgt(ay*2/3,by*2/3);
} else {
tgt(ay,by);
t.stop();
- pc.printf("R=%f L=%f",target_R,target_L);
+ //pc.printf("R=%f L=%f",target_R,target_L);
}
- } else if(y>goaly&&y<=goaly+50&&ay<0) {
- tgt(ay/3,by/3);
+ } else if(y>goaly&&y<=goaly+100&&ay<0) {
+ tgt(ay/2,by/2);
} else {
i++;
t.reset();
@@ -111,22 +112,22 @@
void susumu_xl(double axl,double bxl,double goalxl)//hidarikara mokuhyoutenn ni toutatsu surutameno dousa
{
- if(x<goalxl-50) {
+ if(x<goalxl-100) {
t.start();
- pc.printf("R=%f L=%f\r\n",target_R,target_L);
- if(t.read_ms()<100) {
+ //pc.printf("R=%f L=%f\r\n",target_R,target_L);
+ if(t.read_ms()<150) {
pc.printf("t=%f",t.read());
tgt(axl/3,bxl/3);
- } else if(t.read_ms()>=100&&t.read_ms()<200) {
+ } else if(t.read_ms()>=150&&t.read_ms()<300) {
pc.printf("t=%f",t.read());
tgt(axl*2/3,bxl*2/3);
} else {
tgt(axl,bxl);
t.stop();
- pc.printf("R=%f L=%f",target_R,target_L);
+ ////pc.printf("R=%f L=%f",target_R,target_L);
}
- } else if(x<goalxl&&x>=goalxl-50) {
- tgt(axl/3,bxl/3);
+ } else if(x<goalxl&&x>=goalxl-100) {
+ tgt(axl/2,bxl/2);
} else {
t.reset();
@@ -138,22 +139,22 @@
void susumu_xr(double axr,double bxr,double goalxr)//migikara mokuhyoutenn ni toutatsu surutameno dousa
{
- if(x>goalxr+50) {
+ if(x>goalxr+100) {
t.start();
- pc.printf("R=%f L=%f\r\n",target_R,target_L);
- if(t.read_ms()<100) {
+ //pc.printf("R=%f L=%f\r\n",target_R,target_L);
+ if(t.read_ms()<150) {
pc.printf("t=%f",t.read());
tgt(axr/3,bxr/3);
- } else if(t.read_ms()>=100&&t.read_ms()<200) {
+ } else if(t.read_ms()>=150&&t.read_ms()<300) {
pc.printf("t=%f",t.read());
tgt(axr*2/3,bxr*2/3);
} else {
tgt(axr,bxr);
t.stop();
- pc.printf("R=%f L=%f",target_R,target_L);
+ ////pc.printf("R=%f L=%f",target_R,target_L);
}
- } else if(x>goalxr&&x<=goalxr+50) {
- tgt(axr/3,bxr/3);
+ } else if(x>goalxr&&x<=goalxr+100) {
+ tgt(axr/2,bxr/2);
} else {
t.reset();
@@ -164,9 +165,9 @@
void susumu_ang(double a,double b,double goal)//kakudo
{
- if(goal-5>angle&&a<b) {//usetsu
+ if(goal-30>angle&&a<b) {//usetsu
t.start();
- pc.printf("R=%f L=%f\r\n",target_R,target_L);
+ //pc.printf("R=%f L=%f\r\n",target_R,target_L);
if(t.read_ms()<100) {
pc.printf("t=%f",t.read());
tgt(a/3,b/3);
@@ -176,14 +177,14 @@
} else {
tgt(a,b);
t.stop();
- pc.printf("R=%f L=%f",target_R,target_L);
+ ////pc.printf("R=%f L=%f",target_R,target_L);
}
- } else if(angle<goal&&angle>=goal-5&&a<b) {
- tgt(a/3,b/3);
+ } else if(angle<goal&&angle>=goal-30&&a<b) {
+ tgt(a/2,b/2);
- } else if(angle>goal+5&&a>b) { //sasetsu
+ } else if(angle>goal+30&&a>b) { //sasetsu
t.start();
- pc.printf("R=%f L=%f\r\n",target_R,target_L);
+ ////pc.printf("R=%f L=%f\r\n",target_R,target_L);
if(t.read_ms()<100) {
pc.printf("t=%f",t.read());
tgt(a/3,b/3);
@@ -193,10 +194,10 @@
} else {
tgt(a,b);
t.stop();
- pc.printf("R=%f L=%f",target_R,target_L);
+ ////pc.printf("R=%f L=%f",target_R,target_L);
}
- } else if(angle>goal&&angle<=goal+5&&a>b) {
- tgt(a/3,b/3);
+ } else if(angle>goal&&angle<=goal+30&&a>b) {
+ tgt(a/2,b/2);
} else {
i++;
@@ -211,7 +212,7 @@
int main(void)
{
-
+
gyro.initialize(); //main関数の最初に一度だけ実行
gyro.acc_offset(); //やってもやらなくてもいい
@@ -220,52 +221,52 @@
motorR.setDOconstant(34.1);
motorL.setDOconstant(30);//c
motorR.setPDparam(0,0);
- motorR.setPDparam(0,0);//pd//akirameta
+ motorL.setPDparam(0,0);//pd//akirameta
- EC1.setDiameter_mm(50);//sokuteirinnhannkei
+ EC1.setDiameter_mm(48);//sokuteirinnhannkei
double getDistance_mm();
//int EC1.getCount()=0;
void reset ();
EC1.reset();
- servo.period_ms(20);
+ //servo.period_ms(30);
motor_f.period_ms(30);
motor_b.period_ms(30);//arm
x=start_x;
y=start_y;//start point//keisann wo sinaosu hitsuyouga arimasu
-
+ //servo.pulsewidth_us(950);
while(1) {
+ //target_R=(-1)*BASIC_SPEED;
+ //target_L=(-1)*BASIC_SPEED;
// motorR.turnF(0.3);
//motorL.turnF(0.3);//for debug
- motorR.Sc(target_R);
- motorL.Sc(target_L);//target_R,target_Lの値を変えることで直進・後退・右回転・左回転を行う
+ //motorR.Sc(target_R);
+ //motorL.Sc(target_L);//target_R,target_Lの値を変えることで直進・後退・右回転・左回転を行う
angle=gyro.getAngle()*(-1); //角度の値を受け取る
EC1.getDistance_mm();
// EC1.CalOmega();
-
if(target_R==0) motorR.stop();
else motorR.Sc(target_R);
if(target_L==0) motorL.stop();
else motorL.Sc(target_L);
-
new_dist=EC1.getDistance_mm();
- d_dist=new_dist-old_dist;
- old_dist=new_dist;
+ d_dist=new_dist-old_dist;
+ old_dist=new_dist;
- double d_x=d_dist*sin(angle);
- double d_y=d_dist*cos(angle);
+ double d_x=d_dist*sin(angle*3.1415926535/180);
+ double d_y=d_dist*cos(angle*3.1415926535/180);
x=x+d_x;
y=y+d_y;
if(kai>=3) {
-
+
//double d_x=d_dist*sin(angle);;
//double d_y=d_dist*cos(angle);;
@@ -273,7 +274,7 @@
//y=y+d_y;
pc.printf("R=%f L=%f",target_R,target_L);
- // pc.printf("omg_R=%f omg_L=%f \r\n",motorR.getOmega(),motorL.getOmega());
+ pc.printf("omg_R=%f omg_L=%f \r\n",motorR.getOmega(),motorL.getOmega());
pc.printf("i=%d\r\n",i);
//pc.printf("EC1=%f",EC1.getDistance_mm(),EC1.getCount());
pc.printf("x=%f y=%f",x,y);
@@ -283,164 +284,182 @@
}
kai++;
if(i==0) {
- /*if(reset_f.read()==1) {
+ if(reset_f.read()==0&&button.read()==0) {
+ wait(0.05);
+ if(reset_f.read()==0&&button.read()==0) {
+ denjiben=1;
+ i++;
+ }
+ }
+ if(reset_a.read()==0&&button.read()==0) {
wait(0.05);
- if(reset_f.read()==1) {
- */ denjiben=0;
- i++;
- // }
- //}
- if(reset_a.read()==1) {
- wait(0.05);
- if(reset_a.read()==1) {
- denjiben=0;
- x=70;
- y=2500;
- i=14;
+ if(reset_a.read()==0&&button.read()==0) {
+ denjiben=1;
+
+ x=180;
+ y=2674;
+
+ i=6;
+ }
+ }//x,y
+ }
+ if(i==1) {
+ susumu_y(1,1,857);//x,x+178
+ }
+ if(i==2) {
+ susumu_ang(0,0.7,90);//x+155,x+471.5
+ }
+ if(i==3) {
+ susumu_xl(0.7,0.7,745);//728.5,1078.5
+ }
+ if(i==4) {
+ susumu_ang(0.7,0,0);//850,1372
+ }
+ if(i==5) {
+ susumu_y(0.7,0.7,2016);//850,1700
+ }
+ if(i==6) {
+ // motorR.stop();
+ //motorL.stop();
+ tgt(0,0);
+ wait(2);
+ t.start();
+ if(t<1) {
+ motor_f=0.82;
+ motor_b=0;
+
+ printf("motor");
+ } else {
+ motor_f=0;
+ motor_b=0;
+ printf("finish");
+ t.stop();
+ t.reset();
+ i++;
}
- }//x,y
- }
- if(i==1) {
- susumu_y(1,1,start_x+178);//x,x+178
- }
- if(i==2) {
- susumu_ang(0.5,1,45);//x+121.5,x+471.5
- }
- if(i==3) {
- susumu_xl(1,1,728.5);//728.5,1078.5
- }
- if(i==4) {
- susumu_ang(1,0.5,0);//850,1372
- }
- if(i==5) {
- susumu_y(1,1,1700);//850,1700
- }
- if(i==6) {
- motorR.stop();
- motorL.stop();
- t.start();
- if(t<1) {
- motor_f=0.82;
- motor_b=0;
- } else {
- motor_f=0;
- motor_b=0;
- printf("finish");
- t.reset();
+ }//gatiasari
+
+
+ if(i==7) {
+ wait(2);
+ t.start();
+ if(t<1) {
+ motor_f=0;
+ motor_b=0.82;
+
+ printf("motor");
+ } else {
+ motor_f=0;
+ motor_b=0;
+ printf("finish");
+ t.stop();
+ t.reset();
+ i++;
+ }
+ if(angle>=-89) {
+ target_R=BASIC_SPEED/2;
+ target_L=BASIC_SPEED/(-2);
+ }
+ if(angle<=-91) {
+ target_R=BASIC_SPEED/(-2);
+ target_L=BASIC_SPEED/2;
+ }
+ if(angle>-91&angle<-89) {
+ motorR.stop();
+ motorL.stop();
+ wait(2);
+ if(angle>-91&angle<-89) {
+ i++;
+ }
+ }
+ }//kakudo tyousei
+ if(i==8) {
+ susumu_xr(1,1,555);//700,1700
+ }
+ if(i==9) {
+ susumu_ang(0,0.7,0);//390,2010
+ }
+ if(i==10) {
+ susumu_y(0.8,0.8,asari_y-155);//390,y-310
+ }
+ if(i==11) {
+ susumu_ang(0,1,90);//700,y
+ }
+ if(i==12) {
+ motorR.stop();
+ motorL.stop();
+ //servo.pulsewidth_us(2100);
+ wait(1.5);
+ //servo.pulsewidth_us(2400);
+ wait(2);
i++;
}
- }//gatiasari
-
-
- if(i==7) {
- t.start();
- if(t<1) {
- motor_f=0;
- motor_b=0.82;
- } else {
- motor_f=0;
- motor_b=0;
- printf("finish");
- t.reset();
+ if(i==13) {
+ if(angle>=91) {
+ target_R=BASIC_SPEED/2;
+ target_L=BASIC_SPEED/(-2);
+ }
+ if(angle<=89) {
+ target_R=BASIC_SPEED/(-2);
+ target_L=BASIC_SPEED/2;
+ }
+ if(angle>89&angle<91) {
+ motorR.stop();
+ motorL.stop();
+ wait(2);
+ if(angle>89&angle<91) {
+ i++;
+ }
+ }
+ }
+ if(i==14) {
+ susumu_xl(0.6,0.6,asari_x);//x,y
}
- if(angle>=-89) {
- target_R=BASIC_SPEED/5;
- target_L=BASIC_SPEED/(-5);
+ if(i==15) {
+ motorR.stop();
+ motorL.stop();
+ tgt(0,0);
+ wait(2);
+ wait(0.5);
+ denjiben=0;
+ wait(0.5);
+ //servo.pulsewidth_us(900);
+ wait(2);
+ i++;
}
- if(angle<=-91) {
- target_R=BASIC_SPEED/(-5);
- target_L=BASIC_SPEED/-5;
+ if(i==16) {
+ susumu_xr(-1,-1,555);//700,y
+ }
+ if(i==17) {
+ susumu_ang(0,-0.6,0);//390,y-310
}
- if(angle>-91&angle<-89) {
+ if(i==18) {
+ susumu_y(-1,-1,goal_y+155);//390,y+310
+ }
+ if(i==19) {
+ susumu_ang(0,-0.6,-90);//700,y
+ }
+ if(i==20) {
+ susumu_xl(-1,-1,goal_x);//x,y
+ }
+ if(i==21) {
motorR.stop();
motorL.stop();
wait(0.5);
- if(angle>-91&angle<-89) {
- i++;
- }
- }
- }//kakudo tyousei
- if(i==8) {
- susumu_xr(1,1,700);//700,1700
- }
- if(i==9) {
- susumu_ang(1/3,1,0);//390,2010
- }
- if(i==10) {
- susumu_y(1,1,asari_y-310);//390,y-310
- }
- if(i==11) {
- susumu_ang(1/3,1,90);//700,y
- }
- if(i==12) {
- motorR.stop();
- motorL.stop();
- servo.pulsewidth_us(2100);
- wait(1.5);
- servo.pulsewidth_us(2400);
- wait(1);
- i++;
- }
- if(i==13) {
- if(angle>=91) {
- target_R=BASIC_SPEED/5;
- target_L=BASIC_SPEED/(-5);
- }
- if(angle<=89) {
- target_R=BASIC_SPEED/(-5);
- target_L=BASIC_SPEED/5;
+ denjiben=1;
+ wait(0.5);
+
+ break;
}
- if(angle>89&angle<91) {
- motorR.stop();
- motorL.stop();
- wait(0.1);
- if(angle>89&angle<91) {
- i++;
- }
+ if(i==30) {
+ tgt(-1,-1);
}
- }
- if(i==14) {
- susumu_xl(1,1,asari_x);//x,y
- }
- if(i==15) {
- motorR.stop();
- motorL.stop();
- wait(0.5);
- denjiben=1;
- wait(0.5);
- servo.pulsewidth_us(900);
- wait(2);
- i++;
- }
- if(i==16) {
- susumu_xr(-1,-1,700);//700,y
- }
- if(i==17) {
- susumu_ang(-1/3,-1,0);//390,y-310
- }
- if(i==18) {
- susumu_y(-1,-1,goal_y+310);//390,y+310
- }
- if(i==19) {
- susumu_ang(-1/3,-1,-90);//700,y
- }
- if(i==20) {
- susumu_xl(-1,-1,goal_x);//x,y
- }
- if(i==21) {
- motorR.stop();
- motorL.stop();
- wait(0.5);
- denjiben=0;
- wait(0.5);
-
- break;
- }
-}//while
-tgt(0,0);
-return 0;
+ }//while
+ tgt(0,0);
+ return 0;
}//intmain
+