Ryo Ogata / Mbed 2 deprecated jumpROBO6_3

10/22

Dependencies:   ColorSensor1 Servo mbed

main.cpp@0:fa26bf0aad5e, 2013-10-22 (annotated)

Committer:
OGA
Date:
Tue Oct 22 07:19:27 2013 +0000
Revision:
0:fa26bf0aad5e
10/22;

Who changed what in which revision?

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