oooga
Dependencies: HMC6352 Servo TextLCD mbed
Diff: main.cpp
- Revision:
- 0:e0e1b495278b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Sep 11 05:21:06 2013 +0000 @@ -0,0 +1,177 @@ +//歩くだけ +#include "mbed.h" +#include "ColorSensor.h" +#include "TextLCD.h" +#include "Servo.h" +#include "HMC6352.h" + +#include "main.h" + +//センサの数 +#define COLOR_NUM 3 + +//閾値 +#define R_THR 60 +#define G_THR 60 +#define B_THR 60 + + +//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); + +//ColorSensor color = ColorSensor(); +//Serial pc(USBTX, USBRX); // tx, rx + +DigitalOut led[4] = {LED1,LED2,LED3,LED4}; +DigitalOut air[2] = {p21,p22}; + +Timer color_t[3]; + + +double proportional = 0; +uint16_t com_val = 0; + + +#define Convert_dekaruto(a) ((a+100.0)/2.0/100.0) + +#define STRAIGHT 0.6; +#define SPIN 0.4; + +//#define STRAIGHT 0.0; +//#define SPIN 1.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; +} + + + +//青から赤に反応したらジャンプ +//pidUpdataのせいでカラーセンサが動かない!! +int main() +{ + wait(3); + + unsigned R, G, B; + unsigned threshold, t[3] = {0} ; + double color_sum; + unsigned redp[COLOR_NUM], greenp[COLOR_NUM], bluep[COLOR_NUM]; + //pc.baud(115200); + air[0] = 0; air[1] = 1; + + int vl; + double vs; + compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); + pidUpdata.attach(&PidUpdate, PID_CYCLE); + + + while(1) + { + + color0.checkRGB(redp[0],greenp[0],bluep[0]); + color1.checkRGB(redp[1],greenp[1],bluep[1]); + color2.checkRGB(redp[2],greenp[2],bluep[2]); + + //lcd.cls(); + //pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]); + /* + 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]); + */ + + + + + threshold = 0; + for(int i=0; i<3; 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++; + } + } + + + + if(threshold >= 1){ + led[0] = 1; led[1] = 1; led[2] = 1; led[3] = 1; + air[0] = 1; air[1] = 0; + wait(0.8); + led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0; + air[0] = 0; air[1] = 1; + wait(0.5); + }else{ + led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0; + } + + + + vl = -90; + //vl = 0; + + vl = vl * STRAIGHT ; + vs = vsOut() * SPIN ; + + vl *= 0.5; + vs *= 0.3; + + move(vl,(int)vs); + } +} \ No newline at end of file