Diff: main.cpp

Revision:

4:1604d599d40f

Parent:

3:56b034c41dc5

Child:

5:97d9a34e5016

--- a/main.cpp	Fri Sep 02 06:50:14 2016 +0000
+++ b/main.cpp	Fri Sep 02 12:41:13 2016 +0000
@@ -1,122 +1,23 @@
 #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)
-{
+#include "locate.h"
+#include "move.h"
 
-    float rightduty,leftduty;
-
-    if(right>256) {
-        right=256;
-    }
-    if(left>256) {
-        left=256;
-    }
-    if(right<-256) {
-        right=-256;
-    }
-    if(left<-256) {
-        left=-256;
-    }
+int main()
+{
+    setup();
+    initmotor();
 
-    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);
-    }
+    turn_ccw();
 
-    if(left>0) {
-        M2cw.write(1-leftduty);
-        M2ccw.write(1);
-    } else {
-        M2cw.write(1);
-        M2ccw.write(1+leftduty);
+    while(1) {
+        update();
     }
 }
 
 
-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();
@@ -127,36 +28,4 @@
         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();
-
-
-}
\ No newline at end of file
+}*/
\ No newline at end of file