CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_ver2

2

Dependencies:   AQM0802A HMC6352 PID Servo mbed

main.h@3:0c994b3a882e, 2015-03-02 (annotated)

Committer:
ryuna
Date:
Mon Mar 02 05:55:33 2015 +0000
Revision:
3:0c994b3a882e
Parent:
0:dfe81bdcb486
Child:
4:f7946508daa8
?????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryuna 3:0c994b3a882e 1
ryuna 3:0c994b3a882e 2 /*PID処理*/
ryuna 3:0c994b3a882e 3 #define RATE 0.18//52
ryuna 3:0c994b3a882e 4 #define PID_BIAS 0.3
ryuna 3:0c994b3a882e 5 #define REFERENCE 180.0
ryuna 3:0c994b3a882e 6 #define MINIMUM 0.1
ryuna 3:0c994b3a882e 7 #define MAXIMUM 360.0
ryuna 3:0c994b3a882e 8 #define P_GAIN 1.8//1.4 //0.78
ryuna 3:0c994b3a882e 9 #define I_GAIN 0.0 //0.0
ryuna 3:0c994b3a882e 10 #define D_GAIN 0.05 //0.009
ryuna 3:0c994b3a882e 11 #define OUT_LIMIT 50.0
ryuna 3:0c994b3a882e 12 #define MAX_POW 100
ryuna 3:0c994b3a882e 13 #define MIN_POW -100
ryuna 3:0c994b3a882e 14
ryuna 3:0c994b3a882e 15 /*回り込みの計算用*/
ryuna 3:0c994b3a882e 16 #define PI 3.1415926
ryuna 3:0c994b3a882e 17
ryuna 3:0c994b3a882e 18 /*LPC1114 I2C Slave Address*/
ryuna 3:0c994b3a882e 19 #define ADDRESS_R 0xAA
ryuna 3:0c994b3a882e 20 #define ADDRESS_L 0xAC
ryuna 3:0c994b3a882e 21
ryuna 3:0c994b3a882e 22 /*BusIn sw　入力値*/
ryuna 3:0c994b3a882e 23 #define StartS 0x01
ryuna 3:0c994b3a882e 24 #define Debug1 0x02
ryuna 3:0c994b3a882e 25 #define Debug2 0x04
ryuna 3:0c994b3a882e 26 #define Debug3 0x06
ryuna 3:0c994b3a882e 27 #define Kicker 0x08
ryuna 3:0c994b3a882e 28 #define Calibration 0x10
ryuna 3:0c994b3a882e 29
ryuna 3:0c994b3a882e 30
ryuna 3:0c994b3a882e 31 #define CW 1
ryuna 3:0c994b3a882e 32 #define CCW -1
ryuna 3:0c994b3a882e 33
ryuna 3:0c994b3a882e 34 BusIn Sw(p22,p23,p24,p25,p26);
ryuna 3:0c994b3a882e 35 BusIn Line(p5,p6,p7,p8);//No reading line → 0b1111 ,not pull up
ryuna 3:0c994b3a882e 36 DigitalOut Led[4] = {LED1,LED2,LED3,LED4};
ryuna 3:0c994b3a882e 37 DigitalOut Kick(p29);
ryuna 3:0c994b3a882e 38
ryuna 3:0c994b3a882e 39 Servo S555(p21);
ryuna 3:0c994b3a882e 40 I2C Sensor(p9,p10);//sda,scl
ryuna 3:0c994b3a882e 41 HMC6352 hmc6352(p9,p10);//sda,scl
ryuna 3:0c994b3a882e 42 AQM0802A Lcd(p28,p27);//sda,scl
ryuna 3:0c994b3a882e 43 Serial Motor(p13,p14);//tx,rx
ryuna 3:0c994b3a882e 44 Serial pc(USBTX,USBRX);
ryuna 3:0c994b3a882e 45
ryuna 3:0c994b3a882e 46 /*Compass PID SetUp*/
ryuna 3:0c994b3a882e 47 PID pid(P_GAIN,I_GAIN,D_GAIN, RATE);
ryuna 3:0c994b3a882e 48
ryuna 3:0c994b3a882e 49 extern string StringFIN;
ryuna 3:0c994b3a882e 50 extern void array(int,int,int,int);
ryuna 3:0c994b3a882e 51
ryuna 3:0c994b3a882e 52 int speed[4] = {0};
ryuna 3:0c994b3a882e 53 typedef enum {FRONT = 1, RIGHT, BACK, LEFT, NA} Direction;
ryuna 3:0c994b3a882e 54 enum {SONIC = 0x01, BALL};
ryuna 3:0c994b3a882e 55
ryuna 0:dfe81bdcb486 56
ryuna 0:dfe81bdcb486 57 int TargetCoordinates[12][2][2] = {
ryuna 0:dfe81bdcb486 58 {{0,1},{0,1}},//p,q+6
ryuna 0:dfe81bdcb486 59 {{0,1},{0,1}},//p,q+5
ryuna 0:dfe81bdcb486 60 {{0,1},{0,1}},//p,q+4
ryuna 0:dfe81bdcb486 61 {{0,1},{0,1}},//p,q+3
ryuna 0:dfe81bdcb486 62 {{0,1},{0,1}},//p,q+2
ryuna 0:dfe81bdcb486 63 {{0,1},{0,1}},//p,q+1
ryuna 0:dfe81bdcb486 64 {{0,1},{0,1}},//p,q
ryuna 0:dfe81bdcb486 65 {{0,1},{0,1}},//p-1,q
ryuna 0:dfe81bdcb486 66 {{0,1},{0,1}},//p-2,q
ryuna 0:dfe81bdcb486 67 {{0,1},{0,1}},//p-3,q
ryuna 0:dfe81bdcb486 68 {{0,1},{0,1}},//p-4,q
ryuna 0:dfe81bdcb486 69 {{0,1},{0,1}},//0,q
ryuna 0:dfe81bdcb486 70 };
ryuna 0:dfe81bdcb486 71
ryuna 0:dfe81bdcb486 72 /*
ryuna 0:dfe81bdcb486 73 | |
ryuna 0:dfe81bdcb486 74 | |
ryuna 0:dfe81bdcb486 75 | o |
ryuna 0:dfe81bdcb486 76 | |
ryuna 0:dfe81bdcb486 77 | |
ryuna 0:dfe81bdcb486 78 |---------------p,q
ryuna 0:dfe81bdcb486 79
ryuna 0:dfe81bdcb486 80
ryuna 0:dfe81bdcb486 81 */