Revision 1:fb4277ce4d93, committed 2014-02-28

Comitter:

com3

Date:

Fri Feb 28 10:50:11 2014 +0000

Parent:

0:e6d14fec4954

Child:

2:59edff92b599

Commit message:

main;

Changed in this revision

--- a/main.cpp	Tue Feb 25 03:06:52 2014 +0000
+++ b/main.cpp	Fri Feb 28 10:50:11 2014 +0000
@@ -1,19 +1,34 @@
 #include "mbed.h"
-#include "HMC6352.h"
 #include "TextLCD.h"
 #include "common.h"
 #include <math.h>
 #include <sstream>
 
-#define LINE_P 70
+#define MOTOR_P 30
+#define LINE_LP 30
+#define LINE_FP 40
+#define LINE_ON 0xFFF0
+#define LINE_TIME 0.5
 #define R 1.0
+#define S_MAX 5
+#define S1 15
+#define S2 10
+#define S3 5
+//誤差errの範囲でvalue1がvalue2と等しければ0以外を、等しくなれば0を返す
+#define ERR_EQ(value1,value2,err) ( ((value1) <= ((value2)+(err)))&&((value1) >= ((value2)-(err))) )
 
+DigitalIn sw(p5);
+DigitalIn start(p29);
 DigitalOut myled[4] = {LED1, LED2, LED3, LED4};
 Serial motor(p9,p10);
 Serial sensor(p13,p14);
 Serial pc(USBTX, USBRX);
 TextLCD lcd(p26, p25, p24, p23, p22, p21);
-//AnalogIn adcIn[4] = {p17, p18, p19, p20};
+AnalogIn adcline[4] = {p16, p17, p19, p20};
+Timeout liner0;
+Timeout liner1;
+Timeout liner2;
+Timeout liner3;
 //HMC6352 dcompass(p9,p10);
 
 extern string StringFIN;
@@ -22,12 +37,25 @@
 
 //uint16_t  analogHex[4] = {0};
 int speed[4] = {0};
+uint8_t value_ir = 0, ir_num = 0;
 uint8_t ping[4] = {0};
-uint8_t line[4] = {0};
-uint8_t ir_max = 0, ir_num = 0;
+uint8_t line[4] = {0}, line_stop[4] = {0};
+uint8_t back = 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 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}
+};
 
 
 void move(int vx, int vy, int vs){
@@ -57,22 +85,72 @@
 //ライン判断
 void line_state(){
     if(line[0]){
-        y = -LINE_P;
+        y = -LINE_FP;
     }
     if(line[1]){
-        x = LINE_P;
+        x = LINE_LP;
     }
     if(line[2]){
-        y = LINE_P;
+        y = LINE_FP;
     }
     if(line[3]){
-        x = -LINE_P;
+        x = -LINE_LP;
     }
 }
 
+void lcds(int lcd1){
+    lcd.cls();
+    lcd.locate(0, 0);
+    lcd.printf("%d", lcd1);
+    //lcd.locate(2, 1);
+    //lcd.printf("YATTIYATTI");
+}
+
+void lcd_ping(){
+    lcd.cls();
+    lcd.locate(0,0);
+    lcd.printf("%03d  %03d\n%03d  %03d", ping[FRONT], ping[LEFT], ping[BACK], ping[RIGHT]);
+}
+
+void lcd_line(){
+    lcd.cls();
+    lcd.locate(0,0);
+    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;
+}
+void line_stop1(){
+    line_stop[1] = 0;
+}
+void line_stop2(){
+    line_stop[2] = 0;
+}
+void line_stop3(){
+    line_stop[3] = 0;
+}        
+
 int main() {
     
     //dcompass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    uint8_t num = 0;
     
     wait(1);
     
@@ -83,15 +161,141 @@
     //compassdef = (compass / 10);           //コンパス初期値保存
     //compassdef = (dcompass.sample() / 10);
     
-
-    wait(1);
-
-    //while(1);
+    sw.mode(PullUp);
+    start.mode(PullUp);
     
-    y = 20;
+    myled[0] = 1;
+    while(start){}
+    myled[0] = 0;
+/*
+    while(1){
+        //x = 30;
+        //y = 35;
+        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;
+        }
+        move(x,y,s);
+        //pc.printf("%d\n", s);
+        //pc.printf("%d\n", adcline[3].read_u16());
+    }
+*/
+/*    while(1){
+        //lcd_ping();
+        lcd_line();
+        wait(0.2);
+    }
+*/
+/*    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){
+        if(!line_stop[0]){
+            if(adcline[0].read_u16() > LINE_ON){
+                line[0] = 1;
+                line_stop[0] = 1;
+                myled[0] = 1;
+                liner0.attach(&line_stop0,LINE_TIME);
+            } else {
+                line[0] = 0;
+                myled[0] = 0;
+            }
+        }
+        if(!line_stop[1]){
+            if(adcline[1].read_u16() > LINE_ON){
+                line[1] = 1;
+                line_stop[1] = 1;
+                myled[1] = 1;
+                liner1.attach(&line_stop1,LINE_TIME);
+            } else {
+                line[1] = 0;
+                myled[1] = 0;
+            }
+        }
+        if(!line_stop[2]){
+            if(adcline[2].read_u16() > LINE_ON){
+                line[2] = 1;
+                line_stop[2] = 1;
+                myled[2] = 1;
+                liner2.attach(&line_stop2,LINE_TIME);
+            } else {
+                line[2] = 0;
+                myled[2] = 0;
+            }
+        }
+        if(!line_stop[3]){
+            if(adcline[3].read_u16() > LINE_ON){
+                line[3] = 1;
+                line_stop[3] = 1;
+                myled[3] = 1;
+                liner3.attach(&line_stop3,LINE_TIME);
+            } else {
+                line[3] = 0;
+                myled[3] = 0;
+            }
+        }
+        //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 = 30;//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 > S3){
+            s = S3;
+        } else if(s < -S3){
+            s = -S3;
+        }
+        line_state();
+        move(x,y,s);        
+    }    
+    //y = 20;
     
     while(1){
-        s = (compass - 180) / 5;
+        x = 20 * way[num][0];
+        y = 20 * way[num][1];
+        s = (compass - 180) / 3;
+        
+        if(!sw){
+            num++;
+            wait(0.2);
+            if(num > 7){
+                num = 0;
+            }
+            lcds(num);
+        }
         //pc.printf("%d\n", s);
         
         move(x,y,s);

--- a/usart.cpp	Tue Feb 25 03:06:52 2014 +0000
+++ b/usart.cpp	Fri Feb 28 10:50:11 2014 +0000
@@ -2,9 +2,9 @@
 #include "common.h"
 
 #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]^tx_data[9]^tx_data[10])
-#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]^rx_data[9]^rx_data[10])
-#define DATA_NUM 14
+#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 CHECK (DATA_NUM - 1)
 #define ALL_IR 10
 
@@ -12,6 +12,7 @@
 extern Serial sensor;
 extern Serial pc;
 
+extern uint8_t value_ir, ir_num;
 extern uint8_t ping[4];
 extern uint8_t line[4];
 extern uint8_t ir_max, ir_num;
@@ -30,23 +31,29 @@
     
     if(rx >= DATA_NUM){
         if(rx_data[CHECK] == RX_CHECKCODE){
-             compass = rx_data[11] + rx_data[12];
-             ping[FRONT] = rx_data[2];
-             ping[LEFT] = rx_data[3];
-             ping[BACK] = rx_data[4];
-             ping[RIGHT] = rx_data[5];
-             line[FRONT] = rx_data[6];
-             line[LEFT] = rx_data[7];
-             line[BACK] = rx_data[8];
-             line[RIGHT] = rx_data[9];
-             //pc.printf("%d %d %d %d %d\n", rx_data[1], rx_data[2], rx_data[3], rx_data[4], rx_data[5]);
-             pc.printf("%d %d %d\n", rx_data[11], rx_data[12], compass);
-        }  
+            value_ir = rx_data[1];
+            ir_num = rx_data[2];
+            ping[FRONT] = rx_data[3];
+            ping[LEFT] = rx_data[4];
+            ping[BACK] = rx_data[5];
+            ping[RIGHT] = rx_data[6];
+            compass = rx_data[7] + rx_data[8];
+/*            line[FRONT] = rx_data[7];
+            line[LEFT] = rx_data[8];
+            line[BACK] = rx_data[9];
+            line[RIGHT] = rx_data[10];
+*/          
+            //pc.printf("%d %d\n", rx_data[1], rx_data[2]); 
+            //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);
+        }
      rx = 0;   
     }
     //pc.printf("%d\n", rx_data[rx]);
 }
 
+/*
 void micon_tx(){
     
     static uint8_t tx;
@@ -71,4 +78,5 @@
     
     sensor.putc(tx_data[tx]);
     tx++;
-}
\ No newline at end of file
+}
+*/
\ No newline at end of file