Diff: move.cpp

Revision:

7:7f1721542753

Parent:

6:0aa97a99c9cb

Child:

8:3bf4addaaedd

--- a/move.cpp	Tue Sep 06 10:18:17 2016 +0000
+++ b/move.cpp	Wed Sep 07 04:57:47 2016 +0000
@@ -7,12 +7,17 @@
 PwmOut M1ccw(PB_15);
 PwmOut M2ccw(PB_14);
 PwmOut M2cw(PB_13);
+
+DigitalOut green (PC_2);
+DigitalOut yellow(PC_3);
+DigitalOut red   (PC_0);
+
 /*
 DigitalOut teamledblue(PC_10);
 DigitalOut teamledred(PC_12);
 */
 
-//Serial pc(SERIAL_TX, SERIAL_RX);    //pcと通信
+Serial pc2(SERIAL_TX, SERIAL_RX);    //pcと通信
 
 
 //const int allowlength=5;
@@ -80,9 +85,9 @@
         update();
         //pc.printf("t:%f\n\r", coordinateTheta());
         if(pt-coordinateTheta() < np * rad - ALLOW_RAD) {
-            move(-15, 15);
+            move(-10, 10);
         } else if(pt-coordinateTheta() > np * rad + ALLOW_RAD) {
-            move(15, -15);
+            move(10, -10);
         } else {
             move(0,0);
             return;
@@ -93,6 +98,8 @@
 
 void turn_abs_rad(float rad)
 {
+    green = 1;
+
     update();
 
     int np;
@@ -120,39 +127,49 @@
     hosei_turn(0, false, rad);
 
     wait(0.5);
+    green = 0;
+
 }
 
 void pmove(int x, int y)
-{ 
+{
+    yellow = 1;
+
     float k = 0.9;//ズレ(mm)を回転数に反映させる比例定数
-    
+
     int   k_theta = 2;//ズレ(rad)を回転数に反映させる比例定数
-    
+
     int length, dx, dy;
     int *d_length, *disorder;
     int absd_length;
     float dtheta, ptheta;
     float daikei;
 
-    char direction;
+    int direction;
+    
     if(abs(x - coordinateX()) > abs(y - coordinateY())) {
         y = coordinateY();
         direction = X_PLUS;
         length = abs(x - coordinateX());
         d_length = &dx;
         disorder = &dy;
-    }
-    else {
+    } else {
         x = coordinateX();
         direction = Y_PLUS;
         length = abs(y - coordinateY());
         d_length = &dy;
         disorder = &dx;
     }
+    
+    update();
+    dx = x - coordinateX();
+    dy = y - coordinateY();
 
-    if(*d_length < 0)
+    if(*d_length < 0)   //x,y減少方向なら、*d_length<0
         direction *= -1;
 
+    pc2.printf("direction:%d", direction);
+
     switch(direction) {
         case X_PLUS:
             ptheta = 0;
@@ -177,21 +194,21 @@
     if(length == 0) return;
 
     while(1) {
-        update();
+        update_np();
         dx = x - coordinateX();
         dy = y - coordinateY();
         dtheta = coordinateTheta() - ptheta;
 
-        if(*disorder>1) {
-            *disorder = 1;
-        } else if(*disorder<-1) {
-            *disorder = -1;
+        if(*disorder>2) {
+            *disorder = 2;
+        } else if(*disorder<-2) {
+            *disorder = -2;
         }
 
         absd_length = abs(*d_length);
 
         if(absd_length > length - 30) {
-            daikei = (length - absd_length) / 30.0;
+            daikei = abs(length - absd_length) / 30.0;
         } else if(absd_length < 150) {
             daikei = absd_length / 150.0;
         } else
@@ -200,36 +217,41 @@
         move(daikei * (rightspeed*6/7.0 + k*(*disorder) - k_theta*dtheta) + rightspeed/7.0,
              daikei * (leftspeed*6/7.0  - k*(*disorder) + k_theta*dtheta) + leftspeed/7.0);
 
+        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);
         if((direction > 0 && *d_length <= 0) || (direction < 0 &&  *d_length >= 0)) {
             move(0, 0);
             break;
         }
 
-        //pc.printf("d_length:%d disorder:%d rs:%f ls:%f g:%f\n\r", *d_length, *disorder, (rightspeed/2.0 - k*(*disorder) - k_theta*dtheta) + rightspeed/2.0, (leftspeed/2.0 + k*(*disorder) + k_theta*dtheta) + leftspeed/2.0, daikei);
     }
 
     wait(0.5);
+
+    yellow = 0;
 }
 
 
 void back300()
-{ 
+{
+    red = 1;
+
     float k = 0.9;
     int   k_theta = 2;
 
     int length, px, py, dx, dy;
     float daikei;
-    
+
     update();
-    
-    px = coordinateX(); py = coordinateY();
-    
+
+    px = coordinateX();
+    py = coordinateY();
+
     length = 300;
-    
+
     if(length == 0) return;
 
     turn_abs_rad(PI);
-    
+
     while(1) {
         update();
         dx = coordinateX() - px;
@@ -240,11 +262,11 @@
         } else if(dy<-2) {
             dy = -2;
         }
-    
+
 
-        move(-30*6/7.0 - k*dy, -33 + k*dy);
+        move(-(30 + k*dy), -(31 - k*dy));
 
-        
+
         if(dx>length) {
             move(0, 0);
             break;
@@ -254,5 +276,6 @@
     }
 
     wait(0.5);
-    
+
+    red = 0;
 }