Revision 1:89408fff7cc9, committed 2013-03-10

Comitter:

akudohune

Date:

Sun Mar 10 07:31:31 2013 +0000

Parent:

0:74bf4953c0d1

Commit message:

new_cup;

Changed in this revision

--- a/main.cpp	Sat Mar 09 10:11:06 2013 +0000
+++ b/main.cpp	Sun Mar 10 07:31:31 2013 +0000
@@ -6,6 +6,44 @@
 #include "main.h"
 
 
+
+void PidUpdata()
+{    
+    inputPID = (((int)(compass.sample() - (standard * 10.0) + 5400.0) % 3600) / 10.0);        
+    
+    //pc.printf("%f\n",timer1.read());
+    pid.setProcessValue(inputPID);
+    //timer1.reset();
+    
+    compassPID = -(pid.compute());
+    
+    //pc.printf("%f\n",compassPID);
+
+}
+
+void move(int vxx, int vyy, int vss)
+{
+    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);
+    
+    for(uint8_t i = 0 ; i < MOT_NUM ; i++){
+        if(motVal[i] > MAX_POW)motVal[i] = MAX_POW;
+        else if(motVal[i] < MIN_POW)motVal[i] = MIN_POW;
+        speed[i] = motVal[i];
+    }
+    /*
+    pc.printf("speed1 = %d\n",speed[0]);
+    pc.printf("speed2 = %d\n",speed[1]);
+    pc.printf("speed3 = %d\n",speed[2]);
+    pc.printf("speed4 = %d\n\n",speed[3]);      
+    */
+    ////pc.printf("%s",StringFIN.c_str());
+}
+
 /***********  Serial interrupt  ***********/
 
 void Tx_interrupt()
@@ -26,67 +64,6 @@
 
 /***********  Serial interrupt end **********/
 
-void PidUpdata()
-{    
-    
-    if(standard < 180.0){
-        if((compass.sample() / 10.0) < standard){
-            inputPID = 180.0 -(standard - (compass.sample() / 10.0));
-        }else if((compass.sample() / 10.0) < 180.0 + standard){
-            inputPID = 180.0 +((compass.sample() / 10.0) - standard);
-        }else{
-            inputPID = 180.0 - ((360.0 - (compass.sample() / 10.0)) + standard);
-        }
-    }else if(standard > 180.0){
-        if((compass.sample() / 10.0) > standard){
-            inputPID = 180.0 +((compass.sample() / 10.0) - standard);
-        }else if((compass.sample() / 10.0) > standard - 180.0){
-            inputPID = 180.0 -(standard - (compass.sample() / 10.0));
-        }else{
-            inputPID = 180.0 + ((compass.sample() / 10.0) + (360.0 - standard));
-        }
-    }else{
-        inputPID = compass.sample() / 10.0;
-    }
-    
-    if(inputPID < 0){
-        inputPID *= -1;
-    }
-        
-    //pc.printf("%f\n",timer1.read());
-    pid.setProcessValue(inputPID);
-    //timer1.reset();
-    
-    compassPID = -(pid.compute());
-    
-    //pc.printf("%f\n",compassPID);
-
-}
-
-
-
-void move(int vx, int vy, int vs)
-{
-    double motVal[MOT_NUM] = {0};
-    
-    motVal[0] = (double)(((0.5 * vx)  + ((sqrt(3.0) / 2.0) * vy) + (Long * -vs)) * MOT1);
-    motVal[1] = (double)(((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long *  vs)) * MOT2);
-    motVal[2] = (double)(((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * -vs)) * MOT3);
-    motVal[3] = (double)(((0.5  * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long *  vs)) * MOT4);
-    
-    for(uint8_t i = 0;i < MOT_NUM;i++){
-        if(motVal[i] > 100)motVal[i] = 100;
-        else if(motVal[i] < -100)motVal[i] = -100;
-        speed[i] = motVal[i];
-    }
-    /*
-    pc.printf("speed1 = %d\n",speed[0]);
-    pc.printf("speed2 = %d\n",speed[1]);
-    pc.printf("speed3 = %d\n",speed[2]);
-    pc.printf("speed4 = %d\n\n",speed[3]);      
-   */
-    ////pc.printf("%s",StringFIN.c_str());
-}
 
 void init()
 {
@@ -114,6 +91,7 @@
     }
     
     standard = compass.sample() / 10.0;
+    
     led1 = OFF;
     led3 = OFF;
     
@@ -146,34 +124,27 @@
         vs = compassPID;
         //vx = 15;
         //vy = 10;
+        
         /*
         if(IR_found){
             if((direction == 4) || (direction == 12)||(direction == 3) || (direction == 5) || (direction ==11) || (direction == 13)){
-                vx = (int)(20*ball_sankaku[direction][0]);
-                vy = (int)(20*ball_sankaku[direction][1]);
+                vx = (int)(70*ball_sankaku[direction][0]);
+                vy = (int)(70*ball_sankaku[direction][1]);
             }else{
-                vx = (int)(10*ball_sankaku[direction][0]);
-                vy = (int)(10*ball_sankaku[direction][1]);
-            }
-            
-            if(Distance <= 10){
-                vx *= -1;
-                vy *= -1;
+                vx = (int)(70*ball_sankaku[direction][0]);
+                vy = (int)(70*ball_sankaku[direction][1]);
             }
         }else{
             vx = 0;
             vy = 0;
         }
         */
-        /*
-        if((ultrasonicVal[2] < 700) && (inputPID < 190) && (inputPID > 170))vx = -15;
-        else vx = 0;
-        */
+        
         /*
         if(IR_found)state = DIFFENCE;
         else state = HOME_WAIT;
-        
-        
+        */
+        /*
         if(state == HOME_WAIT){
             if((ultrasonicVal[0] + ultrasonicVal[2]) < 6000){
                 if(ultrasonicVal[0] > 3200){
@@ -183,7 +154,7 @@
                     vx = -15;
                     vy = 0;
                 }else{
-                    if(ultrasonicVal[1] > 700){
+                    if(ultrasonicVal[1] > 800){
                         vx = 0;
                         vy = -15;
                     }else{
@@ -196,19 +167,61 @@
                 vy = 0;
             }
         }else if(state == DIFFENCE){
-            if((direction == 1) || (direction == 2) || (direction == 3) || (direction == 4) || (direction == 5) || (direction == 6) || (direction == 7)){
-                vx = 15;
+            if(ultrasonicVal[1] > 800){
+                vx = 0;
+                vy = -15;
+            }else{
+                if(distance <= 30){
+                
+                    if((direction == 1) || (direction == 2) || (direction == 3) || (direction == 4) || (direction == 5) || (direction == 6) || (direction == 7)){
+                        vx = 15;
                 
-            }else if((direction == 9) || (direction == 10) || (direction == 11) || (direction == 12) || (direction == 13) || (direction == 14) || (direction == 15)){
-                vx = -15;
+                    }else if((direction == 9) || (direction == 10) || (direction == 11) || (direction == 12) || (direction == 13) || (direction == 14) || (direction == 15)){
+                        vx = -15;
             
+                    }else{
+                        vx = 0;
+                    }
+                }else{
+                    
+                }
+            }     
+        }
+        */
+        /*
+        if(state == HOME_WAIT){
+           
+        }*/
+        
+            if((ultrasonicVal[0] + ultrasonicVal[2]) < 1050.0){
+                if(ultrasonicVal[0] > 300.0){
+                    vx = 15;
+                    led3 = ON;
+                    led4 = OFF;
+                }else if(ultrasonicVal[2] > 300.0){
+                    vx = -15;
+                    led3 = ON;
+                    led4 = OFF;
+                }else{
+                    led3 = OFF;
+                    led4 = ON;
+                    if(ultrasonicVal[1] > 200.0){
+                        vy = -15;
+                    }else if(ultrasonicVal[1] < 100.0){
+                        vy = 15;
+                    }else{
+                        vy = 0;
+                    }
+                }
+                led2 = ON;
             }else{
                 vx = 0;
-                
+                vy = 0;
+                led2 = OFF;
             }
-        }  
-        */
-        
+            
+        //pc.printf("%f,%f,%f\n",ultrasonicVal[0],ultrasonicVal[1],ultrasonicVal[2]);
+            
         move(vx,vy,vs);
     }
 }

--- a/main.h	Sat Mar 09 10:11:06 2013 +0000
+++ b/main.h	Sun Mar 10 07:31:31 2013 +0000
@@ -10,23 +10,26 @@
 #define ON      1
 #define OFF     0
 
-#define MOT1    1
-#define MOT2    1
-#define MOT3    1
-#define MOT4    1
+#define MOT1    1.0
+#define MOT2    1.0
+#define MOT3    1.0
+#define MOT4    1.0
 
-#define OUT_LIMIT 30.0
+#define OUT_LIMIT   30.0
+#define MAX_POW     100
+#define MIN_POW     -100
 
 DigitalOut led1(LED1); 
 DigitalOut led2(LED2);
 DigitalOut led3(LED3);
+DigitalOut led4(LED4);
 HMC6352 compass(p9, p10);
 Serial driver(p28, p27); // tx, rx 
 Serial pc(USBTX, USBRX); // tx, rx 
 DigitalIn StartButton(p21);
 DigitalIn CalibEnterButton(p22);
 DigitalIn CalibExitButton(p23);
-PID pid(0.42, 1.0, 0.013, RATE);      //30.0 0.35 1.0 0.012
+PID pid(0.59, 1.0, 0.015, RATE);      //30.0 0.35 1.0 0.012 30.0 0.42 1.0 0.013 
 Ticker pidUpdata;
 Ticker IR;
 Ticker ultrasonic;
@@ -36,7 +39,7 @@
 
 
 int speed[MOT_NUM] = {0};
-
+ 
 static float lastData = 0.0;
 static float inputPID = 180.0;
 static float standard;
@@ -48,7 +51,7 @@
 extern int Distance;
 extern int IR_found;
 extern double ball_sankaku[16][2];
-extern uint16_t ultrasonicVal[3];
+extern double ultrasonicVal[3];
 
 extern void Ultrasonic(void);
 extern void IR_Position(void);

--- a/ultrasonic.cpp	Sat Mar 09 10:11:06 2013 +0000
+++ b/ultrasonic.cpp	Sun Mar 10 07:31:31 2013 +0000
@@ -6,7 +6,7 @@
 extern Timer timer2;
 extern Serial pc; // tx, rx 
 
-uint16_t ultrasonicVal[ALL_ULTRASONIC] = {0};
+double ultrasonicVal[ALL_ULTRASONIC] = {0};
 
 
 void Ultrasonic()
@@ -36,10 +36,10 @@
             }
         }
         if(flag == 0){
-            ultrasonicVal[i] = timer2.read_us();
+            ultrasonicVal[i] = timer2.read_us() / 1000000.0 / 2.0 * 340.0 * 1000.0; //cm
         }
-        //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
     }
-    pc.printf("%d\n",ultrasonicVal[0] + ultrasonicVal[2]);
+    //pc.printf("%f\n",ultrasonicVal[0] + ultrasonicVal[2]);
+    //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
     
 }

--- a/ultrasonic.h	Sat Mar 09 10:11:06 2013 +0000
+++ b/ultrasonic.h	Sun Mar 10 07:31:31 2013 +0000
@@ -1,7 +1,9 @@
-
+#include "HMC6352.h"
 
 #define ALL_ULTRASONIC  3
-#define PING_ERR    0xFFFF
+#define PING_ERR        0xFFFF
+
+extern HMC6352 compass;
 
 
 PinName ultrasonic_pin[ALL_ULTRASONIC] = {