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:
- 0:29c024d6882f
- Child:
- 1:1817e9243a6e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Sep 07 06:32:23 2018 +0000
@@ -0,0 +1,479 @@
+
+#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);
+
+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=250;
+double y=150;//start point
+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)
+{
+ target_R=BASIC_SPEED*r;
+ target_L=BASIC_SPEED*l;
+}
+
+
+void susumu_y(double ay,double by,double 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);
+ }
+ 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);
+ }
+ }
+ if(y<goaly&&y>=goaly-50&&ay>=0) {
+ tgt(ay/3,by/3);
+
+
+ }
+ 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);
+ }
+ 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);
+ }
+ }
+ if(y>goaly&&y<=goaly+50&&ay<0) {
+ tgt(ay/3,by/3);
+ } else {
+ i++;
+ t.reset();
+ pc.printf("owari\r\n");
+ }
+}
+
+
+
+void susumu_x(double ax,double bx,double goalx)
+{
+
+ if(x<goalx-50&&ax>=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(ax/3,bx/3);
+ }
+ if(t.read_ms()>=100&&t.read_ms()<200) {
+ pc.printf("t=%f",t.read());
+ tgt(ax*2/3,bx*2/3);
+ } else {
+ tgt(ax,bx);
+ t.stop();
+ pc.printf("R=%f L=%f",target_R,target_L);
+ }
+ }
+ if(x<goalx&&x>=goalx-50&&ax>=0) {
+ tgt(ax/3,bx/3);
+
+ }
+ if(x>goalx+50&&ax<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(ax/3,bx/3);
+ }
+ if(t.read_ms()>=100&&t.read_ms()<200) {
+ pc.printf("t=%f",t.read());
+ tgt(ax*2/3,bx*2/3);
+ } else {
+ tgt(ax,bx);
+ t.stop();
+ pc.printf("R=%f L=%f",target_R,target_L);
+ }
+ }
+ if(x>goalx&&x<=goalx+50&&ax<0) {
+ tgt(ax/3,bx/3);
+
+ } else {
+ i++;
+ t.reset();
+ pc.printf("owari\r\n");
+ }
+}
+
+
+void susumu_ang(double a,double b,double goal)
+{
+ 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);
+ }
+ 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);
+ }
+ }
+ if(angle<goal&&angle>=goal-5&&a>b) {
+ tgt(a/3,b/3);
+
+ }
+ 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);
+ }
+ 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);
+ }
+ }
+ 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
+
+
+ gyro.initialize(); //main関数の最初に一度だけ実行
+ EC1.setDiameter_mm(50);//sokuteirinnhannkei
+ double getDistance_mm();
+ //int EC1.getCount()=0;
+ EC1.reset();
+ void reset ();
+
+
+ // servo.period_ms(60);
+
+ 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) {
+ t.start();
+ if(t<1) {
+ motor_f=0.8;
+ motor_b=0;
+ } else {
+ motor_f=0;
+ motor_b=0;
+ printf("finish");
+ t.reset();
+ i++;
+ }
+ }//motor test
+
+//kokomade kihonnsettei
+
+
+ /* if(i==0) {
+ printf("a\n\r");
+ target_R=0;
+ target_L=0;
+ i++;
+ pc.printf("i=0\r\n");
+ pc.printf("R=%f L=%f",target_R,target_L);
+ }*/
+ if(i==1) {
+ susumu_y(1,1,472);
+ /* if(y<472) {
+ t.start();
+ pc.printf("R=%f L=%f\r\n",target_R,target_L);
+ if(t.read_ms()<200) {
+ pc.printf("t=%f",t.read());
+ target_R=BASIC_SPEED/3;
+ target_L=BASIC_SPEED/3;
+ } else {
+ target_R=BASIC_SPEED;
+ target_L=BASIC_SPEED;
+ t.stop();
+ pc.printf("R=%f L=%f",target_R,target_L);
+ }//250150 250472
+ } else {
+ i++;
+ t.reset();
+ pc.printf("reset\r\n");
+ */ //}
+ }
+ if(i==2) {
+ susumu_ang(1/3,1,45);
+ /* t.start();
+ if(angle<45) {
+ if(t.read_ms()<200) {
+ target_R=BASIC_SPEED/6;
+ target_L=BASIC_SPEED/2;
+ } else {
+ target_R=BASIC_SPEED/3;
+ target_L=BASIC_SPEED;
+ }//250472 341691
+ } else i++;
+ t.stop();
+ t.reset();
+ */
+ }//kairyoutyuu
+ if(i==3) {
+ susumu_x(1,1,709);
+ // if(x<709) {
+ // target_R=BASIC_SPEED;
+ // target_L=BASIC_SPEED;
+ // //341691 7091059
+ // } else i++;
+ }
+ if(i==4) {
+ susumu_ang(1,1/3,0);
+ /* if(angle>0) {
+ target_R=BASIC_SPEED;
+ target_L=BASIC_SPEED/3;
+ //7091059 8001278
+ } else i++;
+ */
+ }
+ if(i==5) {
+ susumu_y(1,1,1700);
+ // if(y<1700) {
+ // target_R=BASIC_SPEED;
+ // target_L=BASIC_SPEED;
+ // //8001278 8001700
+ // } else i++;
+ motorR.stop();
+ motorL.stop();
+ }
+
+ if(i==6) {
+
+ i++;
+ }//gatiasari
+
+
+ /* if(i==7) {
+ if(angle>=-89) {
+ target_R=BASIC_SPEED/4;
+ target_L=BASIC_SPEED*(-4);
+ }
+ if(angle<=-91) {
+ target_R=BASIC_SPEED/4;
+ target_L=BASIC_SPEED*(-4
+ );
+ }
+ if(angle>-91&angle<89){
+ motorR.stop();
+ motorL.stop();
+ wait(0.5);
+ if(angle>-91&angle<89){
+ i++;}
+ }
+ }
+ if(i==8) {
+ if(x>700) {
+ target_R=BASIC_SPEED;
+ target_L=BASIC_SPEED;
+ //8001700 7001700
+ } else i++;
+ }
+ if(i==9) {
+ if(angle<0) {
+ target_R=BASIC_SPEED/3;
+ target_L=BASIC_SPEED;
+ //7001700 4002000
+ } else i++;
+ }
+ if(i==10) {
+ if(y<2200) {
+ target_R=BASIC_SPEED;
+ target_L=BASIC_SPEED;
+ //4002000 4002200
+ } else i++;
+ }
+ if(i==11) {
+ if(angle<90) {
+ target_R=BASIC_SPEED/3;
+ target_L=BASIC_SPEED;
+ //4002200 7002500
+ } else i++;
+ }
+ if(i==12) {
+ if(x<1000) {
+ target_R=BASIC_SPEED;
+ target_L=BASIC_SPEED;
+ //7002500 10002500
+ } else i++;
+ motorR.stop();
+ motorL.stop();
+ }
+
+ if(i==13){
+ i++;
+ }
+
+
+
+
+ if(i==14) {
+ if(x>700) {
+ target_R=BASIC_SPEED*(-1);
+ target_L=BASIC_SPEED*(-1);
+ //10002500 7002500
+ } else i++;
+ }
+ if(i==15) {
+ if(angle>0) {
+ target_R=BASIC_SPEED/(-3);
+ target_L=BASIC_SPEED/(-1);
+ //7002500 4002200
+ } else i++;
+ }
+ if(i==16) {
+ if(y>2000) {
+ target_R=BASIC_SPEED*(-1);
+ target_L=BASIC_SPEED*(-1);
+ //4002200 4002000
+ } else i++;
+ }
+ if(i==17) {
+ if(angle>-90) {
+ target_R=BASIC_SPEED/(-3);
+ target_L=BASIC_SPEED/(-1);
+ //4002000 7001700
+ } else i++;
+ }
+ if(i==16) {
+ if(x<1100) {
+ target_R=BASIC_SPEED*(-1);
+ target_L=BASIC_SPEED*(-1);
+ //7001700 4002000
+ } else i++;
+ }
+ */
+ //kairyoutyuu kokomade
+
+ }//while
+ motorR.stop();
+ motorL.stop();
+//LR(0,0);
+ return 0;
+
+}//intmain
+