CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_T

CatPot for defence on RoboCup in 2015 winter

Dependencies:   AQM0802A HMC6352 MultiSerial PID Servo mbed

mainh.txt@7:81f57b67dff8, 2015-03-14 (annotated)

Committer:
lilac0112_1
Date:
Sat Mar 14 07:16:46 2015 +0000
Revision:
7:81f57b67dff8
Parent:
3:2f74791564c9 
The end...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lilac0112_1 2:39135c67083d 1
lilac0112_1 2:39135c67083d 2 /*PID処理*/
lilac0112_1 2:39135c67083d 3 #define RATE 0.052//52
lilac0112_1 2:39135c67083d 4 #define PID_BIAS 0.3
lilac0112_1 2:39135c67083d 5 #define REFERENCE 180.0
lilac0112_1 2:39135c67083d 6 #define MINIMUM 0.1
lilac0112_1 2:39135c67083d 7 #define MAXIMUM 360.0
lilac0112_1 2:39135c67083d 8 #define P_GAIN 1.4//1.4 //0.78
lilac0112_1 2:39135c67083d 9 #define I_GAIN 0.0 //0.0
lilac0112_1 2:39135c67083d 10 #define D_GAIN 0.05 //0.009
lilac0112_1 2:39135c67083d 11 #define OUT_LIMIT 40.0
lilac0112_1 2:39135c67083d 12 #define MAX_POW 100
lilac0112_1 2:39135c67083d 13 #define MIN_POW -100
lilac0112_1 2:39135c67083d 14
lilac0112_1 2:39135c67083d 15 /*Serial Mbed間*/
lilac0112_1 2:39135c67083d 16 #define ADDRESS 0xAA
lilac0112_1 2:39135c67083d 17 #define DATA_NUM 9
lilac0112_1 2:39135c67083d 18
lilac0112_1 2:39135c67083d 19
lilac0112_1 2:39135c67083d 20 /*BusIn sw　入力値*/
lilac0112_1 2:39135c67083d 21 #define StartS 0x01
lilac0112_1 2:39135c67083d 22 #define Debug1 0x02
lilac0112_1 2:39135c67083d 23 #define Debug2 0x04
lilac0112_1 2:39135c67083d 24 #define Debug3 0x08
lilac0112_1 2:39135c67083d 25 #define Kicker 0x10
lilac0112_1 2:39135c67083d 26
lilac0112_1 2:39135c67083d 27 #define CW 1
lilac0112_1 2:39135c67083d 28 #define CCW -1
lilac0112_1 2:39135c67083d 29 #define ENTER 0
lilac0112_1 2:39135c67083d 30 #define EXIT 1
lilac0112_1 2:39135c67083d 31
lilac0112_1 2:39135c67083d 32
lilac0112_1 2:39135c67083d 33
lilac0112_1 2:39135c67083d 34 BusIn Sw(p22,p23,p24,p25,p26);
lilac0112_1 2:39135c67083d 35 BusIn Line(p5,p6,p7,p8);//No reading line → 0b1111 ,not pull up
lilac0112_1 2:39135c67083d 36 DigitalOut Led[4] = {LED1,LED2,LED3,LED4};
lilac0112_1 2:39135c67083d 37 DigitalOut Kick(p29);
lilac0112_1 2:39135c67083d 38 Servo S555(p21);
lilac0112_1 2:39135c67083d 39 AQM0802A Lcd(p28,p27);//sda,scl
lilac0112_1 2:39135c67083d 40 MultiSerial Mbed(p9,p10);//tx,rx
lilac0112_1 2:39135c67083d 41 Serial Motor(p13,p14);//tx,rx
lilac0112_1 2:39135c67083d 42 Serial pc(USBTX,USBRX);
lilac0112_1 2:39135c67083d 43
lilac0112_1 2:39135c67083d 44 Ticker Monitor;
lilac0112_1 2:39135c67083d 45
lilac0112_1 2:39135c67083d 46
lilac0112_1 2:39135c67083d 47 /*Compass PID SetUp*/
lilac0112_1 2:39135c67083d 48 PID pid(P_GAIN,I_GAIN,D_GAIN, RATE);
lilac0112_1 2:39135c67083d 49
lilac0112_1 2:39135c67083d 50 /* motor driver*/
lilac0112_1 2:39135c67083d 51 extern string StringFIN;
lilac0112_1 2:39135c67083d 52 extern void array(int,int,int,int);
lilac0112_1 2:39135c67083d 53 int speed[4] = {0};
lilac0112_1 2:39135c67083d 54
lilac0112_1 2:39135c67083d 55
lilac0112_1 2:39135c67083d 56 /*PID補正move加算値 Revise */
lilac0112_1 2:39135c67083d 57 double ReV = 0.0;
lilac0112_1 2:39135c67083d 58
lilac0112_1 2:39135c67083d 59 /*Compass*/
lilac0112_1 2:39135c67083d 60 int CompassDef = 0;
lilac0112_1 2:39135c67083d 61 double SetAngle = 0;
lilac0112_1 2:39135c67083d 62
lilac0112_1 2:39135c67083d 63 enum {FRONT = 1, RIGHT, BACK, LEFT, NA} Direction;
lilac0112_1 2:39135c67083d 64
lilac0112_1 2:39135c67083d 65 int SetRad = 0;
lilac0112_1 2:39135c67083d 66
lilac0112_1 2:39135c67083d 67 /*StartLoop*/
lilac0112_1 2:39135c67083d 68 bool loop;
lilac0112_1 2:39135c67083d 69
lilac0112_1 2:39135c67083d 70 /*Mbed sensor data */
lilac0112_1 2:39135c67083d 71 uint8_t IrData[3] = {0},IrNum = 0;//場所によるirの数を表したもの0~11まではボールがある状態12はボールがない状態
lilac0112_1 2:39135c67083d 72 uint8_t PingData[4]={0};
lilac0112_1 2:39135c67083d 73 unsigned int Compass = 0;
lilac0112_1 2:39135c67083d 74
lilac0112_1 2:39135c67083d 75 uint8_t rx_data[DATA_NUM];
lilac0112_1 2:39135c67083d 76 /*mbed sensor data */
lilac0112_1 2:39135c67083d 77
lilac0112_1 2:39135c67083d 78 volatile int Angle[180] = {
lilac0112_1 2:39135c67083d 79 0 , 1 , 2 , 3 ,
lilac0112_1 2:39135c67083d 80 4 , 5 , 6 , 7 ,
lilac0112_1 2:39135c67083d 81 8 , 9 , 10 , 11 ,
lilac0112_1 2:39135c67083d 82 12 , 13 , 14 , 15 ,
lilac0112_1 2:39135c67083d 83 16 , 17 , 18 , 19 ,
lilac0112_1 2:39135c67083d 84 20 , 21 , 22 , 23 ,
lilac0112_1 2:39135c67083d 85 24 , 25 , 26 , 27 ,
lilac0112_1 2:39135c67083d 86 28 , 29 , 30 , 31 ,
lilac0112_1 2:39135c67083d 87 32 , 33 , 34 , 35 ,
lilac0112_1 2:39135c67083d 88 36 , 37 , 38 , 39 ,
lilac0112_1 2:39135c67083d 89 40 , 41 , 42 , 43 ,
lilac0112_1 2:39135c67083d 90 44 , 45 , 46 , 47 ,
lilac0112_1 2:39135c67083d 91 48 , 49 , 50 , 51 ,
lilac0112_1 2:39135c67083d 92 52 , 53 , 54 , 55 ,
lilac0112_1 2:39135c67083d 93 56 , 57 , 58 , 59 ,
lilac0112_1 2:39135c67083d 94 60 , 61 , 62 , 63 ,
lilac0112_1 2:39135c67083d 95 64 , 65 , 66 , 67 ,
lilac0112_1 2:39135c67083d 96 68 , 69 , 70 , 71 ,
lilac0112_1 2:39135c67083d 97 72 , 73 , 74 , 75 ,
lilac0112_1 2:39135c67083d 98 76 , 77 , 78 , 79 ,
lilac0112_1 2:39135c67083d 99 80 , 81 , 82 , 83 ,
lilac0112_1 2:39135c67083d 100 84 , 85 , 86 , 87 ,
lilac0112_1 2:39135c67083d 101 88 , 89 , 90 , -89 ,
lilac0112_1 2:39135c67083d 102 -88 , -87 , -86 , -85 ,
lilac0112_1 2:39135c67083d 103 -84 , -83 , -82 , -81 ,
lilac0112_1 2:39135c67083d 104 -80 , -79 , -78 , -77 ,
lilac0112_1 2:39135c67083d 105 -76 , -75 , -74 , -73 ,
lilac0112_1 2:39135c67083d 106 -72 , -71 , -70 , -69 ,
lilac0112_1 2:39135c67083d 107 -68 , -67 , -66 , -65 ,
lilac0112_1 2:39135c67083d 108 -64 , -63 , -62 , -61 ,
lilac0112_1 2:39135c67083d 109 -60 , -59 , -58 , -57 ,
lilac0112_1 2:39135c67083d 110 -56 , -55 , -54 , -53 ,
lilac0112_1 2:39135c67083d 111 -52 , -51 , -50 , -49 ,
lilac0112_1 2:39135c67083d 112 -48 , -47 , -46 , -45 ,
lilac0112_1 2:39135c67083d 113 -44 , -43 , -42 , -41 ,
lilac0112_1 2:39135c67083d 114 -40 , -39 , -38 , -37 ,
lilac0112_1 2:39135c67083d 115 -36 , -35 , -34 , -33 ,
lilac0112_1 2:39135c67083d 116 -32 , -31 , -30 , -29 ,
lilac0112_1 2:39135c67083d 117 -28 , -27 , -26 , -25 ,
lilac0112_1 2:39135c67083d 118 -24 , -23 , -22 , -21 ,
lilac0112_1 2:39135c67083d 119 -20 , -19 , -18 , -17 ,
lilac0112_1 2:39135c67083d 120 -16 , -15 , -14 , -13 ,
lilac0112_1 2:39135c67083d 121 -12 , -11 , -10 , -9 ,
lilac0112_1 2:39135c67083d 122 -8 , -7 , -6 , -5 ,
lilac0112_1 2:39135c67083d 123 -4 , -3 , -2 , -1 };
lilac0112_1 2:39135c67083d 124
lilac0112_1 2:39135c67083d 125 volatile int RadToVector[4] = {1,-1,-1,1};
lilac0112_1 2:39135c67083d 126
lilac0112_1 2:39135c67083d 127 int wrapRad[24] = { 0,//
lilac0112_1 2:39135c67083d 128 15,//15
lilac0112_1 2:39135c67083d 129 60,//30
lilac0112_1 2:39135c67083d 130 80,//45
lilac0112_1 2:39135c67083d 131 100,//60
lilac0112_1 2:39135c67083d 132 110,//75
lilac0112_1 2:39135c67083d 133 180,//90
lilac0112_1 2:39135c67083d 134 180,//105
lilac0112_1 2:39135c67083d 135 185,//120
lilac0112_1 2:39135c67083d 136 230,//135
lilac0112_1 2:39135c67083d 137 250,//150
lilac0112_1 2:39135c67083d 138 260,//165
lilac0112_1 2:39135c67083d 139 90, //180 or270
lilac0112_1 2:39135c67083d 140 100,//195
lilac0112_1 2:39135c67083d 141 110,//210
lilac0112_1 2:39135c67083d 142 130,//225
lilac0112_1 2:39135c67083d 143 175,//240
lilac0112_1 2:39135c67083d 144 180,//255
lilac0112_1 2:39135c67083d 145 180,//270
lilac0112_1 2:39135c67083d 146 250,//285
lilac0112_1 2:39135c67083d 147 260,//300
lilac0112_1 2:39135c67083d 148 280,//315
lilac0112_1 2:39135c67083d 149 300,//330
lilac0112_1 2:39135c67083d 150 345//345
lilac0112_1 2:39135c67083d 151 };
lilac0112_1 2:39135c67083d 152
lilac0112_1 2:39135c67083d 153
lilac0112_1 2:39135c67083d 154
lilac0112_1 2:39135c67083d 155
lilac0112_1 2:39135c67083d 156
lilac0112_1 2:39135c67083d 157 /*
lilac0112_1 2:39135c67083d 158 | |
lilac0112_1 2:39135c67083d 159 | |
lilac0112_1 2:39135c67083d 160 | o |
lilac0112_1 2:39135c67083d 161 | |
lilac0112_1 2:39135c67083d 162 | |
lilac0112_1 2:39135c67083d 163 |---------------p,q
lilac0112_1 2:39135c67083d 164
lilac0112_1 2:39135c67083d 165
lilac0112_1 2:39135c67083d 166 */