CatPot 2015-2016 / Mbed 2 deprecated CatPot_SensorRight_T

CatPot for defence on RoboCup in 2015 winter

Dependencies:   HMC6352 Ping IRM2121_2 mbed MultiSerial Watchdog

main.cpp@1:5b2e1ee5e63e, 2015-03-11 (annotated)

Committer:
lilac0112_1
Date:
Wed Mar 11 01:11:40 2015 +0000
Revision:
1:5b2e1ee5e63e
Parent:
0:94ced082e13a 
aaaaaaaa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lilac0112_1 0:94ced082e13a 1 /*
lilac0112_1 1:5b2e1ee5e63e 2 *main用LPC1768と通信するSensor用LPC1768のプログラム()
lilac0112_1 0:94ced082e13a 3 *
lilac0112_1 1:5b2e1ee5e63e 4 *現在は2つのLPC1768でSerial通信します．
lilac0112_1 0:94ced082e13a 5 *
lilac0112_1 1:5b2e1ee5e63e 6 */
lilac0112_1 1:5b2e1ee5e63e 7
lilac0112_1 0:94ced082e13a 8 #include "mbed.h"
lilac0112_1 1:5b2e1ee5e63e 9
lilac0112_1 0:94ced082e13a 10 #include "IRM2121.h"
lilac0112_1 0:94ced082e13a 11 #include "Ping.h"
lilac0112_1 1:5b2e1ee5e63e 12 #include "MultiSerial.h"
lilac0112_1 1:5b2e1ee5e63e 13 #include "HMC6352.h"
lilac0112_1 1:5b2e1ee5e63e 14 #include "Watchdog.h"
lilac0112_1 1:5b2e1ee5e63e 15
lilac0112_1 0:94ced082e13a 16 #define PING_COUNT 5 //5回に1回にPing処理
lilac0112_1 0:94ced082e13a 17 #define IR_LEVEL 400 //irのしきい値
lilac0112_1 1:5b2e1ee5e63e 18 #define ADDRESS 0xAA//
lilac0112_1 1:5b2e1ee5e63e 19 #define DATA_NUM 9
lilac0112_1 1:5b2e1ee5e63e 20
lilac0112_1 1:5b2e1ee5e63e 21 uint8_t tx_data[DATA_NUM]={0};
lilac0112_1 0:94ced082e13a 22
lilac0112_1 1:5b2e1ee5e63e 23 IRM2121 Ir[12] = {p20,p19,p18,p17,p16,p15, p12,p11, p8,p7,p6,p5};
lilac0112_1 0:94ced082e13a 24
lilac0112_1 1:5b2e1ee5e63e 25 Ping Ping1(p22, p21);
lilac0112_1 1:5b2e1ee5e63e 26 Ping Ping2(p24, p23);
lilac0112_1 1:5b2e1ee5e63e 27 Ping Ping3(p26, p25);
lilac0112_1 1:5b2e1ee5e63e 28 Ping Ping4(p30, p29);
lilac0112_1 1:5b2e1ee5e63e 29
lilac0112_1 1:5b2e1ee5e63e 30 DigitalOut Led[4] = {LED1, LED2, LED3, LED4};
lilac0112_1 0:94ced082e13a 31
lilac0112_1 1:5b2e1ee5e63e 32 HMC6352 hmc6352(p28, p27);
lilac0112_1 1:5b2e1ee5e63e 33
lilac0112_1 1:5b2e1ee5e63e 34 MultiSerial Mbed(p13, p14);
lilac0112_1 1:5b2e1ee5e63e 35 //MultiSerial mbed(p9, p10);
lilac0112_1 0:94ced082e13a 36
lilac0112_1 1:5b2e1ee5e63e 37 Serial pc(USBTX,USBRX);
lilac0112_1 1:5b2e1ee5e63e 38
lilac0112_1 0:94ced082e13a 39 /*irm2121平均化*/
lilac0112_1 1:5b2e1ee5e63e 40 uint8_t IrMoving_ave(uint8_t num, int data){
lilac0112_1 0:94ced082e13a 41 static unsigned int sum[10];
lilac0112_1 0:94ced082e13a 42 static unsigned temp[6][10];
lilac0112_1 1:5b2e1ee5e63e 43
lilac0112_1 1:5b2e1ee5e63e 44 sum[num] -= temp[num][9];
lilac0112_1 0:94ced082e13a 45 sum[num] += data;
lilac0112_1 1:5b2e1ee5e63e 46
lilac0112_1 1:5b2e1ee5e63e 47
lilac0112_1 0:94ced082e13a 48 temp[num][9] = temp[num][8];
lilac0112_1 0:94ced082e13a 49 temp[num][8] = temp[num][7];
lilac0112_1 0:94ced082e13a 50 temp[num][7] = temp[num][6];
lilac0112_1 0:94ced082e13a 51 temp[num][6] = temp[num][5];
lilac0112_1 0:94ced082e13a 52 temp[num][5] = temp[num][4];
lilac0112_1 0:94ced082e13a 53 temp[num][4] = temp[num][3];
lilac0112_1 0:94ced082e13a 54 temp[num][3] = temp[num][2];
lilac0112_1 0:94ced082e13a 55 temp[num][2] = temp[num][1];
lilac0112_1 0:94ced082e13a 56 temp[num][1] = temp[num][0];
lilac0112_1 0:94ced082e13a 57 temp[num][0] = data;
lilac0112_1 1:5b2e1ee5e63e 58
lilac0112_1 1:5b2e1ee5e63e 59 return sum[num]/10;
lilac0112_1 0:94ced082e13a 60 }
lilac0112_1 1:5b2e1ee5e63e 61
lilac0112_1 1:5b2e1ee5e63e 62 void IrSort(unsigned int input[], uint8_t output[]){
lilac0112_1 1:5b2e1ee5e63e 63
lilac0112_1 0:94ced082e13a 64 /*
lilac0112_1 0:94ced082e13a 65 *1,2位の値とその場所の番号をアウトプットに格納する.
lilac0112_1 0:94ced082e13a 66 *値が255以上だとボールを検知してないことになる。
lilac0112_1 0:94ced082e13a 67 */
lilac0112_1 1:5b2e1ee5e63e 68 unsigned int near[2] = {1000,1000};
lilac0112_1 1:5b2e1ee5e63e 69 uint8_t num[2] = {12,0}, i;
lilac0112_1 1:5b2e1ee5e63e 70 static int sub[12]={0};
lilac0112_1 1:5b2e1ee5e63e 71
lilac0112_1 1:5b2e1ee5e63e 72 for(i = 0; i < 12; i++ ){
lilac0112_1 1:5b2e1ee5e63e 73 if(!input[i]){
lilac0112_1 0:94ced082e13a 74 continue;
lilac0112_1 0:94ced082e13a 75 }
lilac0112_1 1:5b2e1ee5e63e 76
lilac0112_1 1:5b2e1ee5e63e 77 if(near[1] < input[i]+sub[i]){
lilac0112_1 0:94ced082e13a 78 continue;
lilac0112_1 0:94ced082e13a 79 }
lilac0112_1 1:5b2e1ee5e63e 80 if(near[0] < input[i]+sub[i]){
lilac0112_1 1:5b2e1ee5e63e 81 near[1] = input[i]+sub[i];
lilac0112_1 1:5b2e1ee5e63e 82 num[1] = i;
lilac0112_1 1:5b2e1ee5e63e 83 continue;
lilac0112_1 1:5b2e1ee5e63e 84 }
lilac0112_1 1:5b2e1ee5e63e 85 near[1] = near[0];
lilac0112_1 1:5b2e1ee5e63e 86 num [1] = num[0];
lilac0112_1 1:5b2e1ee5e63e 87 near[0] = input[i]+sub[i];
lilac0112_1 1:5b2e1ee5e63e 88 num [0] = i;
lilac0112_1 0:94ced082e13a 89 }
lilac0112_1 1:5b2e1ee5e63e 90
lilac0112_1 1:5b2e1ee5e63e 91 if(num[0] == 12){//not found
lilac0112_1 1:5b2e1ee5e63e 92 output[0] = 255;
lilac0112_1 0:94ced082e13a 93 output[1] = 255;
lilac0112_1 0:94ced082e13a 94 output[2] = 255;
lilac0112_1 0:94ced082e13a 95 return;
lilac0112_1 0:94ced082e13a 96 }
lilac0112_1 1:5b2e1ee5e63e 97 if(num[1] == 12){
lilac0112_1 1:5b2e1ee5e63e 98 num[1] = 0;
lilac0112_1 1:5b2e1ee5e63e 99 }
lilac0112_1 1:5b2e1ee5e63e 100 /*
lilac0112_1 1:5b2e1ee5e63e 101 for(i = 0; i < 12; i++ ){
lilac0112_1 1:5b2e1ee5e63e 102
lilac0112_1 1:5b2e1ee5e63e 103 if(i == num[0]){
lilac0112_1 1:5b2e1ee5e63e 104
lilac0112_1 1:5b2e1ee5e63e 105 sub[i]++;
lilac0112_1 1:5b2e1ee5e63e 106 continue;
lilac0112_1 1:5b2e1ee5e63e 107 }
lilac0112_1 1:5b2e1ee5e63e 108 if(sub[i]>0){
lilac0112_1 1:5b2e1ee5e63e 109
lilac0112_1 1:5b2e1ee5e63e 110 sub[i]--;
lilac0112_1 1:5b2e1ee5e63e 111 }
lilac0112_1 1:5b2e1ee5e63e 112 }
lilac0112_1 1:5b2e1ee5e63e 113 */
lilac0112_1 1:5b2e1ee5e63e 114 output[0] = num[0]*12 + num[1];//rank1*12 + rank2
lilac0112_1 1:5b2e1ee5e63e 115 output[1] = near[0] /10;//rank1
lilac0112_1 1:5b2e1ee5e63e 116 output[2] = near[1] /10;//rank2
lilac0112_1 1:5b2e1ee5e63e 117
lilac0112_1 0:94ced082e13a 118 }
lilac0112_1 1:5b2e1ee5e63e 119
lilac0112_1 1:5b2e1ee5e63e 120
lilac0112_1 1:5b2e1ee5e63e 121
lilac0112_1 1:5b2e1ee5e63e 122 int main() {
lilac0112_1 1:5b2e1ee5e63e 123
lilac0112_1 1:5b2e1ee5e63e 124 /*begin SetUp*/
lilac0112_1 1:5b2e1ee5e63e 125 Watchdog g;
lilac0112_1 1:5b2e1ee5e63e 126 g.Configure(1.0f);//1秒でWDT発生
lilac0112_1 1:5b2e1ee5e63e 127 /*end Setup*/
lilac0112_1 1:5b2e1ee5e63e 128
lilac0112_1 1:5b2e1ee5e63e 129
lilac0112_1 0:94ced082e13a 130 /*Count*/
lilac0112_1 0:94ced082e13a 131 uint8_t PingCk = 0;
lilac0112_1 1:5b2e1ee5e63e 132
lilac0112_1 1:5b2e1ee5e63e 133 /*Data*/
lilac0112_1 1:5b2e1ee5e63e 134 unsigned int PingData[4] = {0};
lilac0112_1 1:5b2e1ee5e63e 135 unsigned int IrBase[12] = {0};
lilac0112_1 1:5b2e1ee5e63e 136 uint8_t IrData[3] = {0};
lilac0112_1 1:5b2e1ee5e63e 137 uint16_t Compass=0;
lilac0112_1 1:5b2e1ee5e63e 138
lilac0112_1 1:5b2e1ee5e63e 139 /*Serial*/
lilac0112_1 1:5b2e1ee5e63e 140
lilac0112_1 1:5b2e1ee5e63e 141
lilac0112_1 1:5b2e1ee5e63e 142 Mbed.start_write();
lilac0112_1 1:5b2e1ee5e63e 143 Mbed.write_data(tx_data, ADDRESS, DATA_NUM);
lilac0112_1 1:5b2e1ee5e63e 144
lilac0112_1 1:5b2e1ee5e63e 145 //Continuous mode, periodic set/reset, 20Hz measurement rate.
lilac0112_1 1:5b2e1ee5e63e 146 hmc6352.setOpMode(HMC6352_CONTINUOUS, 1, 20);
lilac0112_1 1:5b2e1ee5e63e 147
lilac0112_1 0:94ced082e13a 148 while(1) {
lilac0112_1 1:5b2e1ee5e63e 149
lilac0112_1 1:5b2e1ee5e63e 150 g.Service();
lilac0112_1 1:5b2e1ee5e63e 151
lilac0112_1 1:5b2e1ee5e63e 152 PingCk ++;
lilac0112_1 1:5b2e1ee5e63e 153
lilac0112_1 1:5b2e1ee5e63e 154 Led[0] = 1;//周回の初め
lilac0112_1 1:5b2e1ee5e63e 155
lilac0112_1 1:5b2e1ee5e63e 156
lilac0112_1 1:5b2e1ee5e63e 157
lilac0112_1 1:5b2e1ee5e63e 158 IrBase[0] = Ir[0].Read();
lilac0112_1 1:5b2e1ee5e63e 159 IrBase[1] = Ir[1].Read();
lilac0112_1 1:5b2e1ee5e63e 160 IrBase[2] = Ir[2].Read();
lilac0112_1 1:5b2e1ee5e63e 161 IrBase[3] = Ir[3].Read();
lilac0112_1 1:5b2e1ee5e63e 162 IrBase[4] = Ir[4].Read();
lilac0112_1 1:5b2e1ee5e63e 163 IrBase[5] = Ir[5].Read();
lilac0112_1 1:5b2e1ee5e63e 164 IrBase[6] = Ir[6].Read();
lilac0112_1 1:5b2e1ee5e63e 165 IrBase[7] = Ir[7].Read();
lilac0112_1 1:5b2e1ee5e63e 166 IrBase[8] = Ir[8].Read();
lilac0112_1 1:5b2e1ee5e63e 167 IrBase[9] = Ir[9].Read();
lilac0112_1 1:5b2e1ee5e63e 168 IrBase[10] = Ir[10].Read();
lilac0112_1 1:5b2e1ee5e63e 169 IrBase[11] = Ir[11].Read();
lilac0112_1 1:5b2e1ee5e63e 170
lilac0112_1 1:5b2e1ee5e63e 171 /*平均化
lilac0112_1 1:5b2e1ee5e63e 172 IrTemp[0] = IrMoving_ave(0,IrTemp[0]);
lilac0112_1 1:5b2e1ee5e63e 173 IrTemp[1] = IrMoving_ave(1,IrTemp[1]);
lilac0112_1 1:5b2e1ee5e63e 174 IrTemp[2] = IrMoving_ave(2,IrTemp[2]);
lilac0112_1 1:5b2e1ee5e63e 175 IrTemp[3] = IrMoving_ave(3,IrTemp[3]);
lilac0112_1 1:5b2e1ee5e63e 176 IrTemp[4] = IrMoving_ave(4,IrTemp[4]);
lilac0112_1 1:5b2e1ee5e63e 177 IrTemp[5] = IrMoving_ave(5,IrTemp[5]);
lilac0112_1 0:94ced082e13a 178 */
lilac0112_1 1:5b2e1ee5e63e 179
lilac0112_1 1:5b2e1ee5e63e 180 IrSort(IrBase,IrData);
lilac0112_1 1:5b2e1ee5e63e 181
lilac0112_1 1:5b2e1ee5e63e 182 if(PingCk == 2){
lilac0112_1 1:5b2e1ee5e63e 183
lilac0112_1 1:5b2e1ee5e63e 184 Ping1.Send();
lilac0112_1 1:5b2e1ee5e63e 185 wait_ms(30);
lilac0112_1 1:5b2e1ee5e63e 186 PingData[0] = Ping1.Read_cm();
lilac0112_1 1:5b2e1ee5e63e 187 if(PingData[0]>0xFF) PingData[0]=0xFF;
lilac0112_1 1:5b2e1ee5e63e 188 }
lilac0112_1 1:5b2e1ee5e63e 189 if(PingCk == 3){
lilac0112_1 1:5b2e1ee5e63e 190
lilac0112_1 1:5b2e1ee5e63e 191 Ping2.Send();
lilac0112_1 1:5b2e1ee5e63e 192 wait_ms(30);
lilac0112_1 1:5b2e1ee5e63e 193 PingData[1] = Ping2.Read_cm();
lilac0112_1 1:5b2e1ee5e63e 194 if(PingData[1]>0xFF) PingData[1]=0xFF;
lilac0112_1 1:5b2e1ee5e63e 195 }
lilac0112_1 1:5b2e1ee5e63e 196 if(PingCk == 4){
lilac0112_1 1:5b2e1ee5e63e 197
lilac0112_1 1:5b2e1ee5e63e 198 Ping3.Send();
lilac0112_1 1:5b2e1ee5e63e 199 wait_ms(30);
lilac0112_1 1:5b2e1ee5e63e 200 PingData[2] = Ping3.Read_cm();
lilac0112_1 1:5b2e1ee5e63e 201 if(PingData[2]>0xFF) PingData[2]=0xFF;
lilac0112_1 1:5b2e1ee5e63e 202 }
lilac0112_1 0:94ced082e13a 203 if(PingCk >= PING_COUNT){
lilac0112_1 1:5b2e1ee5e63e 204
lilac0112_1 1:5b2e1ee5e63e 205 Ping4.Send();
lilac0112_1 0:94ced082e13a 206 wait_ms(30);
lilac0112_1 1:5b2e1ee5e63e 207 PingData[3] = Ping4.Read_cm();
lilac0112_1 1:5b2e1ee5e63e 208 if(PingData[3]>0xFF) PingData[3]=0xFF;
lilac0112_1 1:5b2e1ee5e63e 209
lilac0112_1 0:94ced082e13a 210 PingCk = 0;
lilac0112_1 0:94ced082e13a 211 }
lilac0112_1 1:5b2e1ee5e63e 212
lilac0112_1 1:5b2e1ee5e63e 213 Compass = hmc6352.sample();
lilac0112_1 1:5b2e1ee5e63e 214
lilac0112_1 1:5b2e1ee5e63e 215 //ex.
lilac0112_1 1:5b2e1ee5e63e 216 tx_data[0] = IrData[0];
lilac0112_1 1:5b2e1ee5e63e 217 tx_data[1] = IrData[1];
lilac0112_1 1:5b2e1ee5e63e 218 tx_data[2] = IrData[2];
lilac0112_1 1:5b2e1ee5e63e 219 tx_data[3] = PingData[0];
lilac0112_1 1:5b2e1ee5e63e 220 tx_data[4] = PingData[1];
lilac0112_1 1:5b2e1ee5e63e 221 tx_data[5] = PingData[2];
lilac0112_1 1:5b2e1ee5e63e 222 tx_data[6] = PingData[3];
lilac0112_1 1:5b2e1ee5e63e 223 tx_data[7] = (Compass & 0x00FF)>>0;
lilac0112_1 1:5b2e1ee5e63e 224 tx_data[8] = (Compass & 0xFF00)>>8;
lilac0112_1 1:5b2e1ee5e63e 225
lilac0112_1 1:5b2e1ee5e63e 226
lilac0112_1 1:5b2e1ee5e63e 227 for(int i=0; i<7; i++){
lilac0112_1 1:5b2e1ee5e63e 228 pc.printf("%d\t", tx_data[i]);
lilac0112_1 0:94ced082e13a 229 }
lilac0112_1 1:5b2e1ee5e63e 230
lilac0112_1 1:5b2e1ee5e63e 231 pc.printf("%d\t", Compass);
lilac0112_1 1:5b2e1ee5e63e 232
lilac0112_1 1:5b2e1ee5e63e 233 pc.printf("\n");
lilac0112_1 1:5b2e1ee5e63e 234
lilac0112_1 1:5b2e1ee5e63e 235
lilac0112_1 1:5b2e1ee5e63e 236 Led[0] = 0;//周回の終わり
lilac0112_1 1:5b2e1ee5e63e 237
lilac0112_1 1:5b2e1ee5e63e 238 wait_ms(1);
lilac0112_1 0:94ced082e13a 239 }
lilac0112_1 1:5b2e1ee5e63e 240 }