driver
Dependencies: HMC6352 PID mbed
Fork of ver3_1_0 by
main.h@1:3b61675573b1, 2014-01-24 (annotated)
- Committer:
- yusuke_robocup
- Date:
- Fri Jan 24 06:27:03 2014 +0000
- Revision:
- 1:3b61675573b1
- Parent:
- 0:bde8ed56b164
driver
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
akudohune | 0:bde8ed56b164 | 1 | |
akudohune | 0:bde8ed56b164 | 2 | |
akudohune | 0:bde8ed56b164 | 3 | #define RATE 0.052 |
akudohune | 0:bde8ed56b164 | 4 | #define Long 1.0 |
akudohune | 0:bde8ed56b164 | 5 | #define ENTER 0 |
akudohune | 0:bde8ed56b164 | 6 | #define EXIT 1 |
akudohune | 0:bde8ed56b164 | 7 | #define X 0 |
akudohune | 0:bde8ed56b164 | 8 | #define Y 1 |
akudohune | 0:bde8ed56b164 | 9 | #define MOT_NUM 4 |
akudohune | 0:bde8ed56b164 | 10 | #define MOTDRIVER_WAIT 300 //ms |
akudohune | 0:bde8ed56b164 | 11 | #define BAUD_RATE 115200 |
akudohune | 0:bde8ed56b164 | 12 | #define BAUD_RATE2 19200 |
akudohune | 0:bde8ed56b164 | 13 | #define BUT_WAIT 0.3 //s |
akudohune | 0:bde8ed56b164 | 14 | #define ON 1 |
akudohune | 0:bde8ed56b164 | 15 | #define OFF 0 |
akudohune | 0:bde8ed56b164 | 16 | |
akudohune | 0:bde8ed56b164 | 17 | #define PING_ERROR 0xFFFF |
akudohune | 0:bde8ed56b164 | 18 | #define PI 3.14159265 |
akudohune | 0:bde8ed56b164 | 19 | |
akudohune | 0:bde8ed56b164 | 20 | #define MOT1 1.0 |
akudohune | 0:bde8ed56b164 | 21 | #define MOT2 1.0 |
akudohune | 0:bde8ed56b164 | 22 | #define MOT3 1.0 |
akudohune | 0:bde8ed56b164 | 23 | #define MOT4 1.0 |
akudohune | 0:bde8ed56b164 | 24 | |
akudohune | 0:bde8ed56b164 | 25 | #define PID_BIAS 0.0 |
akudohune | 0:bde8ed56b164 | 26 | #define REFERENCE 180.0 |
akudohune | 0:bde8ed56b164 | 27 | #define MINIMUM 0.0 |
akudohune | 0:bde8ed56b164 | 28 | #define MAXIMUM 360.0 |
akudohune | 0:bde8ed56b164 | 29 | |
akudohune | 0:bde8ed56b164 | 30 | #define PID_CYCLE 0.06 //s |
akudohune | 0:bde8ed56b164 | 31 | |
yusuke_robocup | 1:3b61675573b1 | 32 | #define P_GAIN 0.65 //0.78 |
akudohune | 0:bde8ed56b164 | 33 | #define I_GAIN 0.0 //0.0 |
yusuke_robocup | 1:3b61675573b1 | 34 | #define D_GAIN 0.009 //0.009 |
akudohune | 0:bde8ed56b164 | 35 | |
akudohune | 0:bde8ed56b164 | 36 | #define OUT_LIMIT 30.0 |
akudohune | 0:bde8ed56b164 | 37 | #define MAX_POW 100 |
akudohune | 0:bde8ed56b164 | 38 | #define MIN_POW -100 |
akudohune | 0:bde8ed56b164 | 39 | |
akudohune | 0:bde8ed56b164 | 40 | DigitalOut led1(LED1); |
akudohune | 0:bde8ed56b164 | 41 | DigitalOut led2(LED2); |
akudohune | 0:bde8ed56b164 | 42 | DigitalOut led3(LED3); |
akudohune | 0:bde8ed56b164 | 43 | DigitalOut led4(LED4); |
akudohune | 0:bde8ed56b164 | 44 | BusOut mbedleds(LED4,LED3,LED2,LED1); |
akudohune | 0:bde8ed56b164 | 45 | HMC6352 compass(p9, p10); |
akudohune | 0:bde8ed56b164 | 46 | Serial driver(p28, p27); // tx, rx |
akudohune | 0:bde8ed56b164 | 47 | Serial device2(p13, p14); // tx, rx |
akudohune | 0:bde8ed56b164 | 48 | Serial pc(USBTX, USBRX); // tx, rx |
akudohune | 0:bde8ed56b164 | 49 | DigitalIn StartButton(p21); |
akudohune | 0:bde8ed56b164 | 50 | DigitalIn CalibEnterButton(p22); |
akudohune | 0:bde8ed56b164 | 51 | DigitalIn CalibExitButton(p23); |
akudohune | 0:bde8ed56b164 | 52 | DigitalIn EEPROMButton(p24); |
akudohune | 0:bde8ed56b164 | 53 | PID pid(P_GAIN,I_GAIN,D_GAIN, RATE); //battery is low power version 30.0 0.58 1.0 0.015 battery is high power version 30.0 0.42, 1.0, 0.013 power is low but perfect 20.0 0.45, 0.0, 0.010 |
akudohune | 0:bde8ed56b164 | 54 | Ticker pidUpdata; |
akudohune | 0:bde8ed56b164 | 55 | Ticker irDistanceUpdata; |
yusuke_robocup | 1:3b61675573b1 | 56 | Ticker underIR; |
akudohune | 0:bde8ed56b164 | 57 | Timer timer1; |
akudohune | 0:bde8ed56b164 | 58 | Timer Survtimer; |
yusuke_robocup | 1:3b61675573b1 | 59 | Timer lasttimer; |
akudohune | 0:bde8ed56b164 | 60 | LocalFileSystem local("local"); // マウントポイントを定義(ディレクトリパスになる) |
akudohune | 0:bde8ed56b164 | 61 | |
akudohune | 0:bde8ed56b164 | 62 | enum{ |
yusuke_robocup | 1:3b61675573b1 | 63 | XNORMAL, |
akudohune | 0:bde8ed56b164 | 64 | LEFT_OUT, |
akudohune | 0:bde8ed56b164 | 65 | RIGHT_OUT, |
yusuke_robocup | 1:3b61675573b1 | 66 | }; |
yusuke_robocup | 1:3b61675573b1 | 67 | |
yusuke_robocup | 1:3b61675573b1 | 68 | enum{ |
yusuke_robocup | 1:3b61675573b1 | 69 | YNORMAL, |
akudohune | 0:bde8ed56b164 | 70 | FRONT_OUT, |
akudohune | 0:bde8ed56b164 | 71 | BACK_OUT, |
akudohune | 0:bde8ed56b164 | 72 | }; |
akudohune | 0:bde8ed56b164 | 73 | |
akudohune | 0:bde8ed56b164 | 74 | enum{ |
akudohune | 0:bde8ed56b164 | 75 | HOME_WAIT, |
akudohune | 0:bde8ed56b164 | 76 | DIFFENCE, |
akudohune | 0:bde8ed56b164 | 77 | WARNING, |
akudohune | 0:bde8ed56b164 | 78 | HOLD, |
akudohune | 0:bde8ed56b164 | 79 | }; |
akudohune | 0:bde8ed56b164 | 80 | |
yusuke_robocup | 1:3b61675573b1 | 81 | enum{ |
yusuke_robocup | 1:3b61675573b1 | 82 | NONE, |
yusuke_robocup | 1:3b61675573b1 | 83 | ATTACK, |
yusuke_robocup | 1:3b61675573b1 | 84 | SNAKE, |
yusuke_robocup | 1:3b61675573b1 | 85 | SEARCH |
yusuke_robocup | 1:3b61675573b1 | 86 | }; |
yusuke_robocup | 1:3b61675573b1 | 87 | |
akudohune | 0:bde8ed56b164 | 88 | PinName adc_num[6] = { |
akudohune | 0:bde8ed56b164 | 89 | p15, |
akudohune | 0:bde8ed56b164 | 90 | p16, |
akudohune | 0:bde8ed56b164 | 91 | p17, |
akudohune | 0:bde8ed56b164 | 92 | p18, |
akudohune | 0:bde8ed56b164 | 93 | p19, |
akudohune | 0:bde8ed56b164 | 94 | p20, |
akudohune | 0:bde8ed56b164 | 95 | }; |
yusuke_robocup | 1:3b61675573b1 | 96 | |
yusuke_robocup | 1:3b61675573b1 | 97 | int under_val[6] = {0}; |
yusuke_robocup | 1:3b61675573b1 | 98 | |
yusuke_robocup | 1:3b61675573b1 | 99 | int whiteout_flag = 0; |
yusuke_robocup | 1:3b61675573b1 | 100 | |
akudohune | 0:bde8ed56b164 | 101 | double standTu = 0; |
akudohune | 0:bde8ed56b164 | 102 | int speed[MOT_NUM] = {0}; |
akudohune | 0:bde8ed56b164 | 103 | uint8_t hold_flag = 0; |
akudohune | 0:bde8ed56b164 | 104 | uint8_t state = HOME_WAIT; |
yusuke_robocup | 1:3b61675573b1 | 105 | uint8_t lineStateY = YNORMAL; |
yusuke_robocup | 1:3b61675573b1 | 106 | uint8_t lineStateX = XNORMAL; |
akudohune | 0:bde8ed56b164 | 107 | |
akudohune | 0:bde8ed56b164 | 108 | double inputPID = 180.0; |
akudohune | 0:bde8ed56b164 | 109 | static double standard; |
akudohune | 0:bde8ed56b164 | 110 | double compassPID = 0.0; |
akudohune | 0:bde8ed56b164 | 111 | |
yusuke_robocup | 1:3b61675573b1 | 112 | int whiteout; |
yusuke_robocup | 1:3b61675573b1 | 113 | int whitevalue; |
yusuke_robocup | 1:3b61675573b1 | 114 | |
akudohune | 0:bde8ed56b164 | 115 | extern int diff; |
akudohune | 0:bde8ed56b164 | 116 | |
akudohune | 0:bde8ed56b164 | 117 | extern string StringFIN; |
akudohune | 0:bde8ed56b164 | 118 | |
akudohune | 0:bde8ed56b164 | 119 | extern uint8_t direction; |
akudohune | 0:bde8ed56b164 | 120 | extern uint8_t Distance; |
akudohune | 0:bde8ed56b164 | 121 | extern uint8_t IR_found; |
akudohune | 0:bde8ed56b164 | 122 | extern uint8_t xbee; |
akudohune | 0:bde8ed56b164 | 123 | extern int irDistance[6]; |
akudohune | 0:bde8ed56b164 | 124 | |
akudohune | 0:bde8ed56b164 | 125 | extern void array(int,int,int,int); |
akudohune | 0:bde8ed56b164 | 126 | |
akudohune | 0:bde8ed56b164 | 127 | extern void dev_rx(void); |
akudohune | 0:bde8ed56b164 | 128 | extern void dev_tx(void); |
akudohune | 0:bde8ed56b164 | 129 | |
akudohune | 0:bde8ed56b164 | 130 | extern uint16_t ultrasonicVal[4]; |
akudohune | 0:bde8ed56b164 | 131 | |
akudohune | 0:bde8ed56b164 | 132 | int stand[6]; |
akudohune | 0:bde8ed56b164 | 133 | uint8_t compFlag = 0; |
akudohune | 0:bde8ed56b164 | 134 | |
akudohune | 0:bde8ed56b164 | 135 | #define FRONT_SONIC ultrasonicVal[0] |
akudohune | 0:bde8ed56b164 | 136 | #define BACK_SONIC ultrasonicVal[2] |
akudohune | 0:bde8ed56b164 | 137 | #define RIGHT_SONIC ultrasonicVal[1] |
akudohune | 0:bde8ed56b164 | 138 | #define LEFT_SONIC ultrasonicVal[3] |
akudohune | 0:bde8ed56b164 | 139 | |
akudohune | 0:bde8ed56b164 | 140 | |
yusuke_robocup | 1:3b61675573b1 | 141 | double ball_sankaku[16][2] = { |
yusuke_robocup | 1:3b61675573b1 | 142 | {0 , 1 }, |
yusuke_robocup | 1:3b61675573b1 | 143 | {0.390 , 0.920}, |
yusuke_robocup | 1:3b61675573b1 | 144 | {0.866 , 0.500}, |
yusuke_robocup | 1:3b61675573b1 | 145 | {0.927 , 0.374}, |
yusuke_robocup | 1:3b61675573b1 | 146 | {1 , 0 }, |
yusuke_robocup | 1:3b61675573b1 | 147 | {0.920 ,-0.390}, |
yusuke_robocup | 1:3b61675573b1 | 148 | {0.707 ,-0.707}, |
yusuke_robocup | 1:3b61675573b1 | 149 | {0.374 ,-0.927}, |
yusuke_robocup | 1:3b61675573b1 | 150 | {0 ,-1 }, |
yusuke_robocup | 1:3b61675573b1 | 151 | {-0.390,-0.920}, |
yusuke_robocup | 1:3b61675573b1 | 152 | {-0.707,-0.707}, |
yusuke_robocup | 1:3b61675573b1 | 153 | {-0.927,-0.374}, |
yusuke_robocup | 1:3b61675573b1 | 154 | {-1 , 0 }, |
yusuke_robocup | 1:3b61675573b1 | 155 | {-0.920, 0.390}, |
yusuke_robocup | 1:3b61675573b1 | 156 | {-0.866, 0.500}, |
yusuke_robocup | 1:3b61675573b1 | 157 | {-0.374, 0.927} |
yusuke_robocup | 1:3b61675573b1 | 158 | }; |
yusuke_robocup | 1:3b61675573b1 | 159 | |
yusuke_robocup | 1:3b61675573b1 | 160 | double turn_sankaku[16][2] = { |
yusuke_robocup | 1:3b61675573b1 | 161 | { 0 ,0 }, |
yusuke_robocup | 1:3b61675573b1 | 162 | /*{ 1 ,0 },*/{ 0.920,-0.390 }, |
yusuke_robocup | 1:3b61675573b1 | 163 | { 0.707,-0.707 }, //{ 0.500,-0.866 }, |
yusuke_robocup | 1:3b61675573b1 | 164 | { 0.374,-0.927 }, |
yusuke_robocup | 1:3b61675573b1 | 165 | { 0 ,-0.8 }, |
yusuke_robocup | 1:3b61675573b1 | 166 | {-0.390,-0.920 }, |
yusuke_robocup | 1:3b61675573b1 | 167 | {-0.707,-0.707 }, |
yusuke_robocup | 1:3b61675573b1 | 168 | {-0.927,-0.374 }, |
yusuke_robocup | 1:3b61675573b1 | 169 | {-0.927,-0.374 }, |
yusuke_robocup | 1:3b61675573b1 | 170 | {0.920 ,-0.390 }, |
yusuke_robocup | 1:3b61675573b1 | 171 | {0.707 ,-0.707 }, |
yusuke_robocup | 1:3b61675573b1 | 172 | {0.374 ,-0.927 }, |
yusuke_robocup | 1:3b61675573b1 | 173 | {0 ,-0.8 }, |
yusuke_robocup | 1:3b61675573b1 | 174 | {-0.390,-0.920 }, |
yusuke_robocup | 1:3b61675573b1 | 175 | {-0.707,-0.707 }, //{-0.500,-0.866 }, |
yusuke_robocup | 1:3b61675573b1 | 176 | /*{-1 ,0 }*/{-0.927,-0.374 } |
yusuke_robocup | 1:3b61675573b1 | 177 | }; |
yusuke_robocup | 1:3b61675573b1 | 178 | |
yusuke_robocup | 1:3b61675573b1 | 179 | |
yusuke_robocup | 1:3b61675573b1 | 180 | |
yusuke_robocup | 1:3b61675573b1 | 181 |