Revision 0:e6bb0bcba274, committed 2013-10-01

Comitter:

OGA

Date:

Tue Oct 01 07:44:16 2013 +0000

Commit message:

oooooga

Changed in this revision

diff -r 000000000000 -r e6bb0bcba274 ColorSensor.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ColorSensor.lib	Tue Oct 01 07:44:16 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/OGA/code/ColorSensor1/#745c9de82674

diff -r 000000000000 -r e6bb0bcba274 HMC6352.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC6352.lib	Tue Oct 01 07:44:16 2013 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/aberk/code/HMC6352/#83c0cb554099

diff -r 000000000000 -r e6bb0bcba274 PID.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Tue Oct 01 07:44:16 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/PID/#6e12a3e5af19

diff -r 000000000000 -r e6bb0bcba274 QEI.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Tue Oct 01 07:44:16 2013 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/aberk/code/QEI/#5c2ad81551aa

diff -r 000000000000 -r e6bb0bcba274 Servo.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Tue Oct 01 07:44:16 2013 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/simon/code/Servo/#36b69a7ced07

diff -r 000000000000 -r e6bb0bcba274 TextLCD.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TextLCD.lib	Tue Oct 01 07:44:16 2013 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/simon/code/TextLCD/#44f34c09bd37

diff -r 000000000000 -r e6bb0bcba274 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 01 07:44:16 2013 +0000
@@ -0,0 +1,422 @@
+#include "mbed.h"
+#include "PID.h"
+#include "QEI.h"
+#include "Servo.h"
+#include "HMC6352.h"
+#include "ColorSensor.h"
+#include "TextLCD.h"
+
+#include "main.h"
+
+
+
+
+
+void tic_sensor()
+{
+    Ultrasonic();
+    
+    colorUpdate();
+    
+    /*lcd.cls();
+    lcd.locate(0,0);
+    lcd.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
+    */
+}
+
+
+
+
+
+////////////////////////////////////////移動関数//////////////////////////////////////////////
+/////////////////////////////////////////////////////////////////////////////////////////////
+void PidUpdate(){
+    static int state = WAIT,past_state = WAIT,next_state = WAIT;
+
+    if(!mode_comp){
+        if(vl && !vs){
+            state = STRAIGHT;
+        }
+        if(vs){
+            state = TURN;
+        }
+    }
+    if((state != past_state)){
+        mode_comp = 1;
+    
+        if(process == 0){
+            if(abs(enkoda1) > 180 && abs(enkoda1) < 240){
+                process++;
+            }    
+        }
+        if(process == 1){
+            if(state == STRAIGHT){
+                vl = 10;
+                vs = 0;
+            }
+            if(state == TURN){
+                vl = 0;
+                vs = 10;
+            }        
+            if(abs(vc) < 60){
+                vc_count++;
+            }
+            if(vc_count > 2){
+                vc_count = 0;
+                mode_comp = 0;
+                process = 0;
+            }
+        }
+    }
+
+    enkoda1 = (int)(((double)wheel1.getPulses()/CYCLE) * 360.0);
+    enkoda2 = (int)(((double)wheel2.getPulses()/CYCLE) * 360.0);
+
+    if((!vs)&&(mode_comp == 0)){
+        pid.setProcessValue(wheel1.getPulses() + wheel2.getPulses());
+    }else if((vs)&&(mode_comp == 0)){
+        pid.setProcessValue(wheel1.getPulses() - wheel2.getPulses());
+    }
+    
+    if((!vs)&&(mode_comp)){
+        pid.setProcessValue(wheel1.getPulses() + wheel2.getPulses());
+    }else if((vs)&&(mode_comp)){
+        pid.setProcessValue(wheel1.getPulses() - wheel2.getPulses());
+    }
+        
+    vc = (int)pid.compute();
+           
+    printf("vc:%lf\n",vc);
+    
+    double fut_R = 0.5,fut_L = 0.5;
+    
+    if(abs(vc) > 250){
+        vc = 0;
+    }
+
+    int vlR = 0;
+    int vlL = 0;
+    int vcR = vc;
+    int vcL = vc;
+    
+    vlR = -vs;
+    vlL =  vs;
+    
+    vl *= 0.5;
+    vc *= 0.5;
+    
+    vlR *= 0.4;
+    vlL *= 0.4;
+    
+    vcR *= 0.6;
+    vcL *= 0.6;
+
+    
+    if(!vs){   
+        if(vl > 0){
+            fut_R = Convert_dekaruto((int)(vl - vc));
+            fut_L = Convert_dekaruto((int)(vl * 0.95 + vc)); 
+        }
+        if(vl < 0){
+            vc *= -1; 
+            fut_R = Convert_dekaruto((int)(vl * 0.95 + vc));
+            fut_L = Convert_dekaruto((int)(vl - vc)); 
+        }
+    }else{
+        if(vlR < 0){
+            vcR *= -1;  
+        }
+        
+        if(vlL < 0){
+            vcL *= -1;  
+        }
+        if(vs > 0){
+            fut_R = Convert_dekaruto((int)(vlR * 0.9 + vcR));
+            fut_L = Convert_dekaruto((int)(vlL * 0.65 - vcL));
+        }
+        
+        if(vs < 0){
+            fut_R = Convert_dekaruto((int)(vlR * 0.65 - vcR));
+            fut_L = Convert_dekaruto((int)(vlL * 0.9+ vcL));
+        }
+    }
+    
+    servoR = fut_R;
+    servoL = fut_L;
+    
+    if(!mode_comp){
+        past_state = state;
+    }    
+}
+
+void CompassUpdate(){
+    inputPID = (((int)(compass.sample() - (/**/180 * 10.0) + 5400.0) % 3600) / 10.0);
+}
+
+double vssOut(){
+    double vss;
+    vss = ((inputPID / 360.0 - 0.5) * 2.0 * 1000.0) * 1.0;
+    
+    if(abs(vss) < 10){
+        vss = 0;
+    }
+    
+    vss *= 3.0;
+    
+    
+    if((vss)&&(abs(vss) < 500)){
+        vss += (vss/abs(vss)) * 500;
+    }
+        
+    if(abs(vss) > 1000){
+        vss = 1000 * (vss/abs(vss));
+    }
+    
+    return vss;
+}
+
+void move(int vll,int vss){
+    if(!swR){
+        wheel1.reset();
+    }
+    
+    if(!swL){
+        wheel2.reset();
+    }
+
+    vl = vll;
+    vs = vss;
+}
+
+
+////////////////////////////////////////超音波センサの////////////////////////////////////////
+////////////////////////////////////////スイッチ的な関数//////////////////////////////////////
+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() >= 150){
+                    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;    
+}
+
+
+////////////////////////////////////////カラーセンサの////////////////////////////////////////
+////////////////////////////////////////補正プログラム////////////////////////////////////////
+void rivisedate()
+{
+    unsigned long red = 0,green = 0,blue =0;
+    static unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM];
+    
+    //最初の20回だけ平均を取る
+    for (int i=0;i<=20;i++){
+         color0.getRGB(R[0],G[0],B[0]);
+         red       += R[0] ;
+         green     += G[0] ;
+         blue      += B[0] ;
+         //pc.printf(" %d  %d\n",ptm(sum),sum);
+    }
+    
+    rir = (double)green/ red ;
+    rib = (double)green/ blue ;
+}
+
+void colorUpdate()
+{
+    double colorSum[COLOR_NUM];
+    unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM];
+
+    color0.getRGB(R[0],G[0],B[0]);
+    color1.getRGB(R[1],G[1],B[1]);
+    color2.getRGB(R[2],G[2],B[2]);
+    /*color3.getRGB(R[3],G[3],B[3]);
+    color4.getRGB(R[4],G[4],B[4]);
+    color5.getRGB(R[5],G[5],B[5]);*/
+    
+    for (int i=0; i<COLOR_NUM; i++){
+        colorSum[i] = R[i]*rir + G[i] + B[i]*rib ;
+        redp[i]   = R[i]* rir * 100 / colorSum[i];
+        greenp[i] = G[i]      * 100 / colorSum[i];
+        bluep[i]  = B[i]* rib * 100 / colorSum[i];
+    }
+}
+
+
+////////////////////////////////////////ジャンププログラム////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////
+uint8_t jumpcondition()
+{
+    uint8_t threshold = 0, t[3] = {0};
+    
+    //青から赤に0.5秒以内に反応したらジャンプ
+    for(int i=0; i<COLOR_NUM; 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++;
+        }
+    }
+    
+    return threshold;
+}
+
+void jumping(uint8_t threshold)
+{
+    //超音波でジャンプのタイミング合わせる
+    if(threshold >= 1){
+            jump_t.reset();
+            jump_t.start();
+            while(ultrasonicVal[0] < 1700){
+                led[0] = 1; led[1] = 1; led[2] = 0; led[3] = 0;
+                air[0] = 1;  air[1] = 0;
+                
+                if(jump_t.read_ms() > 1000)break;
+            }
+            led[0] = 0; led[1] = 0; led[2] = 1; led[3] = 1;
+            air[0] = 0;  air[1] = 1;
+            wait(0.5);
+    }else{
+            led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
+    }
+}
+
+
+uint8_t robostop()
+{
+    if(bluep[1] >= B_THR){
+        return 1;
+    }else{
+        return 0;
+    }
+}
+
+
+
+
+
+
+
+
+
+
+
+int main() {
+    rivisedate();
+    wait(3);
+    
+    timer2.start();
+    ping_t.start();
+
+    pid.setInputLimits(MINIMUM,MAXIMUM);
+    pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);
+    pid.setBias(PID_BIAS);
+    pid.setMode(AUTO_MODE);
+    pid.setSetPoint(REFERENCE);
+    
+    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    compassUpdata.attach(&CompassUpdate, COMPASS_CYCLE);
+    
+    swR.mode(PullUp);
+    swL.mode(PullUp);
+        
+    air[0] = 0; air[1] = 1;
+ 
+    int setR = 0,setL = 0;
+    int vll = 0,vss = 0;
+    
+    servoR = 0.595;
+    servoL = 0.59;
+    
+    while(1){
+        if(!swR){
+            setR = 1;
+        }
+        
+        if(!swL){
+            setL = 1;
+        }
+        
+        if(setR){
+            servoR = 0.5;    
+        }
+        
+        if(setL){
+            servoL = 0.5;    
+        }
+        
+        if(setR && setL){
+            break;
+        }
+       
+        wait(0.1);
+    }
+    
+    wheel1.reset();
+    wheel2.reset();
+    
+    interrupt0.attach(&tic_sensor, 0.05/*sec*/);//0.04sec以上じゃないとmain動かない
+    pidUpdata.attach(&PidUpdate, PID_CYCLE);
+    
+    uint8_t button, state=0;
+
+    while(1) {
+        vll = 0;
+        vss = 0;
+    
+        pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
+        jumping(jumpcondition());
+    
+    
+    
+        button = ping_button(ultrasonicVal[1],button);
+            
+        if(button){
+            state = GO;
+        }else{
+            state = STOP;
+        }
+        
+        
+        
+        
+        
+        if(state == GO){
+            vll = 700;//led[0] = 1; led[1] = 1;
+        }else if(state == STOP){
+            vll = -700;//led[0] = 0; led[1] = 0;
+        }
+           
+        move(vll,vss); 
+    }
+}

diff -r 000000000000 -r e6bb0bcba274 main.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h	Tue Oct 01 07:44:16 2013 +0000
@@ -0,0 +1,115 @@
+///////////////////////////////////////OGATA//////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////
+//センサの数
+#define COLOR_NUM 3
+
+//閾値
+#define R_THR 65
+#define G_THR 65
+#define B_THR 65
+#define PINR_THR 2000
+
+
+enum{
+    GO,
+    STOP
+};
+
+//TextLCD lcd(p30, p29, p28, p27, p26, p25, TextLCD::LCD20x4); // rs, e, d4-d7
+ColorSensor color0(p20, p17, p18, p19, 10);
+ColorSensor color1(p16, p13, p14, p15, 10);
+ColorSensor color2(p12, p9, p10, p11, 10);
+Serial pc(USBTX, USBRX);    // tx, rx 
+DigitalOut led[4] = {LED1,LED2,LED3,LED4};
+DigitalOut air[2] = {p7,p8};
+
+
+Timer color_t[3];
+Timer jump_t;
+Ticker interrupt0;
+
+
+void rivisedate ();
+void colorUpdate ();
+uint8_t ptm(unsigned sum);
+
+
+double proportional = 0;
+uint16_t com_val = 0;
+unsigned redp[COLOR_NUM], greenp[COLOR_NUM], bluep[COLOR_NUM];
+double rir,rib ;
+
+
+
+
+
+
+
+///////////////////////////////////////YUSUKE/////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////
+#define RATE 0.1
+#define MINIMUM -1440
+#define MAXIMUM 1440 
+
+#define PID_CYCLE   0.03    //s
+#define COMPASS_CYCLE   0.1
+
+#define P_GAIN  1.2   //0.78   //zensin 1.5
+#define I_GAIN  1000000000.0   
+#define D_GAIN  0.0   //0.009
+
+#define PID_BIAS    0.0
+#define REFERENCE   0
+
+#define OUT_LIMIT 1000
+
+extern double ultrasonicValue[4];
+extern uint16_t ultrasonicVal[4];
+extern void Ultrasonic(void);
+Timer timer2;
+Timer ping_t;
+
+HMC6352 compass(p28/*sda*/, p27/*scl*/);
+Ticker compassUpdata;
+
+PID pid(P_GAIN,I_GAIN,D_GAIN, RATE); 
+PID controller(1.0, 0.0, 0.0, RATE);
+Ticker pidUpdata;
+
+Servo servoR(p25);
+Servo servoL(p26);
+
+DigitalIn  swR(p29);
+DigitalIn  swL(p30);
+
+//DigitalOut led1(LED1); 
+//DigitalOut led2(LED2); 
+
+QEI wheel1 (p24/*A*/, p23/*B*/, NC, 624);
+QEI wheel2 (p22, p21, NC, 624);
+
+#define Convert_dekaruto(a) ((a+1000.0)/2.0/1000.0)
+#define CYCLE 705.0
+
+enum{
+    WAIT,
+    STRAIGHT,
+    TURN,
+    COMP
+};
+
+
+double vl = 0;
+double vc = 0;
+double vs = 1;
+int clwise = 0;
+int anclwise = 0;
+
+int enkoda1;
+int enkoda2;
+
+int process = 0;
+int vc_count = 0;
+int mode_comp = 0;
+
+int inputPID = 0;
\ No newline at end of file

diff -r 000000000000 -r e6bb0bcba274 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Oct 01 07:44:16 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/a9913a65894f
\ No newline at end of file

diff -r 000000000000 -r e6bb0bcba274 ping/ping.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ping/ping.cpp	Tue Oct 01 07:44:16 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

diff -r 000000000000 -r e6bb0bcba274 ping/ping.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ping/ping.h	Tue Oct 01 07:44:16 2013 +0000
@@ -0,0 +1,7 @@
+#define ALL_ULTRASONIC  2
+#define PING_ERR        0xFFFF
+
+
+PinName ultrasonic_pin[ALL_ULTRASONIC] = {
+   p5, p6   
+};