10/22
Dependencies: ColorSensor1 Servo mbed
Revision 0:fa26bf0aad5e, committed 2013-10-22
- Comitter:
- OGA
- Date:
- Tue Oct 22 07:19:27 2013 +0000
- Commit message:
- 10/22;
Changed in this revision
diff -r 000000000000 -r fa26bf0aad5e ColorSensor.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ColorSensor.lib Tue Oct 22 07:19:27 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/OGA/code/ColorSensor1/#745c9de82674
diff -r 000000000000 -r fa26bf0aad5e Servo.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Tue Oct 22 07:19:27 2013 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/simon/code/Servo/#36b69a7ced07
diff -r 000000000000 -r fa26bf0aad5e main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Oct 22 07:19:27 2013 +0000 @@ -0,0 +1,244 @@ +//カラーセンサ6つ同時に動かすプログラム +//大会用ロボ +//ジャンプのプログラムも入ってる +//ジャンプ命令のプログラムを変更 +//6_2の使わないところを消したプログラム +//2回連続ジャンプシーケンス予定 +//5回連続ジャンプシーケンス搭載予定 +//10/21使用 +#include "mbed.h" +#include "ColorSensor.h" +#include "Servo.h" + +#include "main.h" + + +void tic_sensor() +{ + colorUpdate(1); + + + for(int i=0; i<COLOR_NUM; i++){ + cAve[i] = moving_ave(i, bluep[i]); + } + + roboF = robotFront(); + jumI = jumpInstruction(roboF); + +} + +////////////////////////////////////////カラーセンサの//////////////////////////////////////// +////////////////////////////////////////更新と補正プログラム/////////////////////////////////// +/*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]); + + 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]; + } +} + + +uint16_t moving_ave(uint8_t num, uint16_t data)//移動平均的な +{ + uint8_t threshold = 0; + static uint16_t tmp[COLOR_NUM][10] = {{0}}; + //static uint32_t sum[COLOR_NUM] = {0}; + + //sum[num] -= tmp[num][9]; + //sum[num] += data; + + for(uint8_t i=9; i>=1; i--){ + tmp[num][i] = tmp[num][i-1]; + } + tmp[num][0] = data; + + for(uint8_t i=0; i<=9; i++){ + if(tmp[num][i] >= B_THR){ + threshold++; + } + } + + return threshold; +} + +////////////////////////////////////////ロボの状態/////////////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////////////// +uint8_t robotFront()//縄回しロボの正面にいるか +{ + uint8_t threshold = 0; + + for(int i=0; i<COLOR_NUM; i++){ + if(cAve[i] >= 8) threshold++; + } + + if(threshold >= 1){ + return 1; + }else{ + return 0; + } +} + +uint8_t jumpInstruction(uint8_t front)//ジャンプ命令 +{ + uint8_t threshold = 0; + + for(int i=0; i<COLOR_NUM; i++){ + if((cAve[i] >= 8) && (redp[i] >= R_THR)) threshold++; + } + + if(threshold >= 1){ + return 1; + }else{ + return 0; + } +} + + + +////////////////////////////////////////ジャンププログラム//////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////////////// +void jumpAction(uint8_t threshold) +{ + static uint8_t swFlag=0, jumpCount=0, jumpwait = 0; + + + if((sw1 == 0) && (swFlag == 0)){//フォトインタラプタがオン + swFlag = 1; + jumpCount++; + }else if(sw1 == 1){//フォトインタラプタがオフ + swFlag = 0; + } + + if(threshold){ + led[0] = 1; led[1] = 1; led[2] = 1; led[3] = 1; + myservo1 = 0; + + wait(0.4); + } + + if(jumpCount == 1){ + /*if(jumpwait == 0){ + myservo1 = 0.5; + wait(1); + jumpwait = 1; + }*/ + myservo1 = 0.3; led[0] = 1;led[1] = 1; led[2] = 1; led[3] = 0; + }else if(jumpCount >= 2){ + myservo1 = 0.5; led[0] = 1;led[1] = 1; led[2] = 0; led[3] = 0; + jumpCount = 0; + jumpwait = 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); + + interrupt0.attach(&tic_sensor, 0.1/*sec*/);//0.04sec以上じゃないとmain動かない? + //sw1.mode(PullUp); + //init end + + myservo1 = 0.1; + wait(0.5); + + while(1) { + //SendData0[1] = jumI; + + 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); + + + jumpAction(jumI); + } +}
diff -r 000000000000 -r fa26bf0aad5e main.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.h Tue Oct 22 07:19:27 2013 +0000 @@ -0,0 +1,64 @@ +///////////////////////////////////////OGATA////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////// +//センサの数 +#define COLOR_NUM 6 + +//閾値 +#define R_THR 65 +#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); +//ColorSensor color4(p26, p23, p24, p25, 10); +//ColorSensor color5(p30, p27, p28, p29, 10); + +Serial pc(USBTX, USBRX); // tx, rx +DigitalOut led[4] = {LED1,LED2,LED3,LED4}; +//DigitalOut air[2] = {p7,p8}; + + +//Servo myservo1(p21); +//DigitalIn sw1(p22); + + + +Timer color_t[COLOR_NUM]; +Timer jump_t; +Ticker interrupt0; + + +void rivisedate (); +void colorUpdate (uint8_t mode); +uint16_t moving_ave(uint8_t num, uint16_t data); +uint8_t robotFront(); +uint8_t jumpInstruction(uint8_t front); +void jumpAction(uint8_t threshold); + + +double proportional = 0; +uint16_t com_val = 0; +unsigned redp[COLOR_NUM], greenp[COLOR_NUM], bluep[COLOR_NUM]; +double rir,rib ; +int cAve[COLOR_NUM] = {0}, roboF, jumI; + + +enum{ + WAIT, + STRAIGHT, + TURN, + COMP +}; \ No newline at end of file
diff -r 000000000000 -r fa26bf0aad5e mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Oct 22 07:19:27 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/a9913a65894f \ No newline at end of file