Revision 0:4c9a3b107572, committed 2013-09-12

Comitter:

OGA

Date:

Thu Sep 12 12:10:38 2013 +0000

Commit message:

sensei

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ColorSensor.lib	Thu Sep 12 12:10:38 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/OGA/code/ColorSensor/#c5bc72734cbf

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC6352.lib	Thu Sep 12 12:10:38 2013 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/aberk/code/HMC6352/#83c0cb554099

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Thu Sep 12 12:10:38 2013 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/simon/code/Servo/#36b69a7ced07

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TextLCD.lib	Thu Sep 12 12:10:38 2013 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/simon/code/TextLCD/#44f34c09bd37

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Sep 12 12:10:38 2013 +0000
@@ -0,0 +1,245 @@
+#include "mbed.h"
+#include "ColorSensor.h"
+#include "TextLCD.h"
+#include "Servo.h"
+#include "HMC6352.h"
+
+#include "main.h"
+
+//センサの数
+#define COLOR_NUM 3
+
+//閾値
+#define R_THR 65
+#define G_THR 65
+#define B_THR 65
+#define PINR_THR 1000
+
+enum{
+    GO,
+    STOP
+};
+
+//TextLCD lcd(p30, p29, p28, p27, p26, p25, TextLCD::LCD20x4); // rs, e, d4-d7
+
+ColorSensor color0(p20, p17, p18, p19, 5);
+ColorSensor color1(p16, p13, p14, p15, 5);
+ColorSensor color2(p12, p9, p10, p11, 5);
+
+Servo servoR(p23);
+Servo servoL(p24);
+
+//ColorSensor color = ColorSensor();
+//Serial pc(USBTX, USBRX); // tx, rx
+
+DigitalOut led[4] = {LED1,LED2,LED3,LED4};
+DigitalOut air[2] = {p21,p22};
+
+Timer color_t[3];
+Timer ping_t;
+Ticker interrupt0;
+
+double proportional = 0;
+uint16_t com_val = 0;
+    unsigned redp[COLOR_NUM], greenp[COLOR_NUM], bluep[COLOR_NUM];
+
+#define Convert_dekaruto(a) ((a+100.0)/2.0/100.0)
+
+#define STRAIGHT 0.6;
+#define SPIN 0.4;
+
+//#define STRAIGHT 0.0;
+//#define SPIN 1.0;
+
+
+
+void tic_sensor()
+{
+    Ultrasonic();
+    
+    color0.checkRGB(redp[0],greenp[0],bluep[0]);
+    color1.checkRGB(redp[1],greenp[1],bluep[1]);
+    color2.checkRGB(redp[2],greenp[2],bluep[2]);
+    
+    //pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
+}
+
+
+void move(int vl,int vs){
+    double fut_R,fut_L,true_vs;
+    
+    true_vs = abs(vs)/SPIN;
+    
+    if(true_vs > 40){
+        vl = 0;
+        vs = 100*(vs/abs(vs));
+    }
+    
+    fut_R = Convert_dekaruto((vl + vs));
+    fut_L = Convert_dekaruto((vl - vs)/**1.4*/);
+    
+    fut_R = Convert_dekaruto(-50);
+    fut_L = Convert_dekaruto(-50);
+ 
+    servoR = fut_R;
+    servoL = fut_L;
+       
+    //printf("R:%lf   L:%lf\n",fut_R,fut_L);
+    //printf("R:%d   L:%d\n",(vl + vs),-(vl - vs));
+}
+
+/*void PidUpdate()
+{   
+    inputPID = (((int)(compass.sample() - ((207.0) * 10.0) + 5400.0) % 3600) / 10.0);
+    //printf("%lf\n",inputPID);      
+}*/
+
+double vsOut(){
+    double vs;
+    vs = ((inputPID / 360 - 0.5) * 2 * 100) * -1;
+    vs = vs * 8;
+    
+    if(vs/abs(vs) < 0){
+        //vs *= 1.3;   
+    }
+    
+    if(abs(vs) > 90)vs = 90*(vs/abs(vs));
+    if(abs(vs) < 25) vs = 25*(vs/abs(vs));
+    
+    return vs;
+}
+
+
+int ping_button(int ping,int button){
+    static int continue_flag = 0;
+    static int change_flag = 0;
+
+    if(continue_flag == 0){
+        if(ping <= PINR_THR){
+            ping_t.start();
+            continue_flag = 1;
+        }
+    }    
+    
+    if(continue_flag == 1){
+        //agatterutoki
+        if(ping <= PINR_THR){
+            if(change_flag == 0){
+                if(ping_t.read_ms() >= 300){
+                    button = !button;
+                    change_flag = 1;
+                }
+            }
+        }
+        //tatisagari
+        if(ping >= (PINR_THR+200)){
+            ping_t.stop();
+            ping_t.reset();
+            continue_flag = 0;
+            change_flag = 0;
+        }       
+    }
+    return button;    
+}
+
+
+//青から赤に反応したらジャンプ
+//pidUpdataのせいでカラーセンサが動かない!!
+int main()
+{
+    //wait(3);
+    
+    timer2.start();
+    ping_t.start();
+    
+        
+    unsigned R, G, B;
+    unsigned threshold, t[3] = {0} ;
+    double color_sum;
+    int vl;
+    double vs;
+    uint8_t button, state=0;
+    
+    //pc.baud(115200);
+    air[0] = 0; air[1] = 1;
+  
+    
+    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    //pidUpdata.attach(&PidUpdate, PID_CYCLE);
+    interrupt0.attach(&tic_sensor, 0.05/*sec*/);//0.04sec以上じゃないとmain動かない
+  
+  int e=0;
+  while(1)
+  {
+    
+    
+    //lcd.cls();
+    //pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
+    /*
+    lcd.locate(0,0);
+    lcd.printf("R:%d G:%d B:%d", redp[0][0], greenp[0][0], bluep[0][0]);
+    lcd.locate(0,1);
+    lcd.printf("R:%d G:%d B:%d", redp[1][0], greenp[1][0], bluep[1][0]);
+     */
+    pc.printf("%d\n", ultrasonicVal[0]);
+    
+    
+    threshold = 0;
+    for(int i=0; i<3; i++){
+        if(bluep[i] >= B_THR){
+            color_t[i].reset();
+            color_t[i].start();
+            t[i] = 0;
+        }else if(redp[i] >= R_THR){
+            t[i] = color_t[i].read_ms();
+        }else{
+            t[i] = 0;
+        }
+ 
+        if((t[i] <= 500) && (t[i] != 0)){
+            threshold++;
+        }
+    }
+    
+    
+    
+    if(threshold >= 1){
+            led[0] = 1; led[1] = 1; led[2] = 1; led[3] = 1;
+            air[0] = 1;  air[1] = 0;
+            wait(0.4);
+            led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
+            air[0] = 0;  air[1] = 1;
+            wait(0.5);
+    }else{
+            led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
+    }
+    
+    
+    
+    button = ping_button(ultrasonicVal[0],button);
+        
+    if(button){
+        state = GO;
+    }else{
+        state = STOP;
+    }
+    
+    
+    
+    
+    
+    if(state == GO){
+        vl = -90;//led[0] = 1; led[1] = 1;
+    }else if(state == STOP){
+        vl = 0;//led[0] = 0; led[1] = 0;
+    }
+    
+    vl = vl      * STRAIGHT ;
+    vs = vsOut() * SPIN     ;
+    
+    vl *= 0.5;
+    vs *= 0.3;
+        
+    move(vl,(int)vs); 
+  }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h	Thu Sep 12 12:10:38 2013 +0000
@@ -0,0 +1,17 @@
+#include "mbed.h"
+
+Timer timer2;
+
+extern double ultrasonicValue[4];
+extern uint16_t ultrasonicVal[4];
+extern void Ultrasonic(void);
+
+#define PID_CYCLE   0.06    //s
+//#define PID_CYCLE   0.1    //s
+
+HMC6352 compass(p28, p27);
+Serial pc(USBTX, USBRX);    // tx, rx 
+Ticker pidUpdata;
+Timer timer1;
+
+double inputPID = 180.0;

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Sep 12 12:10:38 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/9c8f0e3462fb
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ping/ping.cpp	Thu Sep 12 12:10:38 2013 +0000
@@ -0,0 +1,49 @@
+#include "mbed.h"
+#include "ping.h"
+ 
+//DigitalOut myled = LED1; 
+ 
+extern Timer timer2;
+
+uint16_t ultrasonicVal[ALL_ULTRASONIC];
+double ultrasonicValue[ALL_ULTRASONIC] = {0};
+
+
+void Ultrasonic()
+{
+    for(int i = 0 ; i < ALL_ULTRASONIC; i++){
+       
+        uint8_t flag = 0;
+    
+        DigitalOut PingPinOut(ultrasonic_pin[i]);
+        PingPinOut = 1;
+        wait_us(10);
+        PingPinOut = 0;
+        DigitalIn PingPin(ultrasonic_pin[i]);
+        timer2.reset();
+        while(PingPin == 0){
+            if(timer2.read_us() > 1500){   //1.5ms以上応答なし
+                ultrasonicValue[i] =  PING_ERR;
+                flag = 1;
+                break;
+            }
+        } 
+  
+        timer2.reset();
+        while(PingPin == 1){
+            if((timer2.read_us() > 18500) || (flag == 1)){  //18.5ms以上のパルス
+                ultrasonicValue[i] =  PING_ERR;
+                flag = 1;
+                break;
+            }
+        }
+      
+        if(flag == 0){
+            ultrasonicValue[i] = timer2.read_us() / 1000000.0 / 2.0 * 340.0 * 1000.0; //mm  MAX:3145
+            ultrasonicVal[i] = (int)(ultrasonicValue[i] * 10.0);
+        }else{
+            ultrasonicVal[i] = PING_ERR;
+        }
+
+    }
+} 
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ping/ping.h	Thu Sep 12 12:10:38 2013 +0000
@@ -0,0 +1,7 @@
+#define ALL_ULTRASONIC  1
+#define PING_ERR        0xFFFF
+
+
+PinName ultrasonic_pin[ALL_ULTRASONIC] = {
+    p5
+};