Diff: main.cpp

Revision:

0:a92631a4021d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Nov 03 23:13:31 2018 +0000
@@ -0,0 +1,206 @@
+#include "mbed.h"
+#include "F3rc.h"
+
+int main()
+{
+    gyro.initialize();    //main関数の最初に一度だけ実行
+    gyro.acc_offset();    //やってもやらなくてもいい
+
+    printf("start\r\n");
+    motor_tick.attach(&calOmega,0.05);
+    motorR.setDOconstant(30);
+    motorL.setDOconstant(30);//c
+    motorR.setPDparam(0.1,0);
+    motorL.setPDparam(0.1,0);
+
+
+
+    EC1.setDiameter_mm(48);//sokuteirinnhannkei
+    double  getDistance_mm();
+    void reset  ();
+    EC1.reset();
+
+    motor_f.period_ms(30);
+    motor_b.period_ms(30);//arm
+
+    while(1) {
+        angle=gyro.getAngle()*(-1);    //角度の値を受け取る
+        EC1.getDistance_mm();
+        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;
+        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) {
+            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) {
+            servo.pulsewidth_us(950);
+            if(reset_f.read()==0&&button.read()==0) {
+                wait(0.05);
+                if(reset_f.read()==0&&button.read()==0) {
+                    denjiben=1;
+                    x=0;
+                    y=0;
+                    i++;
+                }
+            }
+            if(reset_a.read()==0&&button.read()==0) {
+                wait(0.05);
+                if(reset_a.read()==0&&button.read()==0) {
+                    denjiben=1;
+                    x=0;
+                    y=0;
+                    i=21;
+                }
+            }//x,y
+        }
+        if(i==1) {
+            wait(3);
+            servo.pulsewidth_us(950);
+            wait(1);
+            i++;
+        }
+
+        if(i==2) {
+            susumu_y(1,1,1000);
+        }
+        if(i==3) {
+
+            motorR.stop();
+            motorL.stop();
+            servo.pulsewidth_us(2200);
+            wait(1);
+            denjiben=0;
+            wait(1);
+            i++;
+        }
+        if(i==4) {
+            servo.pulsewidth_us(950);
+            wait(1);
+            i++;
+        }
+        if(i==5) {
+            susumu_y(-1,-1,200);
+        }
+        if(i==6) {
+            motorR.stop();
+            motorL.stop();
+            denjiben=1;
+            wait(1);
+            i++;
+        }
+
+        if(i==7) {
+            tgt(0,0);
+        }
+        if(i==10) {
+            t.start();
+            while(t<1) {
+                motor_f=0.82;
+                motor_b=0;
+            }
+            t.stop();
+            t.reset();
+
+            t.start();
+            while(t<1) {
+                motor_f=0;
+                motor_b=0.82;
+            }
+            t.stop();
+            t.reset();
+            motor_f=0;
+            motor_b=0;
+            i++;
+        }
+        if(i==11) {
+            susumu_y(1,1,1000);
+        }
+        if(i==12) {
+            susumu_ang(0.5,1,90);
+        }
+        if(i==13){
+            susumu_xl(1,1,500);
+        }
+        if(i==14){
+            susumu_ang(0.5,1,180);
+        }
+        if(i==15) {
+            susumu_y(1,1,415);
+        }
+        if(i==16) {
+            susumu_ang(0.5,1,270);
+        }
+        if(i==17){
+            susumu_xr(1,1,585);
+        }
+        if(i==18){
+            susumu_ang(0.5,1,360);
+        }
+        if(i==19){
+            i=11;
+            }
+
+        if(i==21) {
+            susumu_y(1,1,300);
+        }
+        if(i==22) {
+            susumu_ang(0.333,1,270);
+        }
+        if(i==23){
+            susumu_xr(1,1,-300);
+        }
+        if(i==24){
+            susumu_ang(1,0.333,0);
+        }
+        if(i==25) {
+            susumu_y(1,1,300);
+        }
+        if(i==26) {
+            susumu_ang(1,0.333,-270);
+        }
+        if(i==27){
+            susumu_xl(1,1,300);
+        }
+        if(i==28){
+            susumu_ang(0.333,1,0);
+        }
+        if(i==29){
+            susumu_y(1,1,0);
+            }
+
+
+
+
+
+
+
+
+    }
+
+
+
+
+
+
+
+
+
+}