ROBOSTEP_LIBRARY
F3RC4班 自動機プログラム by巨泉 速度制御ユニット使用の2輪(活かせなかった)、測定輪エンコーダ、MicroInfinityを用いている XY座標を読むことには成功したが、まともに制御できなかったのでノーカン()
Dependencies: CruizCore_R1370P EC delta enc_1multi mbed
Fork of
F3RC915
by 
春ロボ1班(元F3RC4班+)
main.cpp
- Committer:
- aoikoizumi
- Date:
- 2018-09-11
- Revision:
- 3:7bd1afb46094
- Parent:
- 2:3176040a4777
- Child:
- 4:e3d739bf09b4
File content as of revision 3:7bd1afb46094:
#include "mbed.h"
#include "SpeedController.h"
#include "EC.h"
#include "R1370P.h"
#include "enc_1multi.h"
#define BASIC_SPEED 30 //モーターはこの角速度で駆動させる
SpeedControl motorR(PB_13,PA_10,NC,500,0.05,PB_10,PB_1); //right enc migi ue
SpeedControl motorL(PB_3,PB_5,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
Serial pc(USBTX, USBRX); // tx, rx //PC USB
R1370P gyro(PC_6,PC_7);
void calOmega() //角速度計算関数
{
motorR.CalOmega();
motorL.CalOmega();
EC1.CalOmega();
}
//DigitalIn button(USER_BUTTON);
DigitalIn reset_f(PC_1,PullUp);
DigitalIn reset_a(PA_4,PullUp);
PwmOut servo(PB_14);//servo
PwmOut motor_f(PC_9);
PwmOut motor_b(PB_9);//arm
DigitalOut denjiben(PC_0);//dennjibenn
double new_dist=0;
double old_dist=0;
double d_dist=0;
double x=185;
double y=150;//start point//keisann wo sinaosu hitsuyouga arimasu
Timer t;
int i=0;
int kai=0;//printf kansuu
double target_R=0,target_L=0;
double angle; //変数宣言
void tgt(double r,double l)//void de sika tsukawanaino ha mottainai kimosuru
{
target_R=BASIC_SPEED*r;
target_L=BASIC_SPEED*l;
}
//printf wo dousa kakunin sidai kesou toha omotte imasu
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) {
t.start();
pc.printf("R=%f L=%f\r\n",target_R,target_L);
if(t.read_ms()<100) {
pc.printf("t=%f",t.read());
tgt(ay/3,by/3);
} else if(t.read_ms()>=100&&t.read_ms()<200) {
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);
}
} else if(y<goaly&&y>=goaly-50&&ay>=0) {
tgt(ay/3,by/3);
} else if(y>goaly+50&&ay<0) {
t.start();
pc.printf("R=%f L=%f\r\n",target_R,target_L);
if(t.read_ms()<100) {
pc.printf("t=%f",t.read());
tgt(ay/3,by/3);
} else if(t.read_ms()>=100&&t.read_ms()<200) {
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);
}
} else if(y>goaly&&y<=goaly+50&&ay<0) {
tgt(ay/3,by/3);
} else {
i++;
t.reset();
pc.printf("owari%d\r\n",i);
}
}
void susumu_xl(double axl,double bxl,double goalxl)//hidarikara mokuhyoutenn ni toutatsu surutameno dousa
{
if(x<goalxl-50) {
t.start();
pc.printf("R=%f L=%f\r\n",target_R,target_L);
if(t.read_ms()<100) {
pc.printf("t=%f",t.read());
tgt(axl/3,bxl/3);
} else if(t.read_ms()>=100&&t.read_ms()<200) {
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);
}
} else if(x<goalxl&&x>=goalxl-50) {
tgt(axl/3,bxl/3);
} else {
t.reset();
i++;
pc.printf("owari\r\n");
}
}
void susumu_xr(double axr,double bxr,double goalxr)//migikara mokuhyoutenn ni toutatsu surutameno dousa
{
if(x>goalxr+50) {
t.start();
pc.printf("R=%f L=%f\r\n",target_R,target_L);
if(t.read_ms()<100) {
pc.printf("t=%f",t.read());
tgt(axr/3,bxr/3);
} else if(t.read_ms()>=100&&t.read_ms()<200) {
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);
}
} else if(x>goalxr&&x<=goalxr+50) {
tgt(axr/3,bxr/3);
} else {
t.reset();
i++;
pc.printf("owari\r\n");
}
}
void susumu_ang(double a,double b,double goal)//kakudo
{
if(x<angle-5&&a>b) {//usetsu
t.start();
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);
} else if(t.read_ms()>=100&&t.read_ms()<200) {
pc.printf("t=%f",t.read());
tgt(a*2/3,b*2/3);
} else {
tgt(a,b);
t.stop();
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+5&&a<b) { //sasetsu
t.start();
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);
} else if(t.read_ms()>=100&&t.read_ms()<200) {
pc.printf("t=%f",t.read());
tgt(a*2/3,b*2/3);
} else {
tgt(a,b);
t.stop();
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 {
i++;
t.reset();
pc.printf("reset\r\n");
}
}
int main(void)
{
printf("start\r\n");
motor_tick.attach(&calOmega,0.05);
motorR.setDOconstant(34.1);
motorL.setDOconstant(30);//c
motorR.setPDparam(0,0);
motorR.setPDparam(0,0);//pd//akirameta
gyro.initialize(); //main関数の最初に一度だけ実行
EC1.setDiameter_mm(50);//sokuteirinnhannkei
double getDistance_mm();
//int EC1.getCount()=0;
EC1.reset();
void reset ();
servo.period_ms(20);
motor_f.period_ms(30);
motor_b.period_ms(30);//arm
while(1) {
// motorR.turnF(0.3);
//motorL.turnF(0.3);//for debug
motorR.Sc(target_R);
motorL.Sc(target_L);//target_R,target_Lの値を変えることで直進・後退・右回転・左回転を行う
angle=gyro.getAngle(); //角度の値を受け取る
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);
double d_x=d_dist*sin(angle);
double d_y=d_dist*cos(angle);
x=x+d_x;
y=y+d_y;
if(kai>=3) {
new_dist=EC1.getDistance_mm();
d_dist=new_dist-old_dist;
old_dist=new_dist;
//double d_x=d_dist*sin(angle);;
//double d_y=d_dist*cos(angle);;
//x=x+d_x;
//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("i=%d\r\n",i);
//pc.printf("EC1=%f",EC1.getDistance_mm(),EC1.getCount());
pc.printf("x=%f y=%f",x,y);
//pc.printf("angle=%f\r\n",angle);
//機体の進む方向、右モーターの角速度、左モーターの角速度を表示
kai=0;
}
kai++;
if(i==0) {
if(reset_f.read()==1) {
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(i==1) {
susumu_y(1,1,407);
}
if(i==2) {
susumu_ang(1/3,1,45);
}
if(i==3) {
susumu_xl(1,1,709);
}
if(i==4) {
susumu_ang(1,1/3,0);
}
if(i==5) {
susumu_y(1,1,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();
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(angle>=-89) {
target_R=BASIC_SPEED/5;
target_L=BASIC_SPEED/(-5);
}
if(angle<=-91) {
target_R=BASIC_SPEED/(-5);
target_L=BASIC_SPEED/-5;
}
if(angle>-91&angle<-89) {
motorR.stop();
motorL.stop();
wait(0.5);
if(angle>-91&angle<-89) {
i++;
}
}
}//kakudo tyousei
if(i==8) {
susumu_xr(1,1,700);
}
if(i==9) {
susumu_ang(1/3,1,0);
}
if(i==10) {
susumu_y(1,1,2200);
}
if(i==11) {
susumu_ang(1/3,1,90);
}
if(i==12) {
motorR.stop();
motorL.stop();
servo.pulsewidth_us(1000);
wait(0.5);
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;
}
if(angle>89&angle<91) {
motorR.stop();
motorL.stop();
wait(0.1);
if(angle>89&angle<91) {
i++;
}
}
}
if(i==14) {
susumu_xl(1,1,1000);
}
if(i==15) {
motorR.stop();
motorL.stop();
wait(0.5);
denjiben=1;
wait(0.5);
servo.pulsewidth_us(1500);
wait(0.5);
i++;
}
if(i==16) {
susumu_xr(-1,-1,700);
}
if(i==17) {
susumu_ang(-1/3,-1,0);
}
if(i==18) {
susumu_y(-1,-1,2000);
}
if(i==19) {
susumu_ang(-1/3,-1,-90);
}
if(i==20) {
susumu_xl(-1,-1,1100);
}
if(i==21) {
motorR.stop();
motorL.stop();
servo.pulsewidth_us(1500);
wait(0.5);
denjiben=0;
wait(0.5);
break;
}
}//while
tgt(0,0);
return 0;
}//intmain