fs

Dependents:   ARAI45th 3servotest 1stcomp

Files at this revision

API Documentation at this revision

Comitter:
sakanakuuun
Date:
Wed Sep 07 04:57:47 2016 +0000
Parent:
6:0aa97a99c9cb
Child:
8:3bf4addaaedd
Commit message:
aaaaaaa

Changed in this revision

move.cpp Show annotated file Show diff for this revision Revisions of this file
move.h Show annotated file Show diff for this revision Revisions of this file
--- 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;
 }
--- a/move.h	Tue Sep 06 10:18:17 2016 +0000
+++ b/move.h	Wed Sep 07 04:57:47 2016 +0000
@@ -1,4 +1,4 @@
-#define ALLOW_RAD   0.005
+#define ALLOW_RAD   0.008
 #define CW          true
 #define CCW         false