Diff: move.cpp

Revision:

0:d7ff86f25eaa

Child:

1:405e28b64fdb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/move.cpp	Fri Sep 02 12:40:45 2016 +0000
@@ -0,0 +1,257 @@
+#include "mbed.h"
+#include "move.h"
+#include "locate.h"
+
+PwmOut M1cw(PA_11);
+PwmOut M1ccw(PB_15);
+PwmOut M2ccw(PB_14);
+PwmOut M2cw(PB_13);
+
+//const int allowlength=5;
+//const float allowdegree=0.02;
+const int rightspeed=29*2;
+const int leftspeed=31*2;
+const int turnspeed=15*2;
+
+//const float PIfact=2.89;
+
+
+void initmotor()
+{
+    M1cw.period_us(256);
+    M1ccw.period_us(256);
+    M2cw.period_us(256);
+    M2ccw.period_us(256);
+
+}
+
+void move(int left,int right)
+{
+
+    float rightduty,leftduty;
+
+    if(right>256) {
+        right=256;
+    }
+    if(left>256) {
+        left=256;
+    }
+    if(right<-256) {
+        right=-256;
+    }
+    if(left<-256) {
+        left=-256;
+    }
+
+    rightduty=right/256.0;
+    leftduty=left/256.0;
+    if(right>0) {
+        M1cw.write(1-rightduty);
+        M1ccw.write(1);
+    } else {
+        M1cw.write(1);
+        M1ccw.write(1+rightduty);
+    }
+
+    if(left>0) {
+        M2cw.write(1-leftduty);
+        M2ccw.write(1);
+    } else {
+        M2cw.write(1);
+        M2ccw.write(1+leftduty);
+    }
+}
+
+
+void movelength(int length)
+{
+    int px,py;
+    update();
+    px=coordinateX();
+    py=coordinateY();
+    //int pt=coordinateTheta();
+
+    move(rightspeed,leftspeed);
+    while(1) {
+
+        update();
+        //pc.printf("dx:%d, dy:%d, l:%d x:%d y:%d t:%f\n\r",px-coordinateX(),py-coordinateY(),length,coordinateX(),coordinateY(), coordinateTheta());
+        if(((px-coordinateX())*(px-coordinateX())+(py-coordinateY())*(py-coordinateY()))>length*length) {
+            break;
+        }
+
+    }
+    move(0,0);
+}
+
+void turncw()
+{
+    float pt;
+    move((-1)*turnspeed,turnspeed);
+
+    update();
+    pt=coordinateTheta();
+
+    while(1) {
+        update();
+        if(pt-coordinateTheta()>PI/2) {
+            move(0,0);
+            break;
+        }
+    }
+}
+
+void turnccw()
+{
+    float pt;
+    move(turnspeed,(-1)*turnspeed);
+
+    update();
+    pt=coordinateTheta();
+
+    while(1) {
+        update();
+        if(pt-coordinateTheta()<(-1)*PI/2) {
+            move(0,0);
+            break;
+        }
+    }
+}
+
+void turn_cw()
+{
+    float pt;
+    move((-1)*turnspeed,turnspeed);
+
+    update();
+    pt=coordinateTheta();
+
+    while(1) {
+        update();
+        if(pt-coordinateTheta()>PI/2) {
+            move(0,0);
+            break;
+        }
+    }
+
+    hosei_turn(pt, true, PI/2);
+}
+
+void turn_ccw()
+{
+    float pt;
+    move(turnspeed,(-1)*turnspeed);
+
+    update();
+    pt=coordinateTheta();
+
+    while(1) {
+        update();
+        if(pt-coordinateTheta()<(-1)*PI/2) {
+            move(0,0);
+            break;
+        }
+    }
+
+    hosei_turn(pt, false, PI/2);
+}
+
+void hosei_turn(float pt, bool cw, float rad)
+{
+    int np;
+    if(cw) np = 1;
+    else   np = -1;
+    while(1) {
+        update();
+        if(pt-coordinateTheta() < np * rad - ALLOW_RAD) {
+            move(-15, 15);
+        } else if(pt-coordinateTheta() > np * rad + ALLOW_RAD) {
+            move(15, -15);
+        } else {
+            move(0,0);
+            return;
+        }
+    }
+}
+
+void turn_direction(int rad)
+{
+    move(turnspeed, (-1)*turnspeed);
+
+    update();
+
+    while(1) {
+        update();
+        if(coordinateTheta() < rad + 0.1 && coordinateTheta() > rad - 0.1) {
+            move(0,0);
+            break;
+        }
+    }
+
+    hosei_turn(0, false, rad);        
+}
+void pmovex(int length)
+{
+    int px,py,ptheta,dx,dy,dtheta;
+    int k_y=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
+    int k_theta=10;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
+
+    update();
+    px=coordinateX();
+    py=coordinateY();
+    ptheta=coordinateTheta();
+    move(rightspeed,leftspeed);
+
+    while(1) {
+        update();
+        dx=coordinateX()-px;
+        dy=coordinateY()-py;
+        dtheta=coordinateTheta()-ptheta;
+
+        if(dy>7) {
+            dy=7;
+        }
+        if(dy<-7) {
+            dy=-7;
+        }
+        move(rightspeed-k_y*dy-k_theta*dtheta,leftspeed+k_y*dy+k_theta*dtheta);
+
+        if(dx>length) {
+            move(0,0);
+            break;
+        }
+    }
+}
+
+void pmovey(int length)
+{
+    int px,py,ptheta,dx,dy,dtheta;
+    int k_x=0.7;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
+    int k_theta=10;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
+
+    update();
+    px=coordinateX();
+    py=coordinateY();
+    ptheta=coordinateTheta();
+    move(rightspeed,leftspeed);
+
+    while(1) {
+        update();
+        dx=coordinateX()-px;
+        dy=coordinateY()-py;
+        dtheta=coordinateTheta()-ptheta;
+
+        if(dx>7) {
+            dx=7;
+        }
+        if(dx<-7) {
+            dx=-7;
+        }
+        move(rightspeed-k_x*dx-k_theta*dtheta,leftspeed+k_x*dx+k_theta*dtheta);
+
+        if(dy>length) {
+            move(0,0);
+            break;
+        }
+    }
+}