Ryo Ogata / Mbed 2 deprecated jumpROBO6_2

aaaa

Dependencies:   ColorSensor1 Servo mbed

Files at this revision

API Documentation at this revision

Revision 0:4c7c138f3891, committed 2013-10-19

Comitter:
OGA
Date:
Sat Oct 19 04:50:26 2013 +0000
Commit message:
10/19;

Changed in this revision

ColorSensor.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
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 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ColorSensor.lib	Sat Oct 19 04:50:26 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/OGA/code/ColorSensor1/#745c9de82674

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Sat Oct 19 04:50:26 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/main.cpp	Sat Oct 19 04:50:26 2013 +0000
@@ -0,0 +1,269 @@
+//カラーセンサ6つ同時に動かすプログラム
+//大会用ロボ
+//ジャンプのプログラムも入ってる
+//10月17日に使う
+#include "mbed.h"
+#include "ColorSensor.h"
+#include "Servo.h"
+
+#include "main.h"
+
+
+
+
+int cAve,roboF, jumI;
+
+void tic_sensor()
+{
+    colorUpdate(1);
+    roboF = robotFront();
+    jumI = jumpInstruction(roboF);
+    //pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
+    /*lcd.cls();
+    lcd.locate(0,0);
+    lcd.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
+    */
+}
+
+
+
+////////////////////////////////////////カラーセンサの////////////////////////////////////////
+////////////////////////////////////////更新と補正プログラム///////////////////////////////////
+/*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(uint8_t mode)
+{
+    double colorSum[COLOR_NUM];
+    unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM];
+
+
+    for (int i=0; i<COLOR_NUM; i++){
+        R[i] = 0;
+        G[i] = 0;
+        B[i] = 0;
+        redp[i]   = 0;
+        greenp[i] = 0;
+        bluep[i]  = 0;
+    }
+    
+    //カラーセンサの切り替え    
+    if(mode == 1){
+        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];
+    }*/ 
+ 
+    for (int i=0; i<COLOR_NUM; i++){
+        colorSum[i] = R[i]*0.65 + G[i] + B[i] *1.3;
+        redp[i]   = R[i]*0.65 * 100 / colorSum[i];
+        greenp[i] = G[i]      * 100 / colorSum[i];
+        bluep[i]  = B[i]*1.3 * 100 / colorSum[i];
+    }
+}
+
+
+uint16_t moving_ave(uint8_t num, uint16_t data)//移動平均的な
+{
+    uint8_t threshold = 0;
+    static uint16_t tmp[COLOR_NUM][10] = {{0}};
+    //static uint32_t sum[COLOR_NUM] = {0};
+    
+    //sum[num] -= tmp[num][9];
+    //sum[num] += data;
+    
+    for(uint8_t i=9; i>=1; i--){
+        tmp[num][i] = tmp[num][i-1];
+    }
+    tmp[num][0] = data;
+    
+    for(uint8_t i=0; i<=9; i++){
+        if(tmp[num][i] >= B_THR){
+            threshold++;
+        }
+    }
+    
+    return threshold;
+}
+
+////////////////////////////////////////ロボの状態///////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////
+uint8_t robotFront()//縄回しロボの正面にいるか
+{
+    uint8_t threshold = 0;
+    
+    for(int i=0; i<COLOR_NUM; i++){
+        if(moving_ave(i, bluep[i]) >= 8) threshold++;
+    }
+    
+    
+    /*for(int i=0; i<COLOR_NUM; i++){
+        cAve = moving_ave(i, bluep[i]);
+        if(cAve >= 8) threshold++;
+    }*/
+
+    if(threshold >= 1){
+        return 1;
+    }else{
+        return 0;
+    }
+}
+
+uint8_t jumpInstruction(uint8_t front)//ジャンプ命令
+{
+    uint8_t threshold = 0;
+    
+    if(front == 1){
+        for(int i=0; i<COLOR_NUM; i++){
+            if(redp[i] >= R_THR) threshold++;
+        }
+    }
+    
+    if(threshold >= 1){
+        return 1;
+    }else{
+        return 0;
+    }
+}
+
+
+uint8_t robotoStop()
+{
+    uint8_t threshold = 0;
+
+    for(int i=0; i<COLOR_NUM; i++){
+        if(bluep[i] >= B_THR) threshold++;
+    }
+    
+    if(threshold >= 1){
+        return 1;
+    }else{
+        return 0;
+    }
+}
+
+
+////////////////////////////////////////ジャンププログラム////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////
+/*
+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;
+    }
+}*/
+
+/*void jumpAction(uint8_t threshold)
+{
+    if(sw1){//フォトインタラプタがオフ
+        myservo1 = 0.3; led[0] = 0;led[1] = 1; led[2] = 0; led[3] = 0;
+    }else{//フォトインタラプタがオン
+        myservo1 = 0.5; led[0] = 1;led[1] = 0; led[2] = 0; led[3] = 0;
+    }
+        
+    if(threshold){
+        led[0] = 1; led[1] = 1; led[2] = 1; led[3] = 1;
+        myservo1 = 0.3;
+        wait(0.2);
+    }else{
+        //led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
+    }
+    
+}*/
+
+
+
+
+
+
+#define SEND_DATA_NUM       7
+#define RECEIVE_DATA_NUM    7
+
+#define KEYCODE             120
+
+Serial device(p28,p27);
+
+static uint8_t SendData0[SEND_DATA_NUM];
+
+void dev_tx()
+{
+    static uint8_t count = 0;
+    
+    SendData0[0] = KEYCODE;
+
+    device.putc(SendData0[count]);
+     
+    count++;
+    
+    if(count > SEND_DATA_NUM){
+        count = 0;
+    }
+    
+    led[4] = 1;
+}
+
+
+
+
+
+int main() {
+    //init
+
+    /*device.baud(9600);
+    device.printf("START");
+    device.attach(&dev_tx,Serial::TxIrq);*/
+
+    //rivisedate();
+    wait(3);
+
+    interrupt0.attach(&tic_sensor, 0.1/*sec*/);//0.04sec以上じゃないとmain動かない?
+    //sw1.mode(PullUp);
+    //init end
+
+    while(1) {
+        SendData0[1] = jumI;    
+        
+        pc.printf("R:%d G:%d B:%d %t moving_ave():%d %t robotFront():%d %t jumpInstruction():%d\n", redp[2], greenp[2], bluep[2], cAve, roboF, jumI);
+        
+        //jumpAction(jumI);
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h	Sat Oct 19 04:50:26 2013 +0000
@@ -0,0 +1,64 @@
+///////////////////////////////////////OGATA//////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////
+//センサの数
+#define COLOR_NUM 6
+
+//閾値
+#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);
+ColorSensor color3(p8, p5, p6, p7, 10);
+
+ColorSensor color4(p24, p21, p22, p23, 10);
+ColorSensor color5(p30, p25, p26, p29, 10);
+//ColorSensor color4(p26, p23, p24, p25, 10);
+//ColorSensor color5(p30, p27, p28, p29, 10);
+
+Serial pc(USBTX, USBRX);    // tx, rx 
+DigitalOut led[4] = {LED1,LED2,LED3,LED4};
+//DigitalOut air[2] = {p7,p8};
+
+
+//Servo myservo1(p21);
+//DigitalIn sw1(p22);
+
+
+
+Timer color_t[COLOR_NUM];
+Timer jump_t;
+Ticker interrupt0;
+
+
+void rivisedate ();
+void colorUpdate (uint8_t mode);
+uint16_t moving_ave(uint8_t num, uint16_t data);
+uint8_t robotFront();
+uint8_t jumpInstruction(uint8_t front);
+uint8_t robotoStop();
+
+
+double proportional = 0;
+uint16_t com_val = 0;
+unsigned redp[COLOR_NUM], greenp[COLOR_NUM], bluep[COLOR_NUM];
+double rir,rib ;
+
+
+
+enum{
+    WAIT,
+    STRAIGHT,
+    TURN,
+    COMP
+};
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sat Oct 19 04:50:26 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/a9913a65894f
\ No newline at end of file