CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_ver3

version 3 通信方式，マイコン等に変更あり

Dependencies:   AQM0802A PID Servo mbed

main.h@6:bfc0e8d2d3ae, 2015-03-13 (annotated)

Committer:
ryuna
Date:
Fri Mar 13 12:26:29 2015 +0000
Revision:
6:bfc0e8d2d3ae
Parent:
5:09afcbe0c18f 
???

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryuna 0:65b9e62cc2b6 1
ryuna 0:65b9e62cc2b6 2 /*PID処理*/
ryuna 0:65b9e62cc2b6 3 #define RATE 0.052//52
ryuna 4:2857f273a7f4 4 #define PID_BIAS 0.2
ryuna 0:65b9e62cc2b6 5 #define REFERENCE 180.0
ryuna 4:2857f273a7f4 6 #define MINIMUM 0.0
ryuna 4:2857f273a7f4 7 #define MAXIMUM 359.0
ryuna 0:65b9e62cc2b6 8 #define P_GAIN 1.4//1.4 //0.78
ryuna 0:65b9e62cc2b6 9 #define I_GAIN 0.0 //0.0
ryuna 0:65b9e62cc2b6 10 #define D_GAIN 0.05 //0.009
ryuna 0:65b9e62cc2b6 11 #define OUT_LIMIT 40.0
ryuna 0:65b9e62cc2b6 12 #define MAX_POW 100
ryuna 0:65b9e62cc2b6 13 #define MIN_POW -100
ryuna 2:e84bb87eea71 14 #define PID_CYCLE 0.05 //s
ryuna 0:65b9e62cc2b6 15
ryuna 5:09afcbe0c18f 16 /*Servoなんか*/
ryuna 5:09afcbe0c18f 17 #define WAIT_TIME 0.20
ryuna 5:09afcbe0c18f 18
ryuna 0:65b9e62cc2b6 19 /*Serial Mbed間*/
ryuna 0:65b9e62cc2b6 20 #define ADDRESS 0xAA
ryuna 0:65b9e62cc2b6 21
ryuna 0:65b9e62cc2b6 22
ryuna 0:65b9e62cc2b6 23 /*BusIn sw　入力値*/
ryuna 0:65b9e62cc2b6 24 #define StartS 0x01
ryuna 0:65b9e62cc2b6 25 #define Debug1 0x02
ryuna 0:65b9e62cc2b6 26 #define Debug2 0x04
ryuna 0:65b9e62cc2b6 27 #define Debug3 0x08
ryuna 5:09afcbe0c18f 28 #define Debug12 0x06
ryuna 5:09afcbe0c18f 29 #define Debug23 0x0C
ryuna 0:65b9e62cc2b6 30 #define Kicker 0x10
ryuna 0:65b9e62cc2b6 31
ryuna 0:65b9e62cc2b6 32 #define CW 1
ryuna 0:65b9e62cc2b6 33 #define CCW -1
ryuna 0:65b9e62cc2b6 34 #define ENTER 0
ryuna 0:65b9e62cc2b6 35 #define EXIT 1
ryuna 0:65b9e62cc2b6 36
ryuna 4:2857f273a7f4 37 Serial Mbed(p9,p10);//tx,rx
ryuna 0:65b9e62cc2b6 38
ryuna 0:65b9e62cc2b6 39 BusIn Sw(p22,p23,p24,p25,p26);
ryuna 0:65b9e62cc2b6 40 BusIn Line(p5,p6,p7,p8);//No reading line → 0b1111 ,not pull up
ryuna 0:65b9e62cc2b6 41 DigitalOut Led[4] = {LED1,LED2,LED3,LED4};
ryuna 0:65b9e62cc2b6 42 DigitalOut Kick(p29);
ryuna 0:65b9e62cc2b6 43 Servo S555(p21);
ryuna 0:65b9e62cc2b6 44 AQM0802A Lcd(p28,p27);//sda,scl
ryuna 0:65b9e62cc2b6 45 Serial Motor(p13,p14);//tx,rx
ryuna 0:65b9e62cc2b6 46 Serial pc(USBTX,USBRX);
ryuna 0:65b9e62cc2b6 47
ryuna 0:65b9e62cc2b6 48
ryuna 0:65b9e62cc2b6 49 /*Compass PID SetUp*/
ryuna 0:65b9e62cc2b6 50 PID pid(P_GAIN,I_GAIN,D_GAIN, RATE);
ryuna 2:e84bb87eea71 51 Ticker pidupdate;
ryuna 0:65b9e62cc2b6 52
ryuna 4:2857f273a7f4 53
ryuna 5:09afcbe0c18f 54 /*Servo timer */
ryuna 5:09afcbe0c18f 55 Timeout STimer;
ryuna 5:09afcbe0c18f 56
ryuna 6:bfc0e8d2d3ae 57 Timeout GoHTimer;
ryuna 6:bfc0e8d2d3ae 58
ryuna 0:65b9e62cc2b6 59 extern string StringFIN;
ryuna 0:65b9e62cc2b6 60 extern void array(int,int,int,int);
ryuna 4:2857f273a7f4 61 extern void micon_rx(void);
ryuna 4:2857f273a7f4 62
ryuna 0:65b9e62cc2b6 63 int speed[4] = {0};
ryuna 0:65b9e62cc2b6 64
ryuna 0:65b9e62cc2b6 65
ryuna 0:65b9e62cc2b6 66
ryuna 2:e84bb87eea71 67 double SetDegree = 0;
ryuna 0:65b9e62cc2b6 68
ryuna 0:65b9e62cc2b6 69 /*Mbed sensor data */
ryuna 0:65b9e62cc2b6 70 uint8_t IrData[3] = {0},IrNum = 0;
ryuna 1:f91d53098d57 71 uint8_t PingData[4]={0};
ryuna 5:09afcbe0c18f 72 int Compass = 0;
ryuna 5:09afcbe0c18f 73 unsigned int CompassDef = 0;
ryuna 2:e84bb87eea71 74 double SetC = 0, InputPID = 180.0, CompassPID = 0.0;
ryuna 1:f91d53098d57 75
ryuna 5:09afcbe0c18f 76 bool WaitFlag = 0;
ryuna 6:bfc0e8d2d3ae 77 bool GoHFlag = 0;
ryuna 4:2857f273a7f4 78
ryuna 4:2857f273a7f4 79 /* motor driver*/
ryuna 4:2857f273a7f4 80
ryuna 4:2857f273a7f4 81 enum {FRONT = 1, RIGHT, BACK, LEFT, NA} Direction;
ryuna 4:2857f273a7f4 82
ryuna 0:65b9e62cc2b6 83 /*mbed sensor data */
ryuna 0:65b9e62cc2b6 84
ryuna 0:65b9e62cc2b6 85 volatile int Angle[180] = {
ryuna 0:65b9e62cc2b6 86 0 , 1 , 2 , 3 ,
ryuna 0:65b9e62cc2b6 87 4 , 5 , 6 , 7 ,
ryuna 0:65b9e62cc2b6 88 8 , 9 , 10 , 11 ,
ryuna 0:65b9e62cc2b6 89 12 , 13 , 14 , 15 ,
ryuna 0:65b9e62cc2b6 90 16 , 17 , 18 , 19 ,
ryuna 0:65b9e62cc2b6 91 20 , 21 , 22 , 23 ,
ryuna 0:65b9e62cc2b6 92 24 , 25 , 26 , 27 ,
ryuna 0:65b9e62cc2b6 93 28 , 29 , 30 , 31 ,
ryuna 0:65b9e62cc2b6 94 32 , 33 , 34 , 35 ,
ryuna 0:65b9e62cc2b6 95 36 , 37 , 38 , 39 ,
ryuna 0:65b9e62cc2b6 96 40 , 41 , 42 , 43 ,
ryuna 0:65b9e62cc2b6 97 44 , 45 , 46 , 47 ,
ryuna 0:65b9e62cc2b6 98 48 , 49 , 50 , 51 ,
ryuna 0:65b9e62cc2b6 99 52 , 53 , 54 , 55 ,
ryuna 0:65b9e62cc2b6 100 56 , 57 , 58 , 59 ,
ryuna 0:65b9e62cc2b6 101 60 , 61 , 62 , 63 ,
ryuna 0:65b9e62cc2b6 102 64 , 65 , 66 , 67 ,
ryuna 0:65b9e62cc2b6 103 68 , 69 , 70 , 71 ,
ryuna 0:65b9e62cc2b6 104 72 , 73 , 74 , 75 ,
ryuna 0:65b9e62cc2b6 105 76 , 77 , 78 , 79 ,
ryuna 0:65b9e62cc2b6 106 80 , 81 , 82 , 83 ,
ryuna 0:65b9e62cc2b6 107 84 , 85 , 86 , 87 ,
ryuna 0:65b9e62cc2b6 108 88 , 89 , 90 , -89 ,
ryuna 0:65b9e62cc2b6 109 -88 , -87 , -86 , -85 ,
ryuna 0:65b9e62cc2b6 110 -84 , -83 , -82 , -81 ,
ryuna 0:65b9e62cc2b6 111 -80 , -79 , -78 , -77 ,
ryuna 0:65b9e62cc2b6 112 -76 , -75 , -74 , -73 ,
ryuna 0:65b9e62cc2b6 113 -72 , -71 , -70 , -69 ,
ryuna 0:65b9e62cc2b6 114 -68 , -67 , -66 , -65 ,
ryuna 0:65b9e62cc2b6 115 -64 , -63 , -62 , -61 ,
ryuna 0:65b9e62cc2b6 116 -60 , -59 , -58 , -57 ,
ryuna 0:65b9e62cc2b6 117 -56 , -55 , -54 , -53 ,
ryuna 0:65b9e62cc2b6 118 -52 , -51 , -50 , -49 ,
ryuna 0:65b9e62cc2b6 119 -48 , -47 , -46 , -45 ,
ryuna 0:65b9e62cc2b6 120 -44 , -43 , -42 , -41 ,
ryuna 0:65b9e62cc2b6 121 -40 , -39 , -38 , -37 ,
ryuna 0:65b9e62cc2b6 122 -36 , -35 , -34 , -33 ,
ryuna 0:65b9e62cc2b6 123 -32 , -31 , -30 , -29 ,
ryuna 0:65b9e62cc2b6 124 -28 , -27 , -26 , -25 ,
ryuna 0:65b9e62cc2b6 125 -24 , -23 , -22 , -21 ,
ryuna 0:65b9e62cc2b6 126 -20 , -19 , -18 , -17 ,
ryuna 0:65b9e62cc2b6 127 -16 , -15 , -14 , -13 ,
ryuna 0:65b9e62cc2b6 128 -12 , -11 , -10 , -9 ,
ryuna 0:65b9e62cc2b6 129 -8 , -7 , -6 , -5 ,
ryuna 0:65b9e62cc2b6 130 -4 , -3 , -2 , -1 };
ryuna 0:65b9e62cc2b6 131
ryuna 2:e84bb87eea71 132 volatile int DegreeToVector[5] = {1,-1,-1,1,1};
ryuna 0:65b9e62cc2b6 133
ryuna 2:e84bb87eea71 134 int wrapDegree[24] = { 0,//
ryuna 5:09afcbe0c18f 135 30,//15
ryuna 6:bfc0e8d2d3ae 136 45,//30
ryuna 6:bfc0e8d2d3ae 137 100,//45
ryuna 6:bfc0e8d2d3ae 138 110,//60
ryuna 5:09afcbe0c18f 139 140,//75
ryuna 5:09afcbe0c18f 140 170,//90
ryuna 5:09afcbe0c18f 141 170,//105
ryuna 6:bfc0e8d2d3ae 142 190,//120
ryuna 6:bfc0e8d2d3ae 143 190,//135
ryuna 5:09afcbe0c18f 144 200,//150
ryuna 6:bfc0e8d2d3ae 145 230,//165
ryuna 0:65b9e62cc2b6 146 90, //180 or270
ryuna 6:bfc0e8d2d3ae 147 150,//195
ryuna 6:bfc0e8d2d3ae 148 160,//210
ryuna 6:bfc0e8d2d3ae 149 170,//225
ryuna 6:bfc0e8d2d3ae 150 170,//240
ryuna 6:bfc0e8d2d3ae 151 190,//255
ryuna 6:bfc0e8d2d3ae 152 190,//270
ryuna 6:bfc0e8d2d3ae 153 220,//285
ryuna 6:bfc0e8d2d3ae 154 250,//300
ryuna 6:bfc0e8d2d3ae 155 260,//315
ryuna 6:bfc0e8d2d3ae 156 315,//330
ryuna 6:bfc0e8d2d3ae 157 330//345
ryuna 0:65b9e62cc2b6 158 };
ryuna 0:65b9e62cc2b6 159
ryuna 0:65b9e62cc2b6 160
ryuna 0:65b9e62cc2b6 161
ryuna 0:65b9e62cc2b6 162
ryuna 0:65b9e62cc2b6 163
ryuna 0:65b9e62cc2b6 164 /*
ryuna 0:65b9e62cc2b6 165 | |
ryuna 0:65b9e62cc2b6 166 | |
ryuna 0:65b9e62cc2b6 167 | o |
ryuna 0:65b9e62cc2b6 168 | |
ryuna 0:65b9e62cc2b6 169 | |
ryuna 0:65b9e62cc2b6 170 |---------------p,q
ryuna 0:65b9e62cc2b6 171
ryuna 0:65b9e62cc2b6 172
ryuna 0:65b9e62cc2b6 173 */