CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_ver2

2

Dependencies:   AQM0802A HMC6352 PID Servo mbed

main.h@5:dace4f3b6e4a, 2015-03-04 (annotated)

Committer:
ryuna
Date:
Wed Mar 04 07:01:19 2015 +0000
Revision:
5:dace4f3b6e4a
Parent:
4:f7946508daa8 
add one function _fool

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryuna 3:0c994b3a882e 1
ryuna 3:0c994b3a882e 2 /*PID処理*/
ryuna 4:f7946508daa8 3 #define RATE 0.052//52
ryuna 4:f7946508daa8 4 #define PID_BIAS 0.1
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 4:f7946508daa8 10 #define D_GAIN 0.03 //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 4:f7946508daa8 33 #define ENTER 0
ryuna 4:f7946508daa8 34 #define EXIT 1
ryuna 4:f7946508daa8 35
ryuna 3:0c994b3a882e 36
ryuna 3:0c994b3a882e 37 BusIn Sw(p22,p23,p24,p25,p26);
ryuna 3:0c994b3a882e 38 BusIn Line(p5,p6,p7,p8);//No reading line → 0b1111 ,not pull up
ryuna 3:0c994b3a882e 39 DigitalOut Led[4] = {LED1,LED2,LED3,LED4};
ryuna 3:0c994b3a882e 40 DigitalOut Kick(p29);
ryuna 3:0c994b3a882e 41
ryuna 3:0c994b3a882e 42 Servo S555(p21);
ryuna 3:0c994b3a882e 43 I2C Sensor(p9,p10);//sda,scl
ryuna 3:0c994b3a882e 44 HMC6352 hmc6352(p9,p10);//sda,scl
ryuna 3:0c994b3a882e 45 AQM0802A Lcd(p28,p27);//sda,scl
ryuna 3:0c994b3a882e 46 Serial Motor(p13,p14);//tx,rx
ryuna 3:0c994b3a882e 47 Serial pc(USBTX,USBRX);
ryuna 3:0c994b3a882e 48
ryuna 3:0c994b3a882e 49 /*Compass PID SetUp*/
ryuna 3:0c994b3a882e 50 PID pid(P_GAIN,I_GAIN,D_GAIN, RATE);
ryuna 3:0c994b3a882e 51
ryuna 3:0c994b3a882e 52 extern string StringFIN;
ryuna 3:0c994b3a882e 53 extern void array(int,int,int,int);
ryuna 3:0c994b3a882e 54
ryuna 3:0c994b3a882e 55 int speed[4] = {0};
ryuna 3:0c994b3a882e 56 typedef enum {FRONT = 1, RIGHT, BACK, LEFT, NA} Direction;
ryuna 3:0c994b3a882e 57 enum {SONIC = 0x01, BALL};
ryuna 5:dace4f3b6e4a 58 int SetRad = 0;
ryuna 0:dfe81bdcb486 59
ryuna 0:dfe81bdcb486 60 int TargetCoordinates[12][2][2] = {
ryuna 0:dfe81bdcb486 61 {{0,1},{0,1}},//p,q+6
ryuna 0:dfe81bdcb486 62 {{0,1},{0,1}},//p,q+5
ryuna 0:dfe81bdcb486 63 {{0,1},{0,1}},//p,q+4
ryuna 0:dfe81bdcb486 64 {{0,1},{0,1}},//p,q+3
ryuna 0:dfe81bdcb486 65 {{0,1},{0,1}},//p,q+2
ryuna 0:dfe81bdcb486 66 {{0,1},{0,1}},//p,q+1
ryuna 0:dfe81bdcb486 67 {{0,1},{0,1}},//p,q
ryuna 0:dfe81bdcb486 68 {{0,1},{0,1}},//p-1,q
ryuna 0:dfe81bdcb486 69 {{0,1},{0,1}},//p-2,q
ryuna 0:dfe81bdcb486 70 {{0,1},{0,1}},//p-3,q
ryuna 0:dfe81bdcb486 71 {{0,1},{0,1}},//p-4,q
ryuna 0:dfe81bdcb486 72 {{0,1},{0,1}},//0,q
ryuna 0:dfe81bdcb486 73 };
ryuna 0:dfe81bdcb486 74
ryuna 5:dace4f3b6e4a 75 volatile int Angle[180] = {
ryuna 5:dace4f3b6e4a 76 0 , 1 , 2 , 3 ,
ryuna 5:dace4f3b6e4a 77 4 , 5 , 6 , 7 ,
ryuna 5:dace4f3b6e4a 78 8 , 9 , 10 , 11 ,
ryuna 5:dace4f3b6e4a 79 12 , 13 , 14 , 15 ,
ryuna 5:dace4f3b6e4a 80 16 , 17 , 18 , 19 ,
ryuna 5:dace4f3b6e4a 81 20 , 21 , 22 , 23 ,
ryuna 5:dace4f3b6e4a 82 24 , 25 , 26 , 27 ,
ryuna 5:dace4f3b6e4a 83 28 , 29 , 30 , 31 ,
ryuna 5:dace4f3b6e4a 84 32 , 33 , 34 , 35 ,
ryuna 5:dace4f3b6e4a 85 36 , 37 , 38 , 39 ,
ryuna 5:dace4f3b6e4a 86 40 , 41 , 42 , 43 ,
ryuna 5:dace4f3b6e4a 87 44 , 45 , 46 , 47 ,
ryuna 5:dace4f3b6e4a 88 48 , 49 , 50 , 51 ,
ryuna 5:dace4f3b6e4a 89 52 , 53 , 54 , 55 ,
ryuna 5:dace4f3b6e4a 90 56 , 57 , 58 , 59 ,
ryuna 5:dace4f3b6e4a 91 60 , 61 , 62 , 63 ,
ryuna 5:dace4f3b6e4a 92 64 , 65 , 66 , 67 ,
ryuna 5:dace4f3b6e4a 93 68 , 69 , 70 , 71 ,
ryuna 5:dace4f3b6e4a 94 72 , 73 , 74 , 75 ,
ryuna 5:dace4f3b6e4a 95 76 , 77 , 78 , 79 ,
ryuna 5:dace4f3b6e4a 96 80 , 81 , 82 , 83 ,
ryuna 5:dace4f3b6e4a 97 84 , 85 , 86 , 87 ,
ryuna 5:dace4f3b6e4a 98 88 , 89 , 90 , -89 ,
ryuna 5:dace4f3b6e4a 99 -88 , -87 , -86 , -85 ,
ryuna 5:dace4f3b6e4a 100 -84 , -83 , -82 , -81 ,
ryuna 5:dace4f3b6e4a 101 -80 , -79 , -78 , -77 ,
ryuna 5:dace4f3b6e4a 102 -76 , -75 , -74 , -73 ,
ryuna 5:dace4f3b6e4a 103 -72 , -71 , -70 , -69 ,
ryuna 5:dace4f3b6e4a 104 -68 , -67 , -66 , -65 ,
ryuna 5:dace4f3b6e4a 105 -64 , -63 , -62 , -61 ,
ryuna 5:dace4f3b6e4a 106 -60 , -59 , -58 , -57 ,
ryuna 5:dace4f3b6e4a 107 -56 , -55 , -54 , -53 ,
ryuna 5:dace4f3b6e4a 108 -52 , -51 , -50 , -49 ,
ryuna 5:dace4f3b6e4a 109 -48 , -47 , -46 , -45 ,
ryuna 5:dace4f3b6e4a 110 -44 , -43 , -42 , -41 ,
ryuna 5:dace4f3b6e4a 111 -40 , -39 , -38 , -37 ,
ryuna 5:dace4f3b6e4a 112 -36 , -35 , -34 , -33 ,
ryuna 5:dace4f3b6e4a 113 -32 , -31 , -30 , -29 ,
ryuna 5:dace4f3b6e4a 114 -28 , -27 , -26 , -25 ,
ryuna 5:dace4f3b6e4a 115 -24 , -23 , -22 , -21 ,
ryuna 5:dace4f3b6e4a 116 -20 , -19 , -18 , -17 ,
ryuna 5:dace4f3b6e4a 117 -16 , -15 , -14 , -13 ,
ryuna 5:dace4f3b6e4a 118 -12 , -11 , -10 , -9 ,
ryuna 5:dace4f3b6e4a 119 -8 , -7 , -6 , -5 ,
ryuna 5:dace4f3b6e4a 120 -4 , -3 , -2 , -1 };
ryuna 5:dace4f3b6e4a 121
ryuna 5:dace4f3b6e4a 122 volatile int RadToVector[4] = {1,-1,-1,1};
ryuna 5:dace4f3b6e4a 123
ryuna 5:dace4f3b6e4a 124
ryuna 0:dfe81bdcb486 125 /*
ryuna 0:dfe81bdcb486 126 | |
ryuna 0:dfe81bdcb486 127 | |
ryuna 0:dfe81bdcb486 128 | o |
ryuna 0:dfe81bdcb486 129 | |
ryuna 0:dfe81bdcb486 130 | |
ryuna 0:dfe81bdcb486 131 |---------------p,q
ryuna 0:dfe81bdcb486 132
ryuna 0:dfe81bdcb486 133
ryuna 0:dfe81bdcb486 134 */