jump!
Dependencies: ColorSensor1 HMC6352 Servo TextLCD mbed
main.h@0:feefc7aaa114, 2013-09-23 (annotated)
- Committer:
- OGA
- Date:
- Mon Sep 23 06:35:28 2013 +0000
- Revision:
- 0:feefc7aaa114
var.4.3
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
OGA | 0:feefc7aaa114 | 1 | #include "mbed.h" |
OGA | 0:feefc7aaa114 | 2 | |
OGA | 0:feefc7aaa114 | 3 | |
OGA | 0:feefc7aaa114 | 4 | extern double ultrasonicValue[4]; |
OGA | 0:feefc7aaa114 | 5 | extern uint16_t ultrasonicVal[4]; |
OGA | 0:feefc7aaa114 | 6 | extern void Ultrasonic(void); |
OGA | 0:feefc7aaa114 | 7 | |
OGA | 0:feefc7aaa114 | 8 | |
OGA | 0:feefc7aaa114 | 9 | |
OGA | 0:feefc7aaa114 | 10 | //センサの数 |
OGA | 0:feefc7aaa114 | 11 | #define COLOR_NUM 3 |
OGA | 0:feefc7aaa114 | 12 | |
OGA | 0:feefc7aaa114 | 13 | //閾値 |
OGA | 0:feefc7aaa114 | 14 | #define R_THR 65 |
OGA | 0:feefc7aaa114 | 15 | #define G_THR 65 |
OGA | 0:feefc7aaa114 | 16 | #define B_THR 65 |
OGA | 0:feefc7aaa114 | 17 | #define PINR_THR 1000 |
OGA | 0:feefc7aaa114 | 18 | |
OGA | 0:feefc7aaa114 | 19 | #define PID_CYCLE 0.06 //s |
OGA | 0:feefc7aaa114 | 20 | //#define PID_CYCLE 0.1 //s |
OGA | 0:feefc7aaa114 | 21 | #define Convert_dekaruto(a) ((a+100.0)/2.0/100.0) |
OGA | 0:feefc7aaa114 | 22 | #define STRAIGHT 0.6; |
OGA | 0:feefc7aaa114 | 23 | #define SPIN 0.4; |
OGA | 0:feefc7aaa114 | 24 | |
OGA | 0:feefc7aaa114 | 25 | enum{ |
OGA | 0:feefc7aaa114 | 26 | GO, |
OGA | 0:feefc7aaa114 | 27 | STOP |
OGA | 0:feefc7aaa114 | 28 | }; |
OGA | 0:feefc7aaa114 | 29 | |
OGA | 0:feefc7aaa114 | 30 | TextLCD lcd(p30, p29, p28, p27, p26, p25, TextLCD::LCD20x4); // rs, e, d4-d7 |
OGA | 0:feefc7aaa114 | 31 | ColorSensor color0(p20, p17, p18, p19, 10); |
OGA | 0:feefc7aaa114 | 32 | ColorSensor color1(p16, p13, p14, p15, 10); |
OGA | 0:feefc7aaa114 | 33 | ColorSensor color2(p12, p9, p10, p11, 10); |
OGA | 0:feefc7aaa114 | 34 | Servo servoR(p23); |
OGA | 0:feefc7aaa114 | 35 | Servo servoL(p24); |
OGA | 0:feefc7aaa114 | 36 | //HMC6352 compass(p28, p27); |
OGA | 0:feefc7aaa114 | 37 | Serial pc(USBTX, USBRX); // tx, rx |
OGA | 0:feefc7aaa114 | 38 | DigitalOut led[4] = {LED1,LED2,LED3,LED4}; |
OGA | 0:feefc7aaa114 | 39 | DigitalOut air[2] = {p21,p22}; |
OGA | 0:feefc7aaa114 | 40 | |
OGA | 0:feefc7aaa114 | 41 | //DigitalIn sw(p7); |
OGA | 0:feefc7aaa114 | 42 | |
OGA | 0:feefc7aaa114 | 43 | Timer timer1; |
OGA | 0:feefc7aaa114 | 44 | Timer timer2; |
OGA | 0:feefc7aaa114 | 45 | Timer color_t[3]; |
OGA | 0:feefc7aaa114 | 46 | Timer ping_t; |
OGA | 0:feefc7aaa114 | 47 | Timer jump_t; |
OGA | 0:feefc7aaa114 | 48 | Ticker interrupt0; |
OGA | 0:feefc7aaa114 | 49 | Ticker pidUpdata; |
OGA | 0:feefc7aaa114 | 50 | |
OGA | 0:feefc7aaa114 | 51 | |
OGA | 0:feefc7aaa114 | 52 | void rivisedate (); |
OGA | 0:feefc7aaa114 | 53 | void colorUpdate (); |
OGA | 0:feefc7aaa114 | 54 | uint8_t ptm(unsigned sum); |
OGA | 0:feefc7aaa114 | 55 | |
OGA | 0:feefc7aaa114 | 56 | |
OGA | 0:feefc7aaa114 | 57 | double proportional = 0; |
OGA | 0:feefc7aaa114 | 58 | uint16_t com_val = 0; |
OGA | 0:feefc7aaa114 | 59 | unsigned redp[COLOR_NUM], greenp[COLOR_NUM], bluep[COLOR_NUM]; |
OGA | 0:feefc7aaa114 | 60 | double rir,rib ; |
OGA | 0:feefc7aaa114 | 61 | |
OGA | 0:feefc7aaa114 | 62 | |
OGA | 0:feefc7aaa114 | 63 | double inputPID = 180.0; |