Ryo Ogata
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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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;
--- /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
--- /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
--- /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
+};