jump!
Dependencies: ColorSensor1 HMC6352 Servo TextLCD mbed
Revision 0:feefc7aaa114, committed 2013-09-23
- Comitter:
- OGA
- Date:
- Mon Sep 23 06:35:28 2013 +0000
- Commit message:
- var.4.3
Changed in this revision
diff -r 000000000000 -r feefc7aaa114 ColorSensor.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ColorSensor.lib Mon Sep 23 06:35:28 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/OGA/code/ColorSensor1/#745c9de82674
diff -r 000000000000 -r feefc7aaa114 HMC6352.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HMC6352.lib Mon Sep 23 06:35:28 2013 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/aberk/code/HMC6352/#83c0cb554099
diff -r 000000000000 -r feefc7aaa114 Servo.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Mon Sep 23 06:35:28 2013 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/simon/code/Servo/#36b69a7ced07
diff -r 000000000000 -r feefc7aaa114 TextLCD.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TextLCD.lib Mon Sep 23 06:35:28 2013 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/simon/code/TextLCD/#44f34c09bd37
diff -r 000000000000 -r feefc7aaa114 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Sep 23 06:35:28 2013 +0000 @@ -0,0 +1,265 @@ +#include "mbed.h" +#include "ColorSensor.h" +#include "TextLCD.h" +#include "Servo.h" +#include "HMC6352.h" + +#include "main.h" + + +void tic_sensor() +{ + Ultrasonic(); + + colorUpdate(); + + /*lcd.cls(); + lcd.locate(0,0); + lcd.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]); + */ +} + +////////////////////////////////////////移動関数////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////////////////////////////// +void move(int vl,int vs){ + double fut_R,fut_L,true_vs; + + true_vs = abs(vs)/SPIN; + + if(true_vs > 40){ + vl = 0; + vs = 100*(vs/abs(vs)); + } + + fut_R = Convert_dekaruto((vl + vs)); + fut_L = Convert_dekaruto((vl - vs)/**1.4*/); + + fut_R = Convert_dekaruto(-50); + fut_L = Convert_dekaruto(-50); + + servoR = fut_R; + servoL = fut_L; + + //printf("R:%lf L:%lf\n",fut_R,fut_L); + //printf("R:%d L:%d\n",(vl + vs),-(vl - vs)); +} + +/*void PidUpdate() +{ + inputPID = (((int)(compass.sample() - ((207.0) * 10.0) + 5400.0) % 3600) / 10.0); + //printf("%lf\n",inputPID); +}*/ + +double vsOut(){ + double vs; + vs = ((inputPID / 360 - 0.5) * 2 * 100) * -1; + vs = vs * 8; + + if(vs/abs(vs) < 0){ + //vs *= 1.3; + } + + if(abs(vs) > 90)vs = 90*(vs/abs(vs)); + if(abs(vs) < 25) vs = 25*(vs/abs(vs)); + + return vs; +} + +////////////////////////////////////////超音波センサの//////////////////////////////////////// +////////////////////////////////////////スイッチ的な関数////////////////////////////////////// +int ping_button(int ping,int button){ + static int continue_flag = 0; + static int change_flag = 0; + + if(continue_flag == 0){ + if(ping <= PINR_THR){ + ping_t.start(); + continue_flag = 1; + } + } + + if(continue_flag == 1){ + //agatterutoki + if(ping <= PINR_THR){ + if(change_flag == 0){ + if(ping_t.read_ms() >= 300){ + button = !button; + change_flag = 1; + } + } + } + //tatisagari + if(ping >= (PINR_THR+200)){ + ping_t.stop(); + ping_t.reset(); + continue_flag = 0; + change_flag = 0; + } + } + return button; +} + +////////////////////////////////////////カラーセンサの//////////////////////////////////////// +////////////////////////////////////////補正プログラム//////////////////////////////////////// +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() +{ + double colorSum[COLOR_NUM]; + unsigned R[COLOR_NUM], G[COLOR_NUM], B[COLOR_NUM]; + + 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]; + } +} + +////////////////////////////////////////ジャンププログラム//////////////////////////////////// +/////////////////////////////////////////////////////////////////////////////////////////// +uint8_t jumpcondition() +{ + uint8_t threshold = 0, t[3] = {0}; + + //青から赤に0.5秒以内に反応したらジャンプ + for(int i=0; i<COLOR_NUM; i++){ + if(bluep[i] >= B_THR){ + color_t[i].reset(); + color_t[i].start(); + t[i] = 0; + }else if(redp[i] >= R_THR){ + t[i] = color_t[i].read_ms(); + }else{ + t[i] = 0; + } + + if((t[i] <= 500) && (t[i] != 0)){ + threshold++; + } + } + + return threshold; +} + +void jumping(uint8_t threshold) +{ + //超音波でジャンプのタイミング合わせる + if(threshold >= 1){ + jump_t.reset(); + jump_t.start(); + while(ultrasonicVal[0] < 1700){ + led[0] = 1; led[1] = 1; led[2] = 0; led[3] = 0; + air[0] = 1; air[1] = 0; + + if(jump_t.read_ms() > 1000)break; + } + led[0] = 0; led[1] = 0; led[2] = 1; led[3] = 1; + air[0] = 0; air[1] = 1; + wait(0.5); + }else{ + led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0; + } +} + + + + + + + + + + + +int main() +{ + rivisedate(); + + timer2.start(); + ping_t.start(); + + + //unsigned R, G, B; + int vl; + double vs; + uint8_t button, state=0; + + //pc.baud(115200); + air[0] = 0; air[1] = 1; + + + //compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); + //pidUpdata.attach(&PidUpdate, PID_CYCLE); + interrupt0.attach(&tic_sensor, 0.05/*sec*/);//0.04sec以上じゃないとmain動かない + + while(1) + { + + + + pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]); + /* + lcd.cls(); + lcd.locate(0,0); + lcd.printf("R:%d G:%d B:%d", redp[0][0], greenp[0][0], bluep[0][0]); + lcd.locate(0,1); + lcd.printf("R:%d G:%d B:%d", redp[1][0], greenp[1][0], bluep[1][0]); + */ + //pc.printf("%d\n", ultrasonicVal[0]); + + + jumping(jumpcondition()); + + + + button = ping_button(ultrasonicVal[1],button); + + if(button){ + state = GO; + }else{ + state = STOP; + } + + + + + + if(state == GO){ + vl = -90;//led[0] = 1; led[1] = 1; + }else if(state == STOP){ + vl = 0;//led[0] = 0; led[1] = 0; + } + + vl = vl * STRAIGHT ; + vs = vsOut() * SPIN ; + + vl *= 0.5; + vs *= 0.3; + + move(vl,(int)vs); + } +} \ No newline at end of file
diff -r 000000000000 -r feefc7aaa114 main.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.h Mon Sep 23 06:35:28 2013 +0000 @@ -0,0 +1,63 @@ +#include "mbed.h" + + +extern double ultrasonicValue[4]; +extern uint16_t ultrasonicVal[4]; +extern void Ultrasonic(void); + + + +//センサの数 +#define COLOR_NUM 3 + +//閾値 +#define R_THR 65 +#define G_THR 65 +#define B_THR 65 +#define PINR_THR 1000 + +#define PID_CYCLE 0.06 //s +//#define PID_CYCLE 0.1 //s +#define Convert_dekaruto(a) ((a+100.0)/2.0/100.0) +#define STRAIGHT 0.6; +#define SPIN 0.4; + +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); +Servo servoR(p23); +Servo servoL(p24); +//HMC6352 compass(p28, p27); +Serial pc(USBTX, USBRX); // tx, rx +DigitalOut led[4] = {LED1,LED2,LED3,LED4}; +DigitalOut air[2] = {p21,p22}; + +//DigitalIn sw(p7); + +Timer timer1; +Timer timer2; +Timer color_t[3]; +Timer ping_t; +Timer jump_t; +Ticker interrupt0; +Ticker pidUpdata; + + +void rivisedate (); +void colorUpdate (); +uint8_t ptm(unsigned sum); + + +double proportional = 0; +uint16_t com_val = 0; +unsigned redp[COLOR_NUM], greenp[COLOR_NUM], bluep[COLOR_NUM]; +double rir,rib ; + + +double inputPID = 180.0;
diff -r 000000000000 -r feefc7aaa114 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Sep 23 06:35:28 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/a9913a65894f \ No newline at end of file
diff -r 000000000000 -r feefc7aaa114 ping/ping.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ping/ping.cpp Mon Sep 23 06:35:28 2013 +0000 @@ -0,0 +1,49 @@ +#include "mbed.h" +#include "ping.h" + +//DigitalOut myled = LED1; + +extern Timer timer2; + +uint16_t ultrasonicVal[ALL_ULTRASONIC]; +double ultrasonicValue[ALL_ULTRASONIC] = {0}; + + +void Ultrasonic() +{ + for(int i = 0 ; i < ALL_ULTRASONIC; i++){ + + uint8_t flag = 0; + + DigitalOut PingPinOut(ultrasonic_pin[i]); + PingPinOut = 1; + wait_us(10); + PingPinOut = 0; + DigitalIn PingPin(ultrasonic_pin[i]); + timer2.reset(); + while(PingPin == 0){ + if(timer2.read_us() > 1500){ //1.5ms以上応答なし + ultrasonicValue[i] = PING_ERR; + flag = 1; + break; + } + } + + timer2.reset(); + while(PingPin == 1){ + if((timer2.read_us() > 18500) || (flag == 1)){ //18.5ms以上のパルス + ultrasonicValue[i] = PING_ERR; + flag = 1; + break; + } + } + + if(flag == 0){ + ultrasonicValue[i] = timer2.read_us() / 1000000.0 / 2.0 * 340.0 * 1000.0; //mm MAX:3145 + ultrasonicVal[i] = (int)(ultrasonicValue[i] * 10.0); + }else{ + ultrasonicVal[i] = PING_ERR; + } + + } +} \ No newline at end of file
diff -r 000000000000 -r feefc7aaa114 ping/ping.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ping/ping.h Mon Sep 23 06:35:28 2013 +0000 @@ -0,0 +1,8 @@ +#define ALL_ULTRASONIC 2 +#define PING_ERR 0xFFFF + + +PinName ultrasonic_pin[ALL_ULTRASONIC] = { + p5, + p6 +};