CatPot 2015-2016 / Mbed 2 deprecated CatPot_SensorRight_F

右側のほうのセンサLPC1114FN28 fumiya版

Dependencies:   IRM2121_2 Ping mbed HMC6352 Watchdog

Fork of CatPot_SensorRight by CatPot 2015-2016

main.cpp@16:83721dac1049, 2015-03-11 (annotated)

Committer:
ryuna
Date:
Wed Mar 11 01:03:39 2015 +0000
Revision:
16:83721dac1049
Parent:
15:3d27e435e014 
without multiserial;

Who changed what in which revision?

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