Revision 3:440e774cc24b, committed 2014-03-12

Comitter:

com3

Date:

Wed Mar 12 04:51:30 2014 +0000

Parent:

2:59edff92b599

Child:

4:536cd493a337

Commit message:

main

Changed in this revision

--- a/common.h	Mon Mar 03 00:24:44 2014 +0000
+++ b/common.h	Wed Mar 12 04:51:30 2014 +0000
@@ -4,4 +4,20 @@
     LEFT,
     BACK,
     RIGHT,
-};
\ No newline at end of file
+};
+
+enum{
+    FF = 0,
+    FL,
+    LL,
+    BL,
+    BB,
+    BR,
+    RR,
+    FR,
+    FFL,
+    FFR,
+    FFFL,
+    FFFR,
+};
+    
\ No newline at end of file

--- a/main.cpp	Mon Mar 03 00:24:44 2014 +0000
+++ b/main.cpp	Wed Mar 12 04:51:30 2014 +0000
@@ -5,21 +5,27 @@
 #include <math.h>
 #include <sstream>
 
-#define MOTOR_P 30
-#define NO_IR 45
+#define MOTOR_P 26
+#define HOME_P 25
+#define NO_BALL 45
 #define LINE_LP 30
-#define LINE_FP 30
+#define LINE_FP 35
 #define LINE_ON 0xFFF0
-#define LINE_TIME 0.5
+#define LINE_TIME 0.6
+#define LINE_PING 40
 #define R 1.0
 #define S_MAX 5
 #define S1 15
 #define S2 10
 #define S3 5
 #define RATE 0.05
-#define P_GAIN 0.8
+#define P_GAIN 1.2
 #define I_GAIN 0.0
-#define D_GAIN 0.02
+#define D_GAIN 0.015
+#define HOME_BACK 30
+#define HOME 1
+#define GOAL 30.0
+#define PING_LR 100
 //誤差errの範囲でvalue1がvalue2と等しければ0以外を、等しくなれば0を返す
 #define ERR_EQ(value1,value2,err) ( ((value1) <= ((value2)+(err)))&&((value1) >= ((value2)-(err))) )
 
@@ -45,29 +51,59 @@
 
 //uint16_t  analogHex[4] = {0};
 int speed[4] = {0};
-uint8_t value_ir = 0, ir_num = 0;
+uint8_t value_ir = 0, ir_num = 0, ir_dis = 0, ball_on = 0;
 uint8_t ping[4] = {0};
-uint8_t line[4] = {0}, line_stop[4] = {0};
-uint8_t back = 0;
+uint8_t line[4] = {0}, line_stop[4] = {0}, line_ping[4] = {0};
+uint8_t kick = 0;
+//uint8_t robot_state = 0;
 int compass = 0;
 int x = 0, y = 0, s = 0, i = 0, line_on = 0;
 int compassdef = 0, data = 0;
 uint8_t pingdef[4] = {0};
 
 double pidinput = 180.0;
-double compasspid = 0;
+double compasspid = 0.0;
+double goal = 0.0;
 
-double way[8][2] = {
-    { 0    , 1    },
-    {-0.707, 0.707},
-    {-1    , 0    },
-    {-0.707,-0.707},
-    { 0    ,-1    },
-    { 0.707,-0.707},
-    { 1    , 0    },
-    { 0.707, 0.707}
+double way[10][2] = {
+    { 0    , 1    },    //FF
+    {-0.707, 0.707},    //FL
+    {-1    , 0    },    //LL
+    {-0.707,-0.707},    //BL
+    { 0    ,-1    },    //BB
+    { 0.707,-0.707},    //BR
+    { 1    , 0    },    //RR
+    { 0.707, 0.707},    //FR
+    {-0.500, 0.866},    //FFL
+    { 0.500, 0.866}     //FFR
+};
+double ball_way[12][2] = {
+    { 0.0  , 1.0  },    //FF
+    {-0.985,-0.173},    //FL
+    {-0.342,-0.940},    //LL
+    { 0.342,-0.940},    //BL
+    { 0.966, 0.259},    //BB
+    {-0.342,-0.940},    //BR
+    { 0.342,-0.940},    //RR
+    { 0.985,-0.173},    //FR
+    {-0.985, 0.174},    //FFL
+    { 0.985, 0.174},    //FFR
+    {-0.173, 0.985},    //FFFL
+    { 0.173, 0.985}     //FFFR
 };
 
+double all_way[10][2] = {
+    { 0    , 1    },    //FF
+    {-0.707, 0.707},    //FL
+    {-0.939,-0.342},    //LL  {-1    , 0    },
+    {-0.707,-0.707},    //BL
+    { 0    ,-1    },    //BB
+    { 0.707,-0.707},    //BR
+    { 0.939,-0.342},    //RR { 1    , 0    },
+    { 0.707, 0.707},    //FR
+    {-0.500, 0.866},    //FFL
+    { 0.500, 0.866}     //FFR
+};
 
 void move(int vx, int vy, int vs){
     double pwm[4] = {0};
@@ -75,7 +111,7 @@
     pwm[0] = (double)((vx) + vs);
     pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0)  * vy) + vs);
     pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs);
-    pwm[3] = 0;
+    pwm[3] = kick;
 
     for(i = 0; i < 4; i++){
         if(pwm[i] > 100){
@@ -94,27 +130,59 @@
 }
 
 //ライン判断
-void line_state(){
-    if(line[0]){
-        y = -LINE_FP;
+void line_state(){    
+    if(line[LEFT]){
+        x = LINE_LP;
+    } else if(line_ping[LEFT]){
+        if(ping[LEFT] < LINE_PING){
+            if(x < 10){
+                x = 10;
+            }
+        } else {
+            line_ping[LEFT] = 0;
+            myled[1] = 0;
+        }
     }
-    if(line[1]){
-        x = LINE_LP;
-    }
-    if(line[2]){
+    
+    if(line[BACK]){
         y = LINE_FP;
+    } else if(line_ping[BACK]){
+        if(ping[BACK] < LINE_PING){
+            if(y < 10){
+                y = 10;
+            }
+        } else {
+            line_ping[BACK] = 0;
+            myled[2] = 0;
+        }
     }
-    if(line[3]){
+    
+    if(line[RIGHT]){
         x = -LINE_LP;
+    } else if(line_ping[RIGHT]){
+        if(ping[RIGHT] < LINE_PING){
+            if(x > 10){
+                x = -10;
+            }
+        } else {
+            line_ping[RIGHT] = 0;
+            myled[3] = 0;
+        }
     }
-}
-
-void lcds(int lcd1){
-    lcd.cls();
-    lcd.locate(0, 0);
-    lcd.printf("%d", lcd1);
-    //lcd.locate(2, 1);
-    //lcd.printf("YATTIYATTI");
+    
+    if(line[FRONT]){
+        y = -LINE_FP;
+    } else if(line_ping[FRONT]){
+        if(ping[FRONT] < LINE_PING){
+            if(y > 10){
+                y = -10;
+            }
+        } else {
+            line_ping[FRONT] = 0;
+            myled[0] = 0;
+        }
+    }
+    
 }
 
 void lcd_ping(){
@@ -129,79 +197,69 @@
     lcd.printf("%03d  %03d\n%03d  %03d", line[FRONT], line[LEFT], line[BACK], line[RIGHT]);
 }
 
-
-void home(){
-    if(ping[LEFT] > 80){
-        x = -20;
-    } else if(ping[RIGHT] > 80){
-        x = 20;
-    }
-    back = 30;
-/*    if(EER_EQ(ping[LEFT]+ping[RIGHT],pingdef[LEFT]+pingdef[RIGHT], 5)){
-        back = 10;
-    }*/
-    if(ping[BACK] > back){
-        y = -20;
-    }
-}
-
 void line_stop0(){
-    line_stop[0] = 0;
+    line_stop[FRONT] = 0;
 }
 void line_stop1(){
-    line_stop[1] = 0;
+    line_stop[LEFT] = 0;
 }
 void line_stop2(){
-    line_stop[2] = 0;
+    line_stop[BACK] = 0;
 }
 void line_stop3(){
-    line_stop[3] = 0;
+    line_stop[RIGHT] = 0;
 }
 
 void line_check()
 {
-    if(!line_stop[0]){
-        if(adcline[0].read_u16() > LINE_ON){
-            line[0] = 1;
-            line_stop[0] = 1;
+    if(!line_stop[FRONT]){
+        if(adcline[FRONT].read_u16() > LINE_ON){
+            line[FRONT] = 1;
+            line_stop[FRONT] = 1;
+            line_ping[FRONT] = 1;
             myled[0] = 1;
-            liner0.attach(&line_stop0,LINE_TIME);
+            liner0.attach(&line_stop0,1);
         } else {
-            line[0] = 0;
-            myled[0] = 0;
+            line[FRONT] = 0;
+            ////myled[0] = 0;
         }
     }
-    if(!line_stop[1]){
-        if(adcline[1].read_u16() > LINE_ON){
-            line[1] = 1;
-            line_stop[1] = 1;
+    if(!line_stop[LEFT]){
+        if(adcline[LEFT].read_u16() > LINE_ON){
+            line[LEFT] = 1;
+            line_stop[LEFT] = 1;
+            line_ping[LEFT] = 1;
             myled[1] = 1;
             liner1.attach(&line_stop1,LINE_TIME);
         } else {
-            line[1] = 0;
-            myled[1] = 0;
+            line[LEFT] = 0;
+            ////myled[1] = 0;
         }
     }
-    if(!line_stop[2]){
-        if(adcline[2].read_u16() > LINE_ON){
-            line[2] = 1;
-            line_stop[2] = 1;
+    if(!line_stop[BACK]){
+        if(adcline[BACK].read_u16() > LINE_ON){
+            line[BACK] = 1;
+            line_stop[BACK] = 1;
+            line_stop[FRONT] = 1;
+            line_ping[BACK] = 1;
             myled[2] = 1;
+            liner0.attach(&line_stop0,LINE_TIME);
             liner2.attach(&line_stop2,LINE_TIME);
         } else {
-            line[2] = 0;
-            myled[2] = 0;
+            line[BACK] = 0;
+            ////myled[2] = 0;
         }
     }
-    if(!line_stop[3]){
-        if(adcline[3].read_u16() > LINE_ON){
-            line[3] = 1;
-            line_stop[3] = 1;
+    if(!line_stop[RIGHT]){
+        if(adcline[RIGHT].read_u16() > LINE_ON){
+            line[RIGHT] = 1;
+            line_stop[RIGHT] = 1;
+            line_ping[RIGHT] = 1;
             myled[3] = 1;
             liner3.attach(&line_stop3,LINE_TIME);
         } else {
-            line[3] = 0;
-            myled[3] = 0;
+            line[RIGHT] = 0;
+            ////myled[3] = 0;
         }
     }
 }
@@ -209,16 +267,42 @@
 
 void pidupdate()
 {
+    pid.setSetPoint(180.0 + goal);
     pidinput = compass;
     pid.setProcessValue(pidinput);
     
     compasspid = -pid.compute();
 }
 
+void home()
+{
+    x = 0;
+    y = 0;
+    kick = 0;
+    goal = 0.0;
+    
+  //  if((145 < ping[LEFT]+ping[RIGHT]) && (ping[LEFT]+ping[RIGHT] < 155)){    
+        if(ping[LEFT] > PING_LR){
+            x = -HOME_P;
+            y = -5;
+        } else if(ping[RIGHT] > PING_LR){
+            x = HOME_P;
+            y = -5;
+        }
+  //  }
+
+    if(ping[BACK] > HOME_BACK){
+        y = -HOME_P;
+    } else if(ping[BACK] < 5){
+        y = HOME_P;
+    }
+}
+
 int main() {
     
     //dcompass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
-    uint8_t num = 0;
+    //uint8_t num = 0;
+    uint8_t dir = 0, power = 0;
     
     wait(1);
     
@@ -238,193 +322,85 @@
     sw.mode(PullUp);
     start.mode(PullUp);
     
-    myled[0] = 1;
-    while(start){}
-    myled[0] = 0;
+    lcd.cls();
+    lcd.locate(0,0);
+    lcd.printf("Chronograph");    
     
- /*   
-    while(1){
-        s = compasspid;
-        pc.printf("%d\n", s);
+        
+    myled[0] = 1;
+    while(!start){
+        lcd.locate(6,1);
+        lcd.printf("%d", compass);
         wait(0.1);
     }
-    */
-
+    myled[0] = 0;
+    lcd.cls();
+    lcd.locate(0,0);
+    lcd.printf("Chronograph");
+    lcd.locate(7,1);
+    lcd.printf("ABURASOBA");
+    
+    
     while(1){
+        
+        x = 0;
+        y = 0;
+        s = compasspid;        
+        power = MOTOR_P;
+        
+        if(/*(ir_dis < 60) && */( (ir_num == FF) || (ir_num == FFL) || (ir_num == FFR) )){
+            kick = 25;
+            //if(130 < ping[LEFT]+ping[RIGHT]){
+                //if(goal == 0){
+                   /* if(ping[LEFT] > 100){
+                        goal = -GOAL;
+                    }*//* else if(ping[RIGHT] > 100){
+                        //goal = GOAL;
+                    }*/
+                //}
+            //}
+                
+        } else {
+            kick = 0;
+            goal = 0.0;
+        }
+        
+        if((ir_dis > 50) && (ir_dis < 150) && (ir_num != BB) && (ir_num != FL) && (ir_num != FR) && (ir_num != BL) && (ir_num != BR)){
+            x = power * way[ir_num][0];
+            y = power * way[ir_num][1];
+        } else {            
+            if((ir_num == FF) || (ir_num == FFFL) || (ir_num == FFFR)){
+                power = MOTOR_P + 5;
+            } else if((ir_num == FFL) || (ir_num == FFR)){
+                power = MOTOR_P;
+            } else {
+                power = MOTOR_P + 5;
+            }
+            x = power * ball_way[ir_num][0];
+            y = power * ball_way[ir_num][1];
+            if(ir_num == BB){
+                if(ping[LEFT] > ping[RIGHT]){
+                    x = -x;
+                }
+            }
+        }
+       
         line_check();
         
-        //x = MOTOR_P;
-        //y = 0;
-        if(ir_num > 7){
-            ir_num = 0;
-        }
-        x = MOTOR_P * way[ir_num][0];
-        y = MOTOR_P * way[ir_num][1];
-        s = compasspid;
-        
-        if(value_ir > NO_IR){
+        if((ir_num == NO_BALL)/* || (ball_on < 40)*/){
             home();
         }
+
+        line_state();
         
-        line_state();
+        if((x == 0) && (y == 0)){
+            if(ping[BACK] > 25){
+                y = -HOME_P;
+            }
+        }
+        
+        //x = 0; y = 0;
         
         move(x,y,s);
     }
-    while(1){
-        line_check();
-        //pc.printf("%d\n", adcline[1].read_u16());
-        //pc.printf("%d %d %d %d\n", line[0], line[1], line[2], line[3]);
-        if(ir_num > 7){
-            ir_num = 0;
-        }
-        x = MOTOR_P;//MOTOR_P * way[ir_num][0];
-        y = 0;//MOTOR_P * way[ir_num][1];
-        s = (compass - 180) / 3;
-        if(s > S1){
-            s = S1;
-        } else if(s < -S1){
-            s = -S1;
-        } else if(s > S2){
-            s = S2;
-        } else if(s < -S2){
-            s = -S2;
-        } else if(s > 0){
-            s = S3;
-        } else if(s < 0){
-            s = -S3;
-        }
-                
-        line_state();
-        
-        move(x,y,s);        
-    }
-    
-/*    
-    while(1){
-        x = MOTOR_P * way[num][0];
-        y = MOTOR_P * way[num][1];
-        s = (compass - 180) / 3;
-        if(s > S1){
-            s = S1;
-        } else if(s < -S1){
-            s = -S1;
-        } else if(s > S2){
-            s = S2;
-        } else if(s < -S2){
-            s = -S2;
-        } else if(s > S3){
-            s = S3;
-        } else if(s < -S3){
-            s = -S3;
-        }
-        if(!sw){
-            num++;
-            wait(0.2);
-            if(num > 7){
-                num = 0;
-            }
-            lcds(num);
-        }
-        //pc.printf("%d\n", s);
-        
-        move(x,y,s);
-        //wait(0.1);
-    }
-*/
-/*
-    while(1){
-        x = 30;
-        y = 0;
-        move(x,y,s);
-        wait(0.5);
-        x = -30;
-        y = 0;
-        move(x,y,s);
-        wait(0.5);
-        //pc.printf("%d\n", s);
-        //pc.printf("%d\n", adcline[3].read_u16());
-    }
-*/
-
-/*    while(1){
-        if(ir_num > 7){
-            ir_num = 0;
-        }
-        x = MOTOR_P * way[ir_num][0];
-        y = MOTOR_P * way[ir_num][1];
-        s = (compass - 180) / 3;
-        if(s > S_MAX){
-            s = S_MAX;
-        } else if(s < -S_MAX){
-            s = -S_MAX;
-        }
-        
-        move(x,y,s);
-    }
-    */    
-/*    
-    while(1){
-        i = 3;
-        analogHex[i] = adcIn[i].read_u16();
-        if(analogHex[i] > 10000){
-            myled[i] = 1;
-            line[i] = 1;
-            line_on = 1;
-        } else {
-            myled[i] = 0;
-            line[i] = 0;
-            line_on = 0;
-        }
-        
-        x = 30;
-        y = 0;
-                
-        if(line[1]){
-            x = LINE_P;
-        } else if(line[3]){
-            x = -LINE_P;
-        }
-        
-        if(line[0]){
-            y = -LINE_P;
-        } else if(line[2]){
-            y = LINE_P;
-        }
-        move(x,y,s);
-        
-        if(line_on)wait(0.3);
-    }
-    */
-    /*
-    while(1){
-        for(i = 0; i < 4; i++){
-            analogHex[i] = adcIn[i].read_u16();
-            if(analogHex[i] > 10000){
-                myled[i] = 1;
-                line[i] = 1;
-            } else {
-                myled[i] = 0;
-                line[i] = 0;
-            }
-            //wait(0.1);
-        }
-        
-        if(line[0]){
-            x = -30;
-        } else if(line[2]){
-            x = 30;
-        }
-        
-        if(line[1]){
-            y = 30;
-        } else if(line[3]){
-            y = -30;
-        }
-        
-        move(x,y);
-        x = 0;
-        y = 0;
-        //pc.printf("%05d   %05d   %05d   %05d\n", analogHex[0], analogHex[1], analogHex[2],  analogHex[3]);
-    }
-    */
 }

--- a/usart.cpp	Mon Mar 03 00:24:44 2014 +0000
+++ b/usart.cpp	Wed Mar 12 04:51:30 2014 +0000
@@ -4,15 +4,15 @@
 #define KEYCODE 0xAA
 #define TX_CHECKCODE (tx_data[1]^tx_data[2]^tx_data[3]^tx_data[4]^tx_data[5]^tx_data[6]^tx_data[7]^tx_data[8])
 #define RX_CHECKCODE (rx_data[1]^rx_data[2]^rx_data[3]^rx_data[4]^rx_data[5]^rx_data[6]^rx_data[7]^rx_data[8])
-#define DATA_NUM 10
+#define DATA_NUM 12
 #define CHECK (DATA_NUM - 1)
-#define ALL_IR 10
+#define ALL_IR 12
 
 
 extern Serial sensor;
 extern Serial pc;
 
-extern uint8_t value_ir, ir_num;
+extern uint8_t value_ir, ir_num, ir_dis, ball_on;
 extern uint8_t ping[4];
 extern uint8_t line[4];
 extern uint8_t ir_max, ir_num;
@@ -38,6 +38,8 @@
             ping[BACK] = rx_data[5];
             ping[RIGHT] = rx_data[6];
             compass = rx_data[7] + rx_data[8];
+            ir_dis = rx_data[9];
+            ball_on = rx_data[10];
 /*            line[FRONT] = rx_data[7];
             line[LEFT] = rx_data[8];
             line[BACK] = rx_data[9];
@@ -47,6 +49,10 @@
             //pc.printf("%d %d %d %d\n", rx_data[3], rx_data[4], rx_data[5], rx_data[6]);
             //pc.printf("%d %d %d %d\n", rx_data[7], rx_data[8], rx_data[9], rx_data[10]);
             //pc.printf("%d %d %d\n", rx_data[7], rx_data[8], compass);
+            //pc.printf("%d\n", rx_data[9]);
+            //pc.printf("%d\t%d\t%d\t%d\n", ping[FRONT], ping[LEFT], ping[BACK], ping[RIGHT]);
+            //pc.printf("%d %d  %d\n", ping[LEFT], ping[RIGHT], ping[LEFT] + ping[RIGHT]);
+            //pc.printf("%d\t%d\t%d\t%d\n", ir_dis, ball_on, value_ir, ir_num);
         }
      rx = 0;   
     }