Ryo Ogata / Mbed 2 deprecated jumpROBO3_5

oooga

Dependencies:   HMC6352 Servo TextLCD mbed

Files at this revision

API Documentation at this revision

Revision 0:e0e1b495278b, committed 2013-09-11

Comitter:
OGA
Date:
Wed Sep 11 05:21:06 2013 +0000
Commit message:
ooo

Changed in this revision

ColorSensor.cpp Show annotated file Show diff for this revision Revisions of this file
ColorSensor.h Show annotated file Show diff for this revision Revisions of this file
HMC6352.lib Show annotated file Show diff for this revision Revisions of this file
Servo.lib Show annotated file Show diff for this revision Revisions of this file
TextLCD.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
main.h Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file 
diff -r 000000000000 -r e0e1b495278b ColorSensor.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ColorSensor.cpp	Wed Sep 11 05:21:06 2013 +0000
@@ -0,0 +1,148 @@
+#include "ColorSensor.h"
+// カラーセンサーテストプログラム
+// 赤、緑、青を判定する。
+#define coef 1.05
+
+ColorSensor::ColorSensor(
+    PinName dout, PinName range, 
+    PinName ck, PinName gate, int tm)
+{
+  Dout  = new DigitalIn(dout);
+  Range = new DigitalOut(range);
+  CK    = new DigitalOut(ck);
+  Gate  = new DigitalOut(gate);
+  time  = tm;       // 積算時間(ms)
+
+  *CK = 0;
+  *Gate = 0;
+  *Range= 1;    // 高感度モード
+  setWhite();
+}
+
+void ColorSensor::setWhite()
+{
+  unsigned short RGB[3];
+  getRGB(RGB);
+  R = RGB[0];
+  G = RGB[1];
+  B = RGB[2];
+}
+
+ColorSensor::ColorSensor(void) {
+  unsigned short RGB[3];
+  double R, G, B, r,g,b;
+
+  Dout  = new DigitalIn(p5);    // Dout
+  Range = new DigitalOut(p6);   // Range
+  CK    = new DigitalOut(p7);   // CK
+  Gate  = new DigitalOut(p8);   // Gate
+
+  *CK   = 0; //ck
+  *Gate = 0; //gate
+  *Range = 1;  // 高感度モード range
+  time = 10;   // 10ms
+  setWhite();
+}
+
+void ColorSensor::getRGB(unsigned short RGB[])
+{
+    unsigned short i, j, coldata;
+
+     *Gate = 1; 
+     wait_ms(time);      // 積算時間
+     *Gate  = 0; 
+     for(i=0; i<3; i++) {
+       coldata=0;
+       for(j=0; j<12; j++) {
+         *CK = 1; 
+         wait_us(1);
+         coldata>>=1;
+         if(*Dout) coldata|=0x800;
+         *CK = 0;
+         wait_us(1);
+      }
+      RGB[i]=coldata;
+    }
+}
+
+void ColorSensor::getRGB(
+  unsigned& R, unsigned& G, unsigned& B)
+{
+    unsigned i, j, coldata;
+
+     *Gate = 1; 
+     wait_ms(time);       //  積算時間
+     *Gate = 0;
+     for(i=0; i<3; i++) {
+       coldata=0;
+       for(j=0; j<12; j++) {
+         *CK = 1;
+         wait_us(1);
+         coldata>>=1;
+         if(*Dout) coldata|=0x800;
+         *CK = 0;
+         wait_us(1);
+       }
+       switch(i) {
+        case 0: R=coldata; break;
+        case 1: G=coldata; break;
+        case 2: B=coldata; break;
+      }
+    }
+  }
+
+  // 比率を%で返す
+unsigned ColorSensor::checkRGB(
+  unsigned& R, unsigned& G, unsigned& B)
+{
+     unsigned i, j, coldata;
+     double I;
+     
+     *Gate = 1; 
+     wait_ms(time);      //  積算時間
+     *Gate = 0;
+     for(i=0; i<3; i++) {
+       coldata=0;
+       for(j=0; j<12; j++) {
+         *CK = 1; 
+         wait_us(1);
+         coldata>>=1;
+         if(*Dout) coldata|=0x800;
+         *CK = false;
+         wait_us(1);
+       }
+       switch(i) {
+        case 0: R=coldata; break;
+        case 1: G=coldata; break;
+        case 2: B=coldata; break;
+      }
+    }
+    I = R*0.65 + G + B*1.3;
+    R = R*0.65 *100/I;
+    G = G*100/I;
+    B = B*1.3 *100/I;
+    return I;
+}
+
+int ColorSensor::judge()
+{
+  unsigned short RGB[3];
+  double r,g,b;
+  *CK   = 0; //ck
+  *Gate = 0; //gate
+  *Range = 1;  // 高感度モード range
+
+  getRGB(RGB);
+  r = RGB[0]*100/R;
+  g = RGB[1]*100/G;
+  b = RGB[2]*100/B;
+  if(r > g) {
+       if(r > b*coef) return RED;
+       else if(b > r*coef) return GREEN;
+  }
+   else {
+     if(g > b*coef) return GREEN;
+     else if(b > g*coef) return BLUE;
+   }
+  return OTHER;
+}
\ No newline at end of file

diff -r 000000000000 -r e0e1b495278b ColorSensor.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ColorSensor.h	Wed Sep 11 05:21:06 2013 +0000
@@ -0,0 +1,23 @@
+#include "mbed.h"
+#define RED   0
+#define GREEN 1
+#define BLUE  2
+#define OTHER 3
+class ColorSensor
+{
+  private:
+    DigitalIn *Dout;
+    DigitalOut *Range;
+    DigitalOut *CK;
+    DigitalOut *Gate;
+    int time;
+    double R, G, B;
+  public:
+    ColorSensor(PinName Dout, PinName Range, PinName CK, PinName Gate, int time);
+    ColorSensor(void);
+    void getRGB(unsigned short RGB[]);
+    void getRGB(unsigned& R, unsigned& G, unsigned& B);
+    unsigned checkRGB(unsigned& R, unsigned& G, unsigned& B);
+    int judge();
+    void setWhite();
+};
\ No newline at end of file

diff -r 000000000000 -r e0e1b495278b HMC6352.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC6352.lib	Wed Sep 11 05:21:06 2013 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/aberk/code/HMC6352/#83c0cb554099

diff -r 000000000000 -r e0e1b495278b Servo.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Wed Sep 11 05:21:06 2013 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/simon/code/Servo/#36b69a7ced07

diff -r 000000000000 -r e0e1b495278b TextLCD.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TextLCD.lib	Wed Sep 11 05:21:06 2013 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/simon/code/TextLCD/#44f34c09bd37

diff -r 000000000000 -r e0e1b495278b main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Sep 11 05:21:06 2013 +0000
@@ -0,0 +1,177 @@
+//歩くだけ
+#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 60
+#define G_THR 60
+#define B_THR 60
+
+
+//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);
+
+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];
+
+
+double proportional = 0;
+uint16_t com_val = 0;
+
+
+#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 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;
+}
+
+
+
+//青から赤に反応したらジャンプ
+//pidUpdataのせいでカラーセンサが動かない!!
+int main()
+{
+    wait(3);
+
+    unsigned R, G, B;
+    unsigned threshold, t[3] = {0} ;
+    double color_sum;
+    unsigned redp[COLOR_NUM], greenp[COLOR_NUM], bluep[COLOR_NUM];
+    //pc.baud(115200);
+    air[0] = 0; air[1] = 1;
+  
+    int vl;
+    double vs;
+    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    pidUpdata.attach(&PidUpdate, PID_CYCLE);
+  
+  
+  while(1)
+  {
+     
+    color0.checkRGB(redp[0],greenp[0],bluep[0]);
+    color1.checkRGB(redp[1],greenp[1],bluep[1]);
+    color2.checkRGB(redp[2],greenp[2],bluep[2]);
+    
+    //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]);
+     */
+     
+
+     
+    
+    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.8);
+            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;
+    }
+    
+    
+    
+    vl = -90;
+    //vl = 0;
+    
+    vl = vl      * STRAIGHT ;
+    vs = vsOut() * SPIN     ;
+    
+    vl *= 0.5;
+    vs *= 0.3;
+        
+    move(vl,(int)vs); 
+  }
+}
\ No newline at end of file

diff -r 000000000000 -r e0e1b495278b main.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h	Wed Sep 11 05:21:06 2013 +0000
@@ -0,0 +1,99 @@
+#include "mbed.h"
+
+Timer timer2;
+
+extern double ultrasonicValue[4];
+extern uint16_t ultrasonicVal[4];
+extern void Ultrasonic(void);
+
+/* robocup */
+
+#define RATE    0.052
+#define Long    1.0
+#define ENTER   0
+#define EXIT    1
+#define X       0
+#define Y       1
+#define MOT_NUM 4
+#define MOTDRIVER_WAIT  300 //ms
+#define BAUD_RATE       115200
+#define BAUD_RATE2      19200
+#define BUT_WAIT        0.3 //s
+#define ON      1
+#define OFF     0
+
+#define PING_ERROR  0xFFFF
+#define PI          3.14159265
+
+#define MOT1    1.0
+#define MOT2    1.0
+#define MOT3    1.0
+#define MOT4    1.0
+
+#define PID_BIAS    0.0
+#define REFERENCE   180.0
+#define MINIMUM     0.0
+#define MAXIMUM     360.0
+
+#define PID_CYCLE   0.06    //s
+
+#define P_GAIN  0.75    //0.78   
+#define I_GAIN  0.0     //0.0
+#define D_GAIN  0.006   //0.009
+    
+#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);
+BusOut mbedleds(LED4,LED3,LED2,LED1); 
+HMC6352 compass(p28, p27);
+//Serial driver(p28, p27);    // tx, rx 
+Serial device2(p13, p14);   // tx, rx
+Serial pc(USBTX, USBRX);    // tx, rx 
+//DigitalIn StartButton(p21);
+//DigitalIn CalibEnterButton(p22);
+DigitalIn CalibExitButton(p23);
+DigitalIn EEPROMButton(p24);
+//PID pid(P_GAIN,I_GAIN,D_GAIN, RATE);      //battery is low power version 30.0 0.58 1.0 0.015    battery is high power version   30.0 0.42, 1.0, 0.013   power is low but perfect 20.0 0.45, 0.0, 0.010
+Ticker pidUpdata;
+Ticker irDistanceUpdata;
+Timer timer1;
+Timer Survtimer;
+LocalFileSystem local("local");  // マウントポイントを定義（ディレクトリパスになる）
+
+enum{
+    NORMAL,
+    LEFT_OUT,
+    RIGHT_OUT,
+    FRONT_OUT,
+    BACK_OUT,
+};
+
+enum{
+    HOME_WAIT,
+    DIFFENCE,
+    WARNING,
+    HOLD,
+};
+
+PinName adc_num[6] = {
+    p15,
+    p16,
+    p17,
+    p18,
+    p19,
+    p20,
+};
+double standTu = 0;
+int speed[MOT_NUM] = {0};
+uint8_t hold_flag = 0;
+uint8_t state = HOME_WAIT;
+uint8_t lineState = NORMAL;
+
+double inputPID = 180.0;
+static double standard;
+double compassPID = 0.0;
\ No newline at end of file

diff -r 000000000000 -r e0e1b495278b mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Sep 11 05:21:06 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/9c8f0e3462fb
\ No newline at end of file