yusuke takahashi
a
Dependencies: ColorSensor1 TextLCD mbed
Revision 0:5b6831d50173, committed 2013-10-09
- Comitter:
- yusuke_robocup
- Date:
- Wed Oct 09 09:37:16 2013 +0000
- Commit message:
- color
Changed in this revision
diff -r 000000000000 -r 5b6831d50173 ColorSensor.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ColorSensor.lib Wed Oct 09 09:37:16 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/OGA/code/ColorSensor1/#745c9de82674
diff -r 000000000000 -r 5b6831d50173 TextLCD.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TextLCD.lib Wed Oct 09 09:37:16 2013 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/simon/code/TextLCD/#44f34c09bd37
diff -r 000000000000 -r 5b6831d50173 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Oct 09 09:37:16 2013 +0000
@@ -0,0 +1,194 @@
+#include "mbed.h"
+#include "ColorSensor.h"
+#include "TextLCD.h"
+
+#include "main.h"
+
+
+void tic_sensor()
+{
+
+ colorUpdate(1);
+ //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]);
+ }else{
+ 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];
+ }
+}
+
+////////////////////////////////////////ジャンププログラム////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////
+uint8_t jumpcondition()
+{
+ uint8_t threshold = 0, t[COLOR_NUM] = {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;
+ }
+}
+
+#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);
+
+
+
+ //air[0] = 0; air[1] = 1;
+
+ interrupt0.attach(&tic_sensor, 0.1/*sec*/);//0.04sec以上じゃないとmain動かない
+
+ //uint8_t button, state=0;
+
+ //init end
+
+ while(1) {
+ SendData0[1] = jumpcondition();
+
+ pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
+ if(jumpcondition()>=1){
+ led[0] = 1; led[1] = 1; led[2] = 1; led[3] = 1;
+ }else{
+ led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
+ }
+ }
+}
diff -r 000000000000 -r 5b6831d50173 main.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h Wed Oct 09 09:37:16 2013 +0000
@@ -0,0 +1,52 @@
+///////////////////////////////////////OGATA//////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////////////
+//センサの数
+#define COLOR_NUM 3
+
+//閾値
+#define R_THR 75
+#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);
+Serial pc(USBTX, USBRX); // tx, rx
+DigitalOut led[4] = {LED1,LED2,LED3,LED4};
+DigitalOut air[2] = {p7,p8};
+
+
+Timer color_t[COLOR_NUM];
+Timer jump_t;
+Ticker interrupt0;
+
+
+void rivisedate ();
+void colorUpdate (uint8_t mode);
+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 ;
+
+
+
+enum{
+ WAIT,
+ STRAIGHT,
+ TURN,
+ COMP
+};
\ No newline at end of file
diff -r 000000000000 -r 5b6831d50173 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Oct 09 09:37:16 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/a9913a65894f \ No newline at end of file