Ryo Ogata / Mbed 2 deprecated jumpROBO5_2

gattai

Dependencies:   ColorSensor1 mbed

main.cpp@0:3a98a198e6d1, 2013-10-05 (annotated)

Committer:
OGA
Date:
Sat Oct 05 02:08:45 2013 +0000
Revision:
0:3a98a198e6d1
tougou

Who changed what in which revision?

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