jump!
Dependencies: ColorSensor1 HMC6352 Servo TextLCD mbed
main.cpp
- Committer:
- OGA
- Date:
- 2013-09-23
- Revision:
- 0:feefc7aaa114
File content as of revision 0:feefc7aaa114:
#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); } }