Revision 3:b4fb2b5365a7, committed 2013-04-18

Comitter:

yusuke_robocup

Date:

Thu Apr 18 08:42:17 2013 +0000

Parent:

2:09fabba6c00d

Commit message:

new new new

Changed in this revision

diff -r 09fabba6c00d -r b4fb2b5365a7 IR.h
--- a/IR.h	Fri Apr 05 07:26:42 2013 +0000
+++ b/IR.h	Thu Apr 18 08:42:17 2013 +0000
@@ -21,8 +21,8 @@
     p19,
     p20,
     p25,
-    p26,
-    p12
+    p30,
+    p12,
 };
 
 int Convert_Direction[DIRECTION] = {

diff -r 09fabba6c00d -r b4fb2b5365a7 main.cpp
--- a/main.cpp	Fri Apr 05 07:26:42 2013 +0000
+++ b/main.cpp	Thu Apr 18 08:42:17 2013 +0000
@@ -1,3 +1,4 @@
+#include <mbed.h>
 #include <math.h>
 #include <sstream>
 #include "mbed.h"
@@ -30,10 +31,19 @@
 {
     double motVal[MOT_NUM] = {0};
     
-    motVal[0] = (double)(((0.5 * vxx)  + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT1);
-    motVal[1] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long *  vss)) * MOT2);
-    motVal[2] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT3);
-    motVal[3] = (double)(((0.5  * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long *  vss)) * MOT4);
+    int angle = static_cast<int>( atan2( (double)vyy, (double)vxx ) * 180/PI + 360 ) % 360;
+    
+    double hosei1 = 1,hosei2 = 1,hosei3 = 1,hosei4 = 1;
+    
+    if((angle > 30)&&(angle < 80)){
+        hosei2 = 0.7;
+        hosei4 = 0.7;
+    }
+    
+    motVal[0] = (double)(((0.5 * vxx)  + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT1)*hosei1;
+    motVal[1] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long *  vss)) * MOT2)*hosei2;
+    motVal[2] = (double)(((-0.5 * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long * -vss)) * MOT3)*hosei3;
+    motVal[3] = (double)(((0.5  * vxx) + ((sqrt(3.0) / 2.0) * vyy) + (Long *  vss)) * MOT4)*hosei4;
     
     for(uint8_t i = 0 ; i < MOT_NUM ; i++){
         if(motVal[i] > MAX_POW)motVal[i] = MAX_POW;
@@ -167,7 +177,7 @@
         //float now_compass = 180.0;
         /*
         while(1){
-            vx = -10;
+            vx = 10;
             vy = 10;
             vs = compassPID;
             
@@ -182,10 +192,7 @@
         if(((direction != 0)&&(direction != 1)&&(direction != 15)&&(direction != 2)&&(direction != 14))&&(Distance <= 30)){
             state = SNAKE;
         }
-        if(((direction != 0)&&(direction != 1)&&(direction != 15)&&(direction != 2)&&(direction != 14))&&(Distance >= 90)){
-            state = SEARCH;
-        }
-        if((state != SNAKE)&&(Distance >= 90)){
+        if(Distance >= 90){
             state = SEARCH;
         } 
         
@@ -239,27 +246,27 @@
                     vy = 15*ball_sankaku[direction][1];  
                 }
                 if(direction == 1){
-                    vx = 30*ball_sankaku[direction][0];
-                    vy = 8*ball_sankaku[direction][1];  
+                    vx = 20*ball_sankaku[direction][0];
+                    vy = 12*ball_sankaku[direction][1];
+                    vs = -2;  
                 }
                 if(direction == 15){
-                    vx = 30*ball_sankaku[direction][0];
-                    vy = 8*ball_sankaku[direction][1];  
+                    vx = 20*ball_sankaku[direction][0];
+                    vy = 12*ball_sankaku[direction][1];
+                    vs = 2;  
                 }
                 if(direction == 2){
                     vx = 10*ball_sankaku[direction][0];
                     vy = 10*ball_sankaku[direction][1];
-                    vs = -3;  
                 }
                 if(direction == 14){
                     vx = 10*ball_sankaku[direction][0];
                     vy = 10*ball_sankaku[direction][1];
-                    vs = 3;  
                 }
                 
             }else if(state == SEARCH){
-                //vx = 15*ball_sankaku[direction][0];
-                //vy = 15*ball_sankaku[direction][1];
+                vx = 15*ball_sankaku[direction][0];
+                vy = 15*ball_sankaku[direction][1];
                 
                 if(direction == 2){
                     vx = 25*ball_sankaku[direction][0];
@@ -269,7 +276,7 @@
                 if(direction == 14){
                     vx = -25*ball_sankaku[direction][0];
                     vy = 10*ball_sankaku[direction][1];
-                    vs = 2;  
+                    //vs = 2;  
                 }          
                 if(direction == 0){
                     vx = 10*ball_sankaku[direction][0];

diff -r 09fabba6c00d -r b4fb2b5365a7 main.h
--- a/main.h	Fri Apr 05 07:26:42 2013 +0000
+++ b/main.h	Thu Apr 18 08:42:17 2013 +0000
@@ -23,6 +23,8 @@
 
 #define FULL 0
 
+#define PI 3.14159265358979
+