sasasa
Dependencies: HMC6352 PID eeprom mbed
Fork of ver1_2_2_1 by
main.h@3:b4fb2b5365a7, 2013-04-18 (annotated)
- Committer:
- yusuke_robocup
- Date:
- Thu Apr 18 08:42:17 2013 +0000
- Revision:
- 3:b4fb2b5365a7
- Parent:
- 2:09fabba6c00d
new new new
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
akudohune | 0:74bf4953c0d1 | 1 | |
akudohune | 0:74bf4953c0d1 | 2 | |
akudohune | 0:74bf4953c0d1 | 3 | #define RATE 0.052 |
akudohune | 0:74bf4953c0d1 | 4 | #define Long 1.0 |
akudohune | 0:74bf4953c0d1 | 5 | #define ENTER 0 |
akudohune | 0:74bf4953c0d1 | 6 | #define EXIT 1 |
akudohune | 0:74bf4953c0d1 | 7 | #define MOT_NUM 4 |
akudohune | 0:74bf4953c0d1 | 8 | #define MOTDRIVER_WAIT 300 //ms |
akudohune | 0:74bf4953c0d1 | 9 | #define BAUD_RATE 115200 |
akudohune | 0:74bf4953c0d1 | 10 | #define ON 1 |
akudohune | 0:74bf4953c0d1 | 11 | #define OFF 0 |
akudohune | 0:74bf4953c0d1 | 12 | |
yusuke_robocup | 2:09fabba6c00d | 13 | #define MOT1 1.0 //60 3.0 0.0 0.5 1.0 |
yusuke_robocup | 2:09fabba6c00d | 14 | #define MOT2 1.0 //120 0.0 3.0 1.0 0.0 |
yusuke_robocup | 2:09fabba6c00d | 15 | #define MOT3 1.0 //240 0.6 0.0 0.0 3.0 |
yusuke_robocup | 2:09fabba6c00d | 16 | #define MOT4 1.0 //300 0.0 0.8 3.0 0.0 |
akudohune | 0:74bf4953c0d1 | 17 | |
akudohune | 1:89408fff7cc9 | 18 | #define OUT_LIMIT 30.0 |
akudohune | 1:89408fff7cc9 | 19 | #define MAX_POW 100 |
akudohune | 1:89408fff7cc9 | 20 | #define MIN_POW -100 |
akudohune | 0:74bf4953c0d1 | 21 | |
yusuke_robocup | 2:09fabba6c00d | 22 | #define SHINPUKU 40 |
yusuke_robocup | 2:09fabba6c00d | 23 | |
yusuke_robocup | 2:09fabba6c00d | 24 | #define FULL 0 |
yusuke_robocup | 2:09fabba6c00d | 25 | |
yusuke_robocup | 3:b4fb2b5365a7 | 26 | #define PI 3.14159265358979 |
yusuke_robocup | 3:b4fb2b5365a7 | 27 | |
yusuke_robocup | 2:09fabba6c00d | 28 | |
yusuke_robocup | 2:09fabba6c00d | 29 | |
yusuke_robocup | 2:09fabba6c00d | 30 | |
akudohune | 0:74bf4953c0d1 | 31 | DigitalOut led1(LED1); |
akudohune | 0:74bf4953c0d1 | 32 | DigitalOut led2(LED2); |
akudohune | 0:74bf4953c0d1 | 33 | DigitalOut led3(LED3); |
akudohune | 1:89408fff7cc9 | 34 | DigitalOut led4(LED4); |
akudohune | 0:74bf4953c0d1 | 35 | HMC6352 compass(p9, p10); |
akudohune | 0:74bf4953c0d1 | 36 | Serial driver(p28, p27); // tx, rx |
akudohune | 0:74bf4953c0d1 | 37 | Serial pc(USBTX, USBRX); // tx, rx |
akudohune | 0:74bf4953c0d1 | 38 | DigitalIn StartButton(p21); |
akudohune | 0:74bf4953c0d1 | 39 | DigitalIn CalibEnterButton(p22); |
akudohune | 0:74bf4953c0d1 | 40 | DigitalIn CalibExitButton(p23); |
yusuke_robocup | 2:09fabba6c00d | 41 | DigitalIn EEPROMButton(p24); |
yusuke_robocup | 2:09fabba6c00d | 42 | PID pid(0.35, 0.0, 0.012, RATE); //30.0 0.35 1.0 0.012 30.0 0.42 1.0 0.013 |
akudohune | 0:74bf4953c0d1 | 43 | Ticker pidUpdata; |
akudohune | 0:74bf4953c0d1 | 44 | Ticker IR; |
akudohune | 0:74bf4953c0d1 | 45 | Ticker ultrasonic; |
akudohune | 0:74bf4953c0d1 | 46 | Timer timer1; |
akudohune | 0:74bf4953c0d1 | 47 | Timer timer2; |
akudohune | 0:74bf4953c0d1 | 48 | Timer timer_ir; /* 赤外線用タイマー */ |
akudohune | 0:74bf4953c0d1 | 49 | |
akudohune | 0:74bf4953c0d1 | 50 | |
akudohune | 0:74bf4953c0d1 | 51 | int speed[MOT_NUM] = {0}; |
akudohune | 1:89408fff7cc9 | 52 | |
akudohune | 0:74bf4953c0d1 | 53 | static float lastData = 0.0; |
akudohune | 0:74bf4953c0d1 | 54 | static float inputPID = 180.0; |
akudohune | 0:74bf4953c0d1 | 55 | static float standard; |
akudohune | 0:74bf4953c0d1 | 56 | static float compassPID = 0.0; |
akudohune | 0:74bf4953c0d1 | 57 | |
akudohune | 0:74bf4953c0d1 | 58 | extern string StringFIN; |
akudohune | 0:74bf4953c0d1 | 59 | |
akudohune | 0:74bf4953c0d1 | 60 | extern int direction; |
akudohune | 0:74bf4953c0d1 | 61 | extern int Distance; |
akudohune | 0:74bf4953c0d1 | 62 | extern int IR_found; |
akudohune | 0:74bf4953c0d1 | 63 | extern double ball_sankaku[16][2]; |
yusuke_robocup | 2:09fabba6c00d | 64 | extern double turn_sankaku[16][2]; |
akudohune | 1:89408fff7cc9 | 65 | extern double ultrasonicVal[3]; |
yusuke_robocup | 2:09fabba6c00d | 66 | float now_compass =20.0; |
yusuke_robocup | 2:09fabba6c00d | 67 | float past_compass; |
yusuke_robocup | 2:09fabba6c00d | 68 | int turn_flag = 0; |
akudohune | 0:74bf4953c0d1 | 69 | |
akudohune | 0:74bf4953c0d1 | 70 | extern void Ultrasonic(void); |
akudohune | 0:74bf4953c0d1 | 71 | extern void IR_Position(void); |
akudohune | 0:74bf4953c0d1 | 72 | extern void PidUpdata(void); |
akudohune | 0:74bf4953c0d1 | 73 | extern void array(int,int,int,int); |
akudohune | 0:74bf4953c0d1 | 74 | |
akudohune | 0:74bf4953c0d1 | 75 | enum{ |
yusuke_robocup | 2:09fabba6c00d | 76 | NONE, |
yusuke_robocup | 2:09fabba6c00d | 77 | SNAKE, |
yusuke_robocup | 2:09fabba6c00d | 78 | ATTACK, |
yusuke_robocup | 2:09fabba6c00d | 79 | SEARCH |
akudohune | 0:74bf4953c0d1 | 80 | }; |
akudohune | 0:74bf4953c0d1 | 81 |