Diff: move.cpp

Revision:

15:39c7e97b37c4

Parent:

14:138af628d979

--- a/move.cpp	Fri Sep 16 08:49:56 2016 +0000
+++ b/move.cpp	Sat Sep 17 23:20:56 2016 +0000
@@ -7,10 +7,11 @@
 
 /*************/
 int rightspeed = 70;
-int leftspeed = (int)(rightspeed);
-const int hosei_turnspeed = 13;
+int leftspeed = (int)(rightspeed)+7;
+const int hosei_turnspeed = 14;
 const int max_disorder = 3;
-const float ratio = 1.0/8.0;
+const float ratio = 1.0/7.0;
+const int t_spe[] = {50,50};
 /*************/
 
 PwmOut M1cw(PA_11);
@@ -22,7 +23,9 @@
 DigitalOut yellow(PC_3);
 DigitalOut red   (PC_0);
 
-Serial pc2(SERIAL_TX, SERIAL_RX);    //pcと通信
+//Serial pc(SERIAL_TX, SERIAL_RX);    //pcと通信
+
+
 
 int tcolor;
 
@@ -110,10 +113,10 @@
     else if(coordinateTheta() < rad) np = -1;
     else return;
 
-    move((-np)*rightspeed, (np)*leftspeed);
-
+    //move((-np)*rightspeed, (np)*leftspeed);
+    move((-np) * t_spe[0], (np) * t_spe[1]);
     while(1) {
-        update_np();
+        update_np();\
         if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) {
             move(0,0);
             break;
@@ -139,8 +142,9 @@
 
     update();
 
-    move(rightspeed, -leftspeed);
-
+    //move(rightspeed, -leftspeed);
+    move(t_spe[0], -t_spe[1]);
+    
     while(1) {
 
 
@@ -169,8 +173,8 @@
 
     update();
 
-    move((-1)*rightspeed, leftspeed);
-
+    //move((-1)*rightspeed, leftspeed);
+    move(-t_spe[0], t_spe[1]);
     while(1) {
         update();
         if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) {
@@ -187,6 +191,7 @@
 }
 
 
+
 void movelength(int length)
 {
     int px,py,pt;
@@ -205,11 +210,34 @@
         }
 
     }
+    move(-32,-30);
+    wait(0.05);
+    move(0,0);
+    
+}
+
+
+void movelengthnoprintf(int length)
+{
+    int px,py,pt;
+    update();
+    px=coordinateX();
+    py=coordinateY();
+    pt=coordinateTheta();
+
+    move(rightspeed,leftspeed);
+    while(1) {
+
+        update_np();
+        if(((px-coordinateX())*(px-coordinateX())+(py-coordinateY())*(py-coordinateY()))>length*length) {
+            break;
+        }
+
+    }
     move(0,0);
 }
 
 
-
 void pmove(int x, int y)
 {
     yellow = 1;
@@ -295,10 +323,10 @@
         } else
             daikei = 1;
 
-        move(daikei * (rightspeed*(1-ratio) + k*(*disorder) - k_theta*dtheta) + rightspeed*ratio,
-             daikei * (leftspeed *(1-ratio) - k*(*disorder) + k_theta*dtheta) + leftspeed *ratio);
+        move(daikei * (rightspeed*(1-ratio) - k*(*disorder) - k_theta*dtheta) + rightspeed*ratio,
+             daikei * (leftspeed *(1-ratio) + k*(*disorder) + k_theta*dtheta) + leftspeed *ratio);
 
-        //pc2.printf("d_length:%d disorder:%d rs:%f ls:%f daikei:%f\n\r", *d_length, *disorder, k*(*disorder) - k_theta*dtheta, -k*(*disorder) + k_theta*dtheta, daikei);
+        //pc.printf("d_length:%d disorder:%d rs:%f ls:%f daikei:%f\n\r", *d_length, *disorder, k*(*disorder) - k_theta*dtheta, -k*(*disorder) + k_theta*dtheta, daikei);
         if((direction > 0 && *d_length <= 0) || (direction < 0 &&  *d_length >= 0)) {
             move(0, 0);
             break;
@@ -323,7 +351,7 @@
     float dtheta, ptheta;
     float daikei;
 
-    length = sqrt(pow((double)(x - coordinateX()), 2) + pow((double)(y - coordinateY()), 2));
+    length = sqrt(pow((double)(x - coordinateX()), 2) + pow((double)(y - coordinateY()), 2)) - 10;
 
     if(length == 0) {
         red=1;
@@ -334,7 +362,7 @@
 
     ptheta += nearPi(coordinateTheta() - ptheta);
 
-    pc2.printf("pt:%f", ptheta);
+    //pc.printf("pt:%f", ptheta);
     turnrad(ptheta);
 
     virtual_setup();
@@ -366,7 +394,7 @@
              daikei * (leftspeed *(1-ratio) - k*disorder + k_theta*dtheta) + leftspeed *ratio - k*disorder + k_theta*dtheta);
 
 
-        //pc2.printf("length:%f, d_length:%d, vx:%d, vy:%d\n\r", length, d_length, virtual_coordinateX(), virtual_coordinateY());
+        //printf("length:%f, d_length:%d, vx:%d, vy:%d\n\r", length, d_length, virtual_coordinateX(), virtual_coordinateY());
         if(d_length <= 0) {
             move(0, 0);
             break;
@@ -394,8 +422,6 @@
 
     length = 300;
 
-    turnrad(PI + nearPi(coordinateTheta() - PI));
-
     while(1) {
         update_np();
         dx = coordinateX() - px;
@@ -529,7 +555,7 @@
     turnrad(-PI/2 + nearPi(coordinateTheta() + PI/2));
 
     while(1) {
-        update_np();
+        update();//npの部分を変更した
         dx = coordinateX() - px;
         dy = coordinateY() - py;
 
@@ -639,10 +665,17 @@
 void yokoMove(int cx, bool Left)
 {
     if(Left) {
-        pmove2(cx, 0);
-        pmove2(cx, 900);
+        pmove2(cx, 100);
+        pmove(cx, 1100);
     } else {
-        pmove2(cx, 900);
-        pmove2(cx, 0);
+        pmove2(cx, 1100);
+        pmove(cx, 100);
     }
+}
+
+void moveTriangle(int c0, int c1, int c2, int c3)
+{
+    pmove2(c0  * 100, c1 * 100);
+    pmove2(c2 * 100, c3 * 100);
+    pmove2(0, 900);   
 }
\ No newline at end of file