Ryo Ogata / Mbed 2 deprecated jumpROBO6_2

aaaa

Dependencies:   ColorSensor1 Servo mbed

main.cpp@0:4c7c138f3891, 2013-10-19 (annotated)

Committer:
OGA
Date:
Sat Oct 19 04:50:26 2013 +0000
Revision:
0:4c7c138f3891
10/19;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
OGA 0:4c7c138f3891 1 //カラーセンサ6つ同時に動かすプログラム
OGA 0:4c7c138f3891 2 //大会用ロボ
OGA 0:4c7c138f3891 3 //ジャンプのプログラムも入ってる
OGA 0:4c7c138f3891 4 //10月17日に使う
OGA 0:4c7c138f3891 5 #include "mbed.h"
OGA 0:4c7c138f3891 6 #include "ColorSensor.h"
OGA 0:4c7c138f3891 7 #include "Servo.h"
OGA 0:4c7c138f3891 8
OGA 0:4c7c138f3891 9 #include "main.h"
OGA 0:4c7c138f3891 10
OGA 0:4c7c138f3891 11
OGA 0:4c7c138f3891 12
OGA 0:4c7c138f3891 13
OGA 0:4c7c138f3891 14 int cAve,roboF, jumI;
OGA 0:4c7c138f3891 15
OGA 0:4c7c138f3891 16 void tic_sensor()
OGA 0:4c7c138f3891 17 {
OGA 0:4c7c138f3891 18 colorUpdate(1);
OGA 0:4c7c138f3891 19 roboF = robotFront();
OGA 0:4c7c138f3891 20 jumI = jumpInstruction(roboF);
OGA 0:4c7c138f3891 21 //pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
OGA 0:4c7c138f3891 22 /*lcd.cls();
OGA 0:4c7c138f3891 23 lcd.locate(0,0);
OGA 0:4c7c138f3891 24 lcd.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
OGA 0:4c7c138f3891 25 */
OGA 0:4c7c138f3891 26 }
OGA 0:4c7c138f3891 27
OGA 0:4c7c138f3891 28
OGA 0:4c7c138f3891 29
OGA 0:4c7c138f3891 30 ////////////////////////////////////////カラーセンサの////////////////////////////////////////
OGA 0:4c7c138f3891 31 ////////////////////////////////////////更新と補正プログラム///////////////////////////////////
OGA 0:4c7c138f3891 32 /*void rivisedate()
OGA 0:4c7c138f3891 33 {
OGA 0:4c7c138f3891 34 unsigned long red = 0,green = 0,blue =0;
OGA 0:4c7c138f3891 35 static unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM];
OGA 0:4c7c138f3891 36
OGA 0:4c7c138f3891 37 //最初の20回だけ平均を取る
OGA 0:4c7c138f3891 38 for (int i=0;i<=20;i++){
OGA 0:4c7c138f3891 39 color0.getRGB(R[0],G[0],B[0]);
OGA 0:4c7c138f3891 40 red += R[0] ;
OGA 0:4c7c138f3891 41 green += G[0] ;
OGA 0:4c7c138f3891 42 blue += B[0] ;
OGA 0:4c7c138f3891 43 //pc.printf(" %d %d\n",ptm(sum),sum);
OGA 0:4c7c138f3891 44 }
OGA 0:4c7c138f3891 45
OGA 0:4c7c138f3891 46 rir = (double)green/ red ;
OGA 0:4c7c138f3891 47 rib = (double)green/ blue ;
OGA 0:4c7c138f3891 48 }*/
OGA 0:4c7c138f3891 49
OGA 0:4c7c138f3891 50 void colorUpdate(uint8_t mode)
OGA 0:4c7c138f3891 51 {
OGA 0:4c7c138f3891 52 double colorSum[COLOR_NUM];
OGA 0:4c7c138f3891 53 unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM];
OGA 0:4c7c138f3891 54
OGA 0:4c7c138f3891 55
OGA 0:4c7c138f3891 56 for (int i=0; i<COLOR_NUM; i++){
OGA 0:4c7c138f3891 57 R[i] = 0;
OGA 0:4c7c138f3891 58 G[i] = 0;
OGA 0:4c7c138f3891 59 B[i] = 0;
OGA 0:4c7c138f3891 60 redp[i] = 0;
OGA 0:4c7c138f3891 61 greenp[i] = 0;
OGA 0:4c7c138f3891 62 bluep[i] = 0;
OGA 0:4c7c138f3891 63 }
OGA 0:4c7c138f3891 64
OGA 0:4c7c138f3891 65 //カラーセンサの切り替え
OGA 0:4c7c138f3891 66 if(mode == 1){
OGA 0:4c7c138f3891 67 color0.getRGB(R[0],G[0],B[0]);
OGA 0:4c7c138f3891 68 color1.getRGB(R[1],G[1],B[1]);
OGA 0:4c7c138f3891 69 color2.getRGB(R[2],G[2],B[2]);
OGA 0:4c7c138f3891 70
OGA 0:4c7c138f3891 71 color3.getRGB(R[3],G[3],B[3]);
OGA 0:4c7c138f3891 72 color4.getRGB(R[4],G[4],B[4]);
OGA 0:4c7c138f3891 73 color5.getRGB(R[5],G[5],B[5]);
OGA 0:4c7c138f3891 74 }
OGA 0:4c7c138f3891 75
OGA 0:4c7c138f3891 76 /*for (int i=0; i<COLOR_NUM; i++){
OGA 0:4c7c138f3891 77 colorSum[i] = R[i]*rir + G[i] + B[i]*rib ;
OGA 0:4c7c138f3891 78 redp[i] = R[i]* rir * 100 / colorSum[i];
OGA 0:4c7c138f3891 79 greenp[i] = G[i] * 100 / colorSum[i];
OGA 0:4c7c138f3891 80 bluep[i] = B[i]* rib * 100 / colorSum[i];
OGA 0:4c7c138f3891 81 }*/
OGA 0:4c7c138f3891 82
OGA 0:4c7c138f3891 83 for (int i=0; i<COLOR_NUM; i++){
OGA 0:4c7c138f3891 84 colorSum[i] = R[i]*0.65 + G[i] + B[i] *1.3;
OGA 0:4c7c138f3891 85 redp[i] = R[i]*0.65 * 100 / colorSum[i];
OGA 0:4c7c138f3891 86 greenp[i] = G[i] * 100 / colorSum[i];
OGA 0:4c7c138f3891 87 bluep[i] = B[i]*1.3 * 100 / colorSum[i];
OGA 0:4c7c138f3891 88 }
OGA 0:4c7c138f3891 89 }
OGA 0:4c7c138f3891 90
OGA 0:4c7c138f3891 91
OGA 0:4c7c138f3891 92 uint16_t moving_ave(uint8_t num, uint16_t data)//移動平均的な
OGA 0:4c7c138f3891 93 {
OGA 0:4c7c138f3891 94 uint8_t threshold = 0;
OGA 0:4c7c138f3891 95 static uint16_t tmp[COLOR_NUM][10] = {{0}};
OGA 0:4c7c138f3891 96 //static uint32_t sum[COLOR_NUM] = {0};
OGA 0:4c7c138f3891 97
OGA 0:4c7c138f3891 98 //sum[num] -= tmp[num][9];
OGA 0:4c7c138f3891 99 //sum[num] += data;
OGA 0:4c7c138f3891 100
OGA 0:4c7c138f3891 101 for(uint8_t i=9; i>=1; i--){
OGA 0:4c7c138f3891 102 tmp[num][i] = tmp[num][i-1];
OGA 0:4c7c138f3891 103 }
OGA 0:4c7c138f3891 104 tmp[num][0] = data;
OGA 0:4c7c138f3891 105
OGA 0:4c7c138f3891 106 for(uint8_t i=0; i<=9; i++){
OGA 0:4c7c138f3891 107 if(tmp[num][i] >= B_THR){
OGA 0:4c7c138f3891 108 threshold++;
OGA 0:4c7c138f3891 109 }
OGA 0:4c7c138f3891 110 }
OGA 0:4c7c138f3891 111
OGA 0:4c7c138f3891 112 return threshold;
OGA 0:4c7c138f3891 113 }
OGA 0:4c7c138f3891 114
OGA 0:4c7c138f3891 115 ////////////////////////////////////////ロボの状態///////////////////////////////////////////
OGA 0:4c7c138f3891 116 ///////////////////////////////////////////////////////////////////////////////////////////
OGA 0:4c7c138f3891 117 uint8_t robotFront()//縄回しロボの正面にいるか
OGA 0:4c7c138f3891 118 {
OGA 0:4c7c138f3891 119 uint8_t threshold = 0;
OGA 0:4c7c138f3891 120
OGA 0:4c7c138f3891 121 for(int i=0; i<COLOR_NUM; i++){
OGA 0:4c7c138f3891 122 if(moving_ave(i, bluep[i]) >= 8) threshold++;
OGA 0:4c7c138f3891 123 }
OGA 0:4c7c138f3891 124
OGA 0:4c7c138f3891 125
OGA 0:4c7c138f3891 126 /*for(int i=0; i<COLOR_NUM; i++){
OGA 0:4c7c138f3891 127 cAve = moving_ave(i, bluep[i]);
OGA 0:4c7c138f3891 128 if(cAve >= 8) threshold++;
OGA 0:4c7c138f3891 129 }*/
OGA 0:4c7c138f3891 130
OGA 0:4c7c138f3891 131 if(threshold >= 1){
OGA 0:4c7c138f3891 132 return 1;
OGA 0:4c7c138f3891 133 }else{
OGA 0:4c7c138f3891 134 return 0;
OGA 0:4c7c138f3891 135 }
OGA 0:4c7c138f3891 136 }
OGA 0:4c7c138f3891 137
OGA 0:4c7c138f3891 138 uint8_t jumpInstruction(uint8_t front)//ジャンプ命令
OGA 0:4c7c138f3891 139 {
OGA 0:4c7c138f3891 140 uint8_t threshold = 0;
OGA 0:4c7c138f3891 141
OGA 0:4c7c138f3891 142 if(front == 1){
OGA 0:4c7c138f3891 143 for(int i=0; i<COLOR_NUM; i++){
OGA 0:4c7c138f3891 144 if(redp[i] >= R_THR) threshold++;
OGA 0:4c7c138f3891 145 }
OGA 0:4c7c138f3891 146 }
OGA 0:4c7c138f3891 147
OGA 0:4c7c138f3891 148 if(threshold >= 1){
OGA 0:4c7c138f3891 149 return 1;
OGA 0:4c7c138f3891 150 }else{
OGA 0:4c7c138f3891 151 return 0;
OGA 0:4c7c138f3891 152 }
OGA 0:4c7c138f3891 153 }
OGA 0:4c7c138f3891 154
OGA 0:4c7c138f3891 155
OGA 0:4c7c138f3891 156 uint8_t robotoStop()
OGA 0:4c7c138f3891 157 {
OGA 0:4c7c138f3891 158 uint8_t threshold = 0;
OGA 0:4c7c138f3891 159
OGA 0:4c7c138f3891 160 for(int i=0; i<COLOR_NUM; i++){
OGA 0:4c7c138f3891 161 if(bluep[i] >= B_THR) threshold++;
OGA 0:4c7c138f3891 162 }
OGA 0:4c7c138f3891 163
OGA 0:4c7c138f3891 164 if(threshold >= 1){
OGA 0:4c7c138f3891 165 return 1;
OGA 0:4c7c138f3891 166 }else{
OGA 0:4c7c138f3891 167 return 0;
OGA 0:4c7c138f3891 168 }
OGA 0:4c7c138f3891 169 }
OGA 0:4c7c138f3891 170
OGA 0:4c7c138f3891 171
OGA 0:4c7c138f3891 172 ////////////////////////////////////////ジャンププログラム////////////////////////////////////
OGA 0:4c7c138f3891 173 ///////////////////////////////////////////////////////////////////////////////////////////
OGA 0:4c7c138f3891 174 /*
OGA 0:4c7c138f3891 175 void jumping(uint8_t threshold)
OGA 0:4c7c138f3891 176 {
OGA 0:4c7c138f3891 177 //超音波でジャンプのタイミング合わせる
OGA 0:4c7c138f3891 178 if(threshold >= 1){
OGA 0:4c7c138f3891 179 jump_t.reset();
OGA 0:4c7c138f3891 180 jump_t.start();
OGA 0:4c7c138f3891 181 while(ultrasonicVal[0] < 1700){
OGA 0:4c7c138f3891 182 led[0] = 1; led[1] = 1; led[2] = 0; led[3] = 0;
OGA 0:4c7c138f3891 183 air[0] = 1; air[1] = 0;
OGA 0:4c7c138f3891 184
OGA 0:4c7c138f3891 185 if(jump_t.read_ms() > 1000)break;
OGA 0:4c7c138f3891 186 }
OGA 0:4c7c138f3891 187 led[0] = 0; led[1] = 0; led[2] = 1; led[3] = 1;
OGA 0:4c7c138f3891 188 air[0] = 0; air[1] = 1;
OGA 0:4c7c138f3891 189 wait(0.5);
OGA 0:4c7c138f3891 190 }else{
OGA 0:4c7c138f3891 191 led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
OGA 0:4c7c138f3891 192 }
OGA 0:4c7c138f3891 193 }*/
OGA 0:4c7c138f3891 194
OGA 0:4c7c138f3891 195 /*void jumpAction(uint8_t threshold)
OGA 0:4c7c138f3891 196 {
OGA 0:4c7c138f3891 197 if(sw1){//フォトインタラプタがオフ
OGA 0:4c7c138f3891 198 myservo1 = 0.3; led[0] = 0;led[1] = 1; led[2] = 0; led[3] = 0;
OGA 0:4c7c138f3891 199 }else{//フォトインタラプタがオン
OGA 0:4c7c138f3891 200 myservo1 = 0.5; led[0] = 1;led[1] = 0; led[2] = 0; led[3] = 0;
OGA 0:4c7c138f3891 201 }
OGA 0:4c7c138f3891 202
OGA 0:4c7c138f3891 203 if(threshold){
OGA 0:4c7c138f3891 204 led[0] = 1; led[1] = 1; led[2] = 1; led[3] = 1;
OGA 0:4c7c138f3891 205 myservo1 = 0.3;
OGA 0:4c7c138f3891 206 wait(0.2);
OGA 0:4c7c138f3891 207 }else{
OGA 0:4c7c138f3891 208 //led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
OGA 0:4c7c138f3891 209 }
OGA 0:4c7c138f3891 210
OGA 0:4c7c138f3891 211 }*/
OGA 0:4c7c138f3891 212
OGA 0:4c7c138f3891 213
OGA 0:4c7c138f3891 214
OGA 0:4c7c138f3891 215
OGA 0:4c7c138f3891 216
OGA 0:4c7c138f3891 217
OGA 0:4c7c138f3891 218 #define SEND_DATA_NUM 7
OGA 0:4c7c138f3891 219 #define RECEIVE_DATA_NUM 7
OGA 0:4c7c138f3891 220
OGA 0:4c7c138f3891 221 #define KEYCODE 120
OGA 0:4c7c138f3891 222
OGA 0:4c7c138f3891 223 Serial device(p28,p27);
OGA 0:4c7c138f3891 224
OGA 0:4c7c138f3891 225 static uint8_t SendData0[SEND_DATA_NUM];
OGA 0:4c7c138f3891 226
OGA 0:4c7c138f3891 227 void dev_tx()
OGA 0:4c7c138f3891 228 {
OGA 0:4c7c138f3891 229 static uint8_t count = 0;
OGA 0:4c7c138f3891 230
OGA 0:4c7c138f3891 231 SendData0[0] = KEYCODE;
OGA 0:4c7c138f3891 232
OGA 0:4c7c138f3891 233 device.putc(SendData0[count]);
OGA 0:4c7c138f3891 234
OGA 0:4c7c138f3891 235 count++;
OGA 0:4c7c138f3891 236
OGA 0:4c7c138f3891 237 if(count > SEND_DATA_NUM){
OGA 0:4c7c138f3891 238 count = 0;
OGA 0:4c7c138f3891 239 }
OGA 0:4c7c138f3891 240
OGA 0:4c7c138f3891 241 led[4] = 1;
OGA 0:4c7c138f3891 242 }
OGA 0:4c7c138f3891 243
OGA 0:4c7c138f3891 244
OGA 0:4c7c138f3891 245
OGA 0:4c7c138f3891 246
OGA 0:4c7c138f3891 247
OGA 0:4c7c138f3891 248 int main() {
OGA 0:4c7c138f3891 249 //init
OGA 0:4c7c138f3891 250
OGA 0:4c7c138f3891 251 /*device.baud(9600);
OGA 0:4c7c138f3891 252 device.printf("START");
OGA 0:4c7c138f3891 253 device.attach(&dev_tx,Serial::TxIrq);*/
OGA 0:4c7c138f3891 254
OGA 0:4c7c138f3891 255 //rivisedate();
OGA 0:4c7c138f3891 256 wait(3);
OGA 0:4c7c138f3891 257
OGA 0:4c7c138f3891 258 interrupt0.attach(&tic_sensor, 0.1/*sec*/);//0.04sec以上じゃないとmain動かない?
OGA 0:4c7c138f3891 259 //sw1.mode(PullUp);
OGA 0:4c7c138f3891 260 //init end
OGA 0:4c7c138f3891 261
OGA 0:4c7c138f3891 262 while(1) {
OGA 0:4c7c138f3891 263 SendData0[1] = jumI;
OGA 0:4c7c138f3891 264
OGA 0:4c7c138f3891 265 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);
OGA 0:4c7c138f3891 266
OGA 0:4c7c138f3891 267 //jumpAction(jumI);
OGA 0:4c7c138f3891 268 }
OGA 0:4c7c138f3891 269 }