yusuke takahashi / Mbed 2 deprecated ver3_1_0

driver

Dependencies:   HMC6352 PID mbed

Fork of ver3_1_0 by ryo seki

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?

UserRevisionLine numberNew 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