yusuke takahashi / Mbed 2 deprecated walkROBO_3

aa

Dependencies:   HMC6352 QEI Servo mbed

Fork of walkROBO by Ryo Ogata

main.h@2:955cdadf5ecc, 2013-09-05 (annotated)

Committer:
yusuke_robocup
Date:
Thu Sep 05 09:57:22 2013 +0000
Revision:
2:955cdadf5ecc
Parent:
0:4644bf6bca6a 
aa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
OGA 0:4644bf6bca6a 1 #include "mbed.h"
OGA 0:4644bf6bca6a 2
OGA 0:4644bf6bca6a 3 Timer timer2;
OGA 0:4644bf6bca6a 4
OGA 0:4644bf6bca6a 5 extern double ultrasonicValue[4];
OGA 0:4644bf6bca6a 6 extern uint16_t ultrasonicVal[4];
yusuke_robocup 2:955cdadf5ecc 7 extern void Ultrasonic(void);
yusuke_robocup 2:955cdadf5ecc 8
yusuke_robocup 2:955cdadf5ecc 9 /* robocup */
yusuke_robocup 2:955cdadf5ecc 10
yusuke_robocup 2:955cdadf5ecc 11 #define RATE 0.052
yusuke_robocup 2:955cdadf5ecc 12 #define Long 1.0
yusuke_robocup 2:955cdadf5ecc 13 #define ENTER 0
yusuke_robocup 2:955cdadf5ecc 14 #define EXIT 1
yusuke_robocup 2:955cdadf5ecc 15 #define X 0
yusuke_robocup 2:955cdadf5ecc 16 #define Y 1
yusuke_robocup 2:955cdadf5ecc 17 #define MOT_NUM 4
yusuke_robocup 2:955cdadf5ecc 18 #define MOTDRIVER_WAIT 300 //ms
yusuke_robocup 2:955cdadf5ecc 19 #define BAUD_RATE 115200
yusuke_robocup 2:955cdadf5ecc 20 #define BAUD_RATE2 19200
yusuke_robocup 2:955cdadf5ecc 21 #define BUT_WAIT 0.3 //s
yusuke_robocup 2:955cdadf5ecc 22 #define ON 1
yusuke_robocup 2:955cdadf5ecc 23 #define OFF 0
yusuke_robocup 2:955cdadf5ecc 24
yusuke_robocup 2:955cdadf5ecc 25 #define PING_ERROR 0xFFFF
yusuke_robocup 2:955cdadf5ecc 26 #define PI 3.14159265
yusuke_robocup 2:955cdadf5ecc 27
yusuke_robocup 2:955cdadf5ecc 28 #define MOT1 1.0
yusuke_robocup 2:955cdadf5ecc 29 #define MOT2 1.0
yusuke_robocup 2:955cdadf5ecc 30 #define MOT3 1.0
yusuke_robocup 2:955cdadf5ecc 31 #define MOT4 1.0
yusuke_robocup 2:955cdadf5ecc 32
yusuke_robocup 2:955cdadf5ecc 33 #define PID_BIAS 0.0
yusuke_robocup 2:955cdadf5ecc 34 #define REFERENCE 180.0
yusuke_robocup 2:955cdadf5ecc 35 #define MINIMUM 0.0
yusuke_robocup 2:955cdadf5ecc 36 #define MAXIMUM 360.0
yusuke_robocup 2:955cdadf5ecc 37
yusuke_robocup 2:955cdadf5ecc 38 #define PID_CYCLE 0.06 //s
yusuke_robocup 2:955cdadf5ecc 39
yusuke_robocup 2:955cdadf5ecc 40 #define P_GAIN 0.75 //0.78
yusuke_robocup 2:955cdadf5ecc 41 #define I_GAIN 0.0 //0.0
yusuke_robocup 2:955cdadf5ecc 42 #define D_GAIN 0.006 //0.009
yusuke_robocup 2:955cdadf5ecc 43
yusuke_robocup 2:955cdadf5ecc 44 #define OUT_LIMIT 30.0
yusuke_robocup 2:955cdadf5ecc 45 #define MAX_POW 100
yusuke_robocup 2:955cdadf5ecc 46 #define MIN_POW -100
yusuke_robocup 2:955cdadf5ecc 47
yusuke_robocup 2:955cdadf5ecc 48 DigitalOut led1(LED1);
yusuke_robocup 2:955cdadf5ecc 49 DigitalOut led2(LED2);
yusuke_robocup 2:955cdadf5ecc 50 DigitalOut led3(LED3);
yusuke_robocup 2:955cdadf5ecc 51 DigitalOut led4(LED4);
yusuke_robocup 2:955cdadf5ecc 52 BusOut mbedleds(LED4,LED3,LED2,LED1);
yusuke_robocup 2:955cdadf5ecc 53 HMC6352 compass(p28, p27);
yusuke_robocup 2:955cdadf5ecc 54 //Serial driver(p28, p27); // tx, rx
yusuke_robocup 2:955cdadf5ecc 55 Serial device2(p13, p14); // tx, rx
yusuke_robocup 2:955cdadf5ecc 56 Serial pc(USBTX, USBRX); // tx, rx
yusuke_robocup 2:955cdadf5ecc 57 //DigitalIn StartButton(p21);
yusuke_robocup 2:955cdadf5ecc 58 //DigitalIn CalibEnterButton(p22);
yusuke_robocup 2:955cdadf5ecc 59 DigitalIn CalibExitButton(p23);
yusuke_robocup 2:955cdadf5ecc 60 DigitalIn EEPROMButton(p24);
yusuke_robocup 2:955cdadf5ecc 61 //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
yusuke_robocup 2:955cdadf5ecc 62 Ticker pidUpdata;
yusuke_robocup 2:955cdadf5ecc 63 Ticker irDistanceUpdata;
yusuke_robocup 2:955cdadf5ecc 64 Timer timer1;
yusuke_robocup 2:955cdadf5ecc 65 Timer Survtimer;
yusuke_robocup 2:955cdadf5ecc 66 LocalFileSystem local("local"); // マウントポイントを定義（ディレクトリパスになる）
yusuke_robocup 2:955cdadf5ecc 67
yusuke_robocup 2:955cdadf5ecc 68 enum{
yusuke_robocup 2:955cdadf5ecc 69 NORMAL,
yusuke_robocup 2:955cdadf5ecc 70 LEFT_OUT,
yusuke_robocup 2:955cdadf5ecc 71 RIGHT_OUT,
yusuke_robocup 2:955cdadf5ecc 72 FRONT_OUT,
yusuke_robocup 2:955cdadf5ecc 73 BACK_OUT,
yusuke_robocup 2:955cdadf5ecc 74 };
yusuke_robocup 2:955cdadf5ecc 75
yusuke_robocup 2:955cdadf5ecc 76 enum{
yusuke_robocup 2:955cdadf5ecc 77 HOME_WAIT,
yusuke_robocup 2:955cdadf5ecc 78 DIFFENCE,
yusuke_robocup 2:955cdadf5ecc 79 WARNING,
yusuke_robocup 2:955cdadf5ecc 80 HOLD,
yusuke_robocup 2:955cdadf5ecc 81 };
yusuke_robocup 2:955cdadf5ecc 82
yusuke_robocup 2:955cdadf5ecc 83 PinName adc_num[6] = {
yusuke_robocup 2:955cdadf5ecc 84 p15,
yusuke_robocup 2:955cdadf5ecc 85 p16,
yusuke_robocup 2:955cdadf5ecc 86 p17,
yusuke_robocup 2:955cdadf5ecc 87 p18,
yusuke_robocup 2:955cdadf5ecc 88 p19,
yusuke_robocup 2:955cdadf5ecc 89 p20,
yusuke_robocup 2:955cdadf5ecc 90 };
yusuke_robocup 2:955cdadf5ecc 91 double standTu = 0;
yusuke_robocup 2:955cdadf5ecc 92 int speed[MOT_NUM] = {0};
yusuke_robocup 2:955cdadf5ecc 93 uint8_t hold_flag = 0;
yusuke_robocup 2:955cdadf5ecc 94 uint8_t state = HOME_WAIT;
yusuke_robocup 2:955cdadf5ecc 95 uint8_t lineState = NORMAL;
yusuke_robocup 2:955cdadf5ecc 96
yusuke_robocup 2:955cdadf5ecc 97 double inputPID = 180.0;
yusuke_robocup 2:955cdadf5ecc 98 static double standard;
yusuke_robocup 2:955cdadf5ecc 99 double compassPID = 0.0;