Diff: main.cpp

Revision:

3:56b034c41dc5

Parent:

1:10cc86cabdce

Child:

4:db36396371e0

Child:

6:26ac9f539e8b

--- a/main.cpp	Thu Sep 01 09:19:34 2016 +0000
+++ b/main.cpp	Fri Sep 02 06:50:14 2016 +0000
@@ -1,13 +1,162 @@
 #include "mbed.h"
 #include "locate.h"
+#include "math.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,pt;
+    update();
+    px=coordinateX();
+    py=coordinateY();
+    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()>PIfact/2) {
+            move(0,0);
+            break;
+        }
+    }
+}
+
+void turnccw()
+{
+    float pt;
+    move(turnspeed,(-1)*turnspeed);
+
+    update();
+    pt=coordinateTheta();
+
+    while(1) {
+        update();
+        if(pt-coordinateTheta()<(-1)*PIfact/2) {
+            move(0,0);
+            break;
+        }
+    }
+}
+/*
+int main(){
+    setup();
+    initmotor();
+
+    for(int i=0; i < 4; i++)
+    {
+        movelength(1200);
+        turnccw();
+    }
+}*/
+
+void pmovex(int length){
+    int px,py,dx,dy;
+    int k=1;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
+    update();
+    px=coordinateX();
+    py=coordinateY();
+    move(rightspeed,leftspeed);
+        
+    while(1){
+        update();
+        dx=coordinateX()-px;
+        dy=coordinateY()-py;
+
+        if(dy>7){dy=7;}
+        if(dy<-7){dy=-7;}
+        move(rightspeed-k*dy,leftspeed+k*dy);
+        
+        if(dx>length){
+            move(0,0);
+            break;
+        }
+    }
+}
 
 int main()
 {
     setup();
+    initmotor();
 
-    while(1) {
-        pc.printf("a");
-        update();
-        pc.printf("x:%d     y:%d    t:%f\r\n", coordinateX(), coordinateY(), coordinateTheta());
-    }
-}
+
+}
\ No newline at end of file