Ryo Ogata / Mbed 2 deprecated jumpROBO6_3

10/22

Dependencies:   ColorSensor1 Servo mbed

Files at this revision

API Documentation at this revision

Revision 0:fa26bf0aad5e, committed 2013-10-22

Comitter:
OGA
Date:
Tue Oct 22 07:19:27 2013 +0000
Commit message:
10/22;

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 
diff -r 000000000000 -r fa26bf0aad5e ColorSensor.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ColorSensor.lib	Tue Oct 22 07:19:27 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/OGA/code/ColorSensor1/#745c9de82674

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

diff -r 000000000000 -r fa26bf0aad5e main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 22 07:19:27 2013 +0000
@@ -0,0 +1,244 @@
+//カラーセンサ6つ同時に動かすプログラム
+//大会用ロボ
+//ジャンプのプログラムも入ってる
+//ジャンプ命令のプログラムを変更
+//6_2の使わないところを消したプログラム
+//2回連続ジャンプシーケンス予定
+//5回連続ジャンプシーケンス搭載予定
+//10/21使用
+#include "mbed.h"
+#include "ColorSensor.h"
+#include "Servo.h"
+
+#include "main.h"
+
+
+void tic_sensor()
+{
+    colorUpdate(1);
+    
+    
+    for(int i=0; i<COLOR_NUM; i++){
+        cAve[i] = moving_ave(i, bluep[i]);
+    }
+    
+    roboF = robotFront();
+    jumI = jumpInstruction(roboF);
+
+}
+
+////////////////////////////////////////カラーセンサの////////////////////////////////////////
+////////////////////////////////////////更新と補正プログラム///////////////////////////////////
+/*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(cAve[i] >= 8) threshold++;
+    }
+
+    if(threshold >= 1){
+        return 1;
+    }else{
+        return 0;
+    }
+}
+
+uint8_t jumpInstruction(uint8_t front)//ジャンプ命令
+{
+    uint8_t threshold = 0;
+    
+    for(int i=0; i<COLOR_NUM; i++){
+        if((cAve[i] >= 8) && (redp[i] >= R_THR))  threshold++;
+    }
+    
+    if(threshold >= 1){
+        return 1;
+    }else{
+        return 0;
+    }
+}
+
+
+
+////////////////////////////////////////ジャンププログラム////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////
+void jumpAction(uint8_t threshold)
+{
+    static uint8_t swFlag=0, jumpCount=0, jumpwait = 0;
+    
+    
+    if((sw1 == 0) && (swFlag == 0)){//フォトインタラプタがオン
+        swFlag = 1;
+        jumpCount++;
+    }else if(sw1 == 1){//フォトインタラプタがオフ
+        swFlag = 0;
+    }
+    
+    if(threshold){
+        led[0] = 1; led[1] = 1; led[2] = 1; led[3] = 1;
+        myservo1 = 0;
+        
+        wait(0.4);
+    }
+    
+    if(jumpCount == 1){
+        /*if(jumpwait == 0){
+            myservo1 = 0.5;
+            wait(1);
+            jumpwait = 1;
+        }*/
+        myservo1 = 0.3; led[0] = 1;led[1] = 1; led[2] = 1; led[3] = 0;
+    }else if(jumpCount >= 2){
+        myservo1 = 0.5; led[0] = 1;led[1] = 1; led[2] = 0; led[3] = 0;
+        jumpCount = 0;
+        jumpwait = 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
+
+    myservo1 = 0.1;
+    wait(0.5);
+
+    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);
+    }
+}

diff -r 000000000000 -r fa26bf0aad5e main.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h	Tue Oct 22 07:19:27 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);
+void jumpAction(uint8_t threshold);
+
+
+double proportional = 0;
+uint16_t com_val = 0;
+unsigned redp[COLOR_NUM], greenp[COLOR_NUM], bluep[COLOR_NUM];
+double rir,rib ;
+int cAve[COLOR_NUM] = {0}, roboF, jumI;
+
+
+enum{
+    WAIT,
+    STRAIGHT,
+    TURN,
+    COMP
+};
\ No newline at end of file

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