Diff: main.cpp

Revision:

4:db36396371e0

Parent:

3:56b034c41dc5

--- a/main.cpp	Fri Sep 02 06:50:14 2016 +0000
+++ b/main.cpp	Sat Sep 03 06:29:49 2016 +0000
@@ -1,162 +1,21 @@
 #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;
+#include "function.h"
 
-    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();
-
-
+ movelength(30);
+ while(1){
+ open_arm();
+ close_hand();
+ movelength(1);
+ wait(1);
+ close_arm();
+ open_hand();
+ movelength(255);
+ wait(1);
+ }
+ return 0;   
 }
\ No newline at end of file