yusuke takahashi / Mbed 2 deprecated TX_Coloers

a

Dependencies:   ColorSensor1 TextLCD mbed

main.cpp@0:5b6831d50173, 2013-10-09 (annotated)

Committer:
yusuke_robocup
Date:
Wed Oct 09 09:37:16 2013 +0000
Revision:
0:5b6831d50173
color

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yusuke_robocup 0:5b6831d50173 1 #include "mbed.h"
yusuke_robocup 0:5b6831d50173 2 #include "ColorSensor.h"
yusuke_robocup 0:5b6831d50173 3 #include "TextLCD.h"
yusuke_robocup 0:5b6831d50173 4
yusuke_robocup 0:5b6831d50173 5 #include "main.h"
yusuke_robocup 0:5b6831d50173 6
yusuke_robocup 0:5b6831d50173 7
yusuke_robocup 0:5b6831d50173 8 void tic_sensor()
yusuke_robocup 0:5b6831d50173 9 {
yusuke_robocup 0:5b6831d50173 10
yusuke_robocup 0:5b6831d50173 11 colorUpdate(1);
yusuke_robocup 0:5b6831d50173 12 //pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
yusuke_robocup 0:5b6831d50173 13 /*lcd.cls();
yusuke_robocup 0:5b6831d50173 14 lcd.locate(0,0);
yusuke_robocup 0:5b6831d50173 15 lcd.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
yusuke_robocup 0:5b6831d50173 16 */
yusuke_robocup 0:5b6831d50173 17 }
yusuke_robocup 0:5b6831d50173 18
yusuke_robocup 0:5b6831d50173 19
yusuke_robocup 0:5b6831d50173 20
yusuke_robocup 0:5b6831d50173 21 ////////////////////////////////////////カラーセンサの////////////////////////////////////////
yusuke_robocup 0:5b6831d50173 22 ////////////////////////////////////////補正プログラム////////////////////////////////////////
yusuke_robocup 0:5b6831d50173 23 void rivisedate()
yusuke_robocup 0:5b6831d50173 24 {
yusuke_robocup 0:5b6831d50173 25 unsigned long red = 0,green = 0,blue =0;
yusuke_robocup 0:5b6831d50173 26 static unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM];
yusuke_robocup 0:5b6831d50173 27
yusuke_robocup 0:5b6831d50173 28 //最初の20回だけ平均を取る
yusuke_robocup 0:5b6831d50173 29 for (int i=0;i<=20;i++){
yusuke_robocup 0:5b6831d50173 30 color0.getRGB(R[0],G[0],B[0]);
yusuke_robocup 0:5b6831d50173 31 red += R[0] ;
yusuke_robocup 0:5b6831d50173 32 green += G[0] ;
yusuke_robocup 0:5b6831d50173 33 blue += B[0] ;
yusuke_robocup 0:5b6831d50173 34 //pc.printf(" %d %d\n",ptm(sum),sum);
yusuke_robocup 0:5b6831d50173 35 }
yusuke_robocup 0:5b6831d50173 36
yusuke_robocup 0:5b6831d50173 37 rir = (double)green/ red ;
yusuke_robocup 0:5b6831d50173 38 rib = (double)green/ blue ;
yusuke_robocup 0:5b6831d50173 39 }
yusuke_robocup 0:5b6831d50173 40
yusuke_robocup 0:5b6831d50173 41 void colorUpdate(uint8_t mode)
yusuke_robocup 0:5b6831d50173 42 {
yusuke_robocup 0:5b6831d50173 43 double colorSum[COLOR_NUM];
yusuke_robocup 0:5b6831d50173 44 unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM];
yusuke_robocup 0:5b6831d50173 45
yusuke_robocup 0:5b6831d50173 46
yusuke_robocup 0:5b6831d50173 47 for (int i=0; i<COLOR_NUM; i++){
yusuke_robocup 0:5b6831d50173 48 R[i] = 0;
yusuke_robocup 0:5b6831d50173 49 G[i] = 0;
yusuke_robocup 0:5b6831d50173 50 B[i] = 0;
yusuke_robocup 0:5b6831d50173 51 redp[i] = 0;
yusuke_robocup 0:5b6831d50173 52 greenp[i] = 0;
yusuke_robocup 0:5b6831d50173 53 bluep[i] = 0;
yusuke_robocup 0:5b6831d50173 54 }
yusuke_robocup 0:5b6831d50173 55
yusuke_robocup 0:5b6831d50173 56 //カラーセンサの切り替え
yusuke_robocup 0:5b6831d50173 57 if(mode == 1){
yusuke_robocup 0:5b6831d50173 58 color0.getRGB(R[0],G[0],B[0]);
yusuke_robocup 0:5b6831d50173 59 color1.getRGB(R[1],G[1],B[1]);
yusuke_robocup 0:5b6831d50173 60 color2.getRGB(R[2],G[2],B[2]);
yusuke_robocup 0:5b6831d50173 61 }else{
yusuke_robocup 0:5b6831d50173 62 color3.getRGB(R[3],G[3],B[3]);
yusuke_robocup 0:5b6831d50173 63 color4.getRGB(R[4],G[4],B[4]);
yusuke_robocup 0:5b6831d50173 64 color5.getRGB(R[5],G[5],B[5]);
yusuke_robocup 0:5b6831d50173 65 }
yusuke_robocup 0:5b6831d50173 66
yusuke_robocup 0:5b6831d50173 67 /*for (int i=0; i<COLOR_NUM; i++){
yusuke_robocup 0:5b6831d50173 68 colorSum[i] = R[i]*rir + G[i] + B[i]*rib ;
yusuke_robocup 0:5b6831d50173 69 redp[i] = R[i]* rir * 100 / colorSum[i];
yusuke_robocup 0:5b6831d50173 70 greenp[i] = G[i] * 100 / colorSum[i];
yusuke_robocup 0:5b6831d50173 71 bluep[i] = B[i]* rib * 100 / colorSum[i];
yusuke_robocup 0:5b6831d50173 72 }*/
yusuke_robocup 0:5b6831d50173 73
yusuke_robocup 0:5b6831d50173 74 for (int i=0; i<COLOR_NUM; i++){
yusuke_robocup 0:5b6831d50173 75 colorSum[i] = R[i]*0.65 + G[i] + B[i] *1.3;
yusuke_robocup 0:5b6831d50173 76 redp[i] = R[i]*0.65 * 100 / colorSum[i];
yusuke_robocup 0:5b6831d50173 77 greenp[i] = G[i] * 100 / colorSum[i];
yusuke_robocup 0:5b6831d50173 78 bluep[i] = B[i]*1.3 * 100 / colorSum[i];
yusuke_robocup 0:5b6831d50173 79 }
yusuke_robocup 0:5b6831d50173 80 }
yusuke_robocup 0:5b6831d50173 81
yusuke_robocup 0:5b6831d50173 82 ////////////////////////////////////////ジャンププログラム////////////////////////////////////
yusuke_robocup 0:5b6831d50173 83 ///////////////////////////////////////////////////////////////////////////////////////////
yusuke_robocup 0:5b6831d50173 84 uint8_t jumpcondition()
yusuke_robocup 0:5b6831d50173 85 {
yusuke_robocup 0:5b6831d50173 86 uint8_t threshold = 0, t[COLOR_NUM] = {0};
yusuke_robocup 0:5b6831d50173 87
yusuke_robocup 0:5b6831d50173 88 //青から赤に0.5秒以内に反応したらジャンプ
yusuke_robocup 0:5b6831d50173 89 for(int i=0; i<COLOR_NUM; i++){
yusuke_robocup 0:5b6831d50173 90 if(bluep[i] >= B_THR){
yusuke_robocup 0:5b6831d50173 91 color_t[i].reset();
yusuke_robocup 0:5b6831d50173 92 color_t[i].start();
yusuke_robocup 0:5b6831d50173 93 t[i] = 0;
yusuke_robocup 0:5b6831d50173 94 }else if(redp[i] >= R_THR){
yusuke_robocup 0:5b6831d50173 95 t[i] = color_t[i].read_ms();
yusuke_robocup 0:5b6831d50173 96 }else{
yusuke_robocup 0:5b6831d50173 97 t[i] = 0;
yusuke_robocup 0:5b6831d50173 98 }
yusuke_robocup 0:5b6831d50173 99
yusuke_robocup 0:5b6831d50173 100 if((t[i] <= 500) && (t[i] != 0)){
yusuke_robocup 0:5b6831d50173 101 threshold++;
yusuke_robocup 0:5b6831d50173 102 }
yusuke_robocup 0:5b6831d50173 103 }
yusuke_robocup 0:5b6831d50173 104
yusuke_robocup 0:5b6831d50173 105 return threshold;
yusuke_robocup 0:5b6831d50173 106 }
yusuke_robocup 0:5b6831d50173 107 /*
yusuke_robocup 0:5b6831d50173 108 void jumping(uint8_t threshold)
yusuke_robocup 0:5b6831d50173 109 {
yusuke_robocup 0:5b6831d50173 110 //超音波でジャンプのタイミング合わせる
yusuke_robocup 0:5b6831d50173 111 if(threshold >= 1){
yusuke_robocup 0:5b6831d50173 112 jump_t.reset();
yusuke_robocup 0:5b6831d50173 113 jump_t.start();
yusuke_robocup 0:5b6831d50173 114 while(ultrasonicVal[0] < 1700){
yusuke_robocup 0:5b6831d50173 115 led[0] = 1; led[1] = 1; led[2] = 0; led[3] = 0;
yusuke_robocup 0:5b6831d50173 116 air[0] = 1; air[1] = 0;
yusuke_robocup 0:5b6831d50173 117
yusuke_robocup 0:5b6831d50173 118 if(jump_t.read_ms() > 1000)break;
yusuke_robocup 0:5b6831d50173 119 }
yusuke_robocup 0:5b6831d50173 120 led[0] = 0; led[1] = 0; led[2] = 1; led[3] = 1;
yusuke_robocup 0:5b6831d50173 121 air[0] = 0; air[1] = 1;
yusuke_robocup 0:5b6831d50173 122 wait(0.5);
yusuke_robocup 0:5b6831d50173 123 }else{
yusuke_robocup 0:5b6831d50173 124 led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
yusuke_robocup 0:5b6831d50173 125 }
yusuke_robocup 0:5b6831d50173 126 }*/
yusuke_robocup 0:5b6831d50173 127
yusuke_robocup 0:5b6831d50173 128 uint8_t robostop()
yusuke_robocup 0:5b6831d50173 129 {
yusuke_robocup 0:5b6831d50173 130 if(bluep[1] >= B_THR){
yusuke_robocup 0:5b6831d50173 131 return 1;
yusuke_robocup 0:5b6831d50173 132 }else{
yusuke_robocup 0:5b6831d50173 133 return 0;
yusuke_robocup 0:5b6831d50173 134 }
yusuke_robocup 0:5b6831d50173 135 }
yusuke_robocup 0:5b6831d50173 136
yusuke_robocup 0:5b6831d50173 137 #define SEND_DATA_NUM 7
yusuke_robocup 0:5b6831d50173 138 #define RECEIVE_DATA_NUM 7
yusuke_robocup 0:5b6831d50173 139
yusuke_robocup 0:5b6831d50173 140 #define KEYCODE 120
yusuke_robocup 0:5b6831d50173 141
yusuke_robocup 0:5b6831d50173 142 Serial device(p28,p27);
yusuke_robocup 0:5b6831d50173 143
yusuke_robocup 0:5b6831d50173 144 static uint8_t SendData0[SEND_DATA_NUM];
yusuke_robocup 0:5b6831d50173 145
yusuke_robocup 0:5b6831d50173 146 void dev_tx()
yusuke_robocup 0:5b6831d50173 147 {
yusuke_robocup 0:5b6831d50173 148 static uint8_t count = 0;
yusuke_robocup 0:5b6831d50173 149
yusuke_robocup 0:5b6831d50173 150 SendData0[0] = KEYCODE;
yusuke_robocup 0:5b6831d50173 151
yusuke_robocup 0:5b6831d50173 152 device.putc(SendData0[count]);
yusuke_robocup 0:5b6831d50173 153
yusuke_robocup 0:5b6831d50173 154 count++;
yusuke_robocup 0:5b6831d50173 155
yusuke_robocup 0:5b6831d50173 156 if(count > SEND_DATA_NUM){
yusuke_robocup 0:5b6831d50173 157 count = 0;
yusuke_robocup 0:5b6831d50173 158 }
yusuke_robocup 0:5b6831d50173 159
yusuke_robocup 0:5b6831d50173 160 led[4] = 1;
yusuke_robocup 0:5b6831d50173 161 }
yusuke_robocup 0:5b6831d50173 162
yusuke_robocup 0:5b6831d50173 163
yusuke_robocup 0:5b6831d50173 164 int main() {
yusuke_robocup 0:5b6831d50173 165 //init
yusuke_robocup 0:5b6831d50173 166
yusuke_robocup 0:5b6831d50173 167 device.baud(9600);
yusuke_robocup 0:5b6831d50173 168 device.printf("START");
yusuke_robocup 0:5b6831d50173 169 device.attach(&dev_tx,Serial::TxIrq);
yusuke_robocup 0:5b6831d50173 170
yusuke_robocup 0:5b6831d50173 171 //rivisedate();
yusuke_robocup 0:5b6831d50173 172 wait(3);
yusuke_robocup 0:5b6831d50173 173
yusuke_robocup 0:5b6831d50173 174
yusuke_robocup 0:5b6831d50173 175
yusuke_robocup 0:5b6831d50173 176 //air[0] = 0; air[1] = 1;
yusuke_robocup 0:5b6831d50173 177
yusuke_robocup 0:5b6831d50173 178 interrupt0.attach(&tic_sensor, 0.1/*sec*/);//0.04sec以上じゃないとmain動かない
yusuke_robocup 0:5b6831d50173 179
yusuke_robocup 0:5b6831d50173 180 //uint8_t button, state=0;
yusuke_robocup 0:5b6831d50173 181
yusuke_robocup 0:5b6831d50173 182 //init end
yusuke_robocup 0:5b6831d50173 183
yusuke_robocup 0:5b6831d50173 184 while(1) {
yusuke_robocup 0:5b6831d50173 185 SendData0[1] = jumpcondition();
yusuke_robocup 0:5b6831d50173 186
yusuke_robocup 0:5b6831d50173 187 pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
yusuke_robocup 0:5b6831d50173 188 if(jumpcondition()>=1){
yusuke_robocup 0:5b6831d50173 189 led[0] = 1; led[1] = 1; led[2] = 1; led[3] = 1;
yusuke_robocup 0:5b6831d50173 190 }else{
yusuke_robocup 0:5b6831d50173 191 led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
yusuke_robocup 0:5b6831d50173 192 }
yusuke_robocup 0:5b6831d50173 193 }
yusuke_robocup 0:5b6831d50173 194 }