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@4:bac44e117fce, 2015-02-23 (annotated)

Committer:
ryuna
Date:
Mon Feb 23 09:39:33 2015 +0000
Revision:
4:bac44e117fce
Parent:
2:099a6c81a02e
Child:
5:a41471351ae3
debug????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryuna 0:182437c14483 1 /*
ryuna 1:8e9ee100d854 2 *メインと通信するLPC1114のプログラム()
ryuna 1:8e9ee100d854 3 *
ryuna 1:8e9ee100d854 4 *現在は両方がmainと通信します．
ryuna 1:8e9ee100d854 5 *
ryuna 1:8e9ee100d854 6 */
ryuna 0:182437c14483 7
ryuna 0:182437c14483 8 #include "mbed.h"
ryuna 2:099a6c81a02e 9
ryuna 0:182437c14483 10 #define SYSAHBCLKDIV_Val 0x00000001 // Reset: 0x001
ryuna 0:182437c14483 11 #define CLOCK_SETUP 1 // デフォルト=1
ryuna 0:182437c14483 12 #define SYSPLLCTRL_Val 0x00000023 // デフォルト=23 Reset: 0x000
ryuna 0:182437c14483 13 #define SYSPLLCLKSEL_Val 0x00000001 // デフォルト=0 Reset: 0x000
ryuna 0:182437c14483 14 #define MAINCLKSEL_Val 0x00000003 // デフォルト=3 Reset: 0x000
ryuna 2:099a6c81a02e 15
ryuna 0:182437c14483 16 #include "IRM2121.h"
ryuna 0:182437c14483 17 #include "Ping.h"
ryuna 0:182437c14483 18
ryuna 0:182437c14483 19 #define PING_COUNT 5 //5回に1回にPing処理
ryuna 1:8e9ee100d854 20 #define IR_LEVEL 400 //irのしきい値
ryuna 2:099a6c81a02e 21 #define I2C_ADDRESS 0xAA //
ryuna 1:8e9ee100d854 22
ryuna 1:8e9ee100d854 23
ryuna 1:8e9ee100d854 24
ryuna 2:099a6c81a02e 25 IRM2121 Ir[6] = {dp17,dp18,dp24,dp25,dp26,dp28};
ryuna 0:182437c14483 26 Ping Ping1(dp1,dp2);
ryuna 2:099a6c81a02e 27 Ping Ping2(dp4,dp6);
ryuna 0:182437c14483 28 DigitalOut Led(LED1);
ryuna 0:182437c14483 29
ryuna 0:182437c14483 30 I2CSlave Mbed(dp5,dp27);//sda,scl
ryuna 0:182437c14483 31 Serial pc(USBTX,USBRX);
ryuna 0:182437c14483 32
ryuna 1:8e9ee100d854 33 /*irm2121平均化*/
ryuna 1:8e9ee100d854 34 uint8_t IrMoving_ave(uint8_t num, int data){
ryuna 1:8e9ee100d854 35 static unsigned int sum[10];
ryuna 1:8e9ee100d854 36 static unsigned temp[6][10];
ryuna 0:182437c14483 37
ryuna 1:8e9ee100d854 38 sum[num] -= temp[num][9];
ryuna 0:182437c14483 39 sum[num] += data;
ryuna 0:182437c14483 40
ryuna 1:8e9ee100d854 41
ryuna 1:8e9ee100d854 42 temp[num][9] = temp[num][8];
ryuna 1:8e9ee100d854 43 temp[num][8] = temp[num][7];
ryuna 1:8e9ee100d854 44 temp[num][7] = temp[num][6];
ryuna 1:8e9ee100d854 45 temp[num][6] = temp[num][5];
ryuna 1:8e9ee100d854 46 temp[num][5] = temp[num][4];
ryuna 0:182437c14483 47 temp[num][4] = temp[num][3];
ryuna 0:182437c14483 48 temp[num][3] = temp[num][2];
ryuna 0:182437c14483 49 temp[num][2] = temp[num][1];
ryuna 0:182437c14483 50 temp[num][1] = temp[num][0];
ryuna 0:182437c14483 51 temp[num][0] = data;
ryuna 0:182437c14483 52
ryuna 1:8e9ee100d854 53 return sum[num]/10;
ryuna 0:182437c14483 54 }
ryuna 0:182437c14483 55
ryuna 1:8e9ee100d854 56 void IrDataSelect(int input[], uint8_t output[]){
ryuna 1:8e9ee100d854 57 /*
ryuna 1:8e9ee100d854 58 *1,2位の値とその場所の番号をアウトプットに格納する.
ryuna 1:8e9ee100d854 59 *値が255以上だとボールを検知してないことになる。
ryuna 1:8e9ee100d854 60 */
ryuna 1:8e9ee100d854 61 uint8_t nigh1 = 255, nigh2 = 255;
ryuna 1:8e9ee100d854 62 uint8_t ni1=12, ni2 = 12, i;
ryuna 1:8e9ee100d854 63
ryuna 1:8e9ee100d854 64 for(i = 6; i > 0; i-- ){
ryuna 1:8e9ee100d854 65 if(nigh2 < input[i]){
ryuna 1:8e9ee100d854 66 continue;
ryuna 1:8e9ee100d854 67 }
ryuna 1:8e9ee100d854 68 if(nigh1 < input[i]){
ryuna 1:8e9ee100d854 69 nigh2 = input[i];
ryuna 1:8e9ee100d854 70 ni2 = i;
ryuna 1:8e9ee100d854 71 continue;
ryuna 1:8e9ee100d854 72 }
ryuna 1:8e9ee100d854 73 nigh2 = nigh1;
ryuna 1:8e9ee100d854 74 ni2 = ni1;
ryuna 1:8e9ee100d854 75 nigh1 = input[i];
ryuna 1:8e9ee100d854 76 ni1 = i;
ryuna 1:8e9ee100d854 77 }
ryuna 2:099a6c81a02e 78
ryuna 2:099a6c81a02e 79 if(ni1 == 12){//not found
ryuna 2:099a6c81a02e 80 output[0] = 255;
ryuna 1:8e9ee100d854 81 output[1] = 255;
ryuna 1:8e9ee100d854 82 output[2] = 255;
ryuna 1:8e9ee100d854 83 return;
ryuna 1:8e9ee100d854 84 }
ryuna 1:8e9ee100d854 85
ryuna 1:8e9ee100d854 86 output[0] = ni1*6 + ni2;
ryuna 1:8e9ee100d854 87 output[1] = nigh1;
ryuna 1:8e9ee100d854 88 output[2] = nigh2;
ryuna 1:8e9ee100d854 89
ryuna 1:8e9ee100d854 90 }
ryuna 1:8e9ee100d854 91 /*周回の初期動作*/
ryuna 0:182437c14483 92 void ResetSet(uint8_t *PingCk){
ryuna 0:182437c14483 93
ryuna 0:182437c14483 94 Led = 1;//周回の初め
ryuna 0:182437c14483 95 PingCk ++;
ryuna 0:182437c14483 96
ryuna 0:182437c14483 97 }
ryuna 0:182437c14483 98
ryuna 0:182437c14483 99
ryuna 0:182437c14483 100
ryuna 0:182437c14483 101 int main() {
ryuna 0:182437c14483 102
ryuna 2:099a6c81a02e 103 /*begin SetUp*/
ryuna 0:182437c14483 104 //default Mbed.frequency();
ryuna 1:8e9ee100d854 105 Mbed.address(I2C_ADDRESS);
ryuna 2:099a6c81a02e 106
ryuna 2:099a6c81a02e 107 /*end Setup*/
ryuna 2:099a6c81a02e 108
ryuna 2:099a6c81a02e 109
ryuna 2:099a6c81a02e 110 /*Count*/
ryuna 2:099a6c81a02e 111 uint8_t PingCk = 0;
ryuna 2:099a6c81a02e 112
ryuna 0:182437c14483 113 /*Data*/
ryuna 2:099a6c81a02e 114 uint8_t PingData[2] = {0};
ryuna 2:099a6c81a02e 115 int IrTemp[6] = {0}; //仮置き場
ryuna 2:099a6c81a02e 116 uint8_t IrData[3]; // irのデータ
ryuna 0:182437c14483 117
ryuna 1:8e9ee100d854 118 /*State(operate_value etc...)*/
ryuna 1:8e9ee100d854 119 int I2CState;
ryuna 1:8e9ee100d854 120
ryuna 0:182437c14483 121 /* I2Cdata*/
ryuna 0:182437c14483 122 char SendData[9]={0};
ryuna 1:8e9ee100d854 123 char AddressData = 0;
ryuna 1:8e9ee100d854 124
ryuna 1:8e9ee100d854 125 /*Debug valiable*/
ryuna 1:8e9ee100d854 126 bool valid = 0,busy = 0;
ryuna 0:182437c14483 127
ryuna 0:182437c14483 128 while(1) {
ryuna 0:182437c14483 129
ryuna 0:182437c14483 130 ResetSet(&PingCk);
ryuna 2:099a6c81a02e 131
ryuna 0:182437c14483 132 Ir[0].Set();
ryuna 0:182437c14483 133 Ir[1].Set();
ryuna 0:182437c14483 134 Ir[2].Set();
ryuna 0:182437c14483 135 Ir[3].Set();
ryuna 0:182437c14483 136 Ir[4].Set();
ryuna 2:099a6c81a02e 137 Ir[5].Set();
ryuna 1:8e9ee100d854 138
ryuna 2:099a6c81a02e 139 wait_ms(80);//wait値は最適値を探す必要がある．
ryuna 2:099a6c81a02e 140 /*8bit*/
ryuna 0:182437c14483 141
ryuna 2:099a6c81a02e 142 IrTemp[0] = (IR_LEVEL - Ir[0].Read())*255/400;
ryuna 2:099a6c81a02e 143 IrTemp[1] = (IR_LEVEL - Ir[1].Read())*255/400;
ryuna 2:099a6c81a02e 144 IrTemp[2] = (IR_LEVEL - Ir[2].Read())*255/400;
ryuna 2:099a6c81a02e 145 IrTemp[3] = (IR_LEVEL - Ir[3].Read())*255/400;
ryuna 2:099a6c81a02e 146 IrTemp[4] = (IR_LEVEL - Ir[4].Read())*255/400;
ryuna 2:099a6c81a02e 147 IrTemp[5] = (IR_LEVEL - Ir[5].Read())*255/400;
ryuna 0:182437c14483 148
ryuna 2:099a6c81a02e 149 /*
ryuna 1:8e9ee100d854 150 Ir[0].ReturnVB(&valid,&busy);
ryuna 1:8e9ee100d854 151 pc.printf("valid %d, busy %d \n\r",valid, busy);
ryuna 1:8e9ee100d854 152 */
ryuna 0:182437c14483 153
ryuna 2:099a6c81a02e 154 /*平均化
ryuna 2:099a6c81a02e 155 IrTemp[0] = IrMoving_ave(0,IrTemp[0]);
ryuna 2:099a6c81a02e 156 IrTemp[1] = IrMoving_ave(1,IrTemp[1]);
ryuna 2:099a6c81a02e 157 IrTemp[2] = IrMoving_ave(2,IrTemp[2]);
ryuna 2:099a6c81a02e 158 IrTemp[3] = IrMoving_ave(3,IrTemp[3]);
ryuna 2:099a6c81a02e 159 IrTemp[4] = IrMoving_ave(4,IrTemp[4]);
ryuna 2:099a6c81a02e 160 IrTemp[5] = IrMoving_ave(5,IrTemp[5]);
ryuna 2:099a6c81a02e 161 */
ryuna 2:099a6c81a02e 162
ryuna 2:099a6c81a02e 163
ryuna 2:099a6c81a02e 164 IrDataSelect(IrTemp,IrData);
ryuna 2:099a6c81a02e 165
ryuna 1:8e9ee100d854 166 /*
ryuna 0:182437c14483 167 if(PingCk >= PING_COUNT){
ryuna 0:182437c14483 168 Ping1.Send();
ryuna 0:182437c14483 169 Ping2.Send();
ryuna 0:182437c14483 170 wait_ms(30);
ryuna 2:099a6c81a02e 171 PingData[0] = Ping1.Read_cm();
ryuna 2:099a6c81a02e 172 PingData[1] = Ping2.Read_cm();
ryuna 0:182437c14483 173 PingCk = 0;
ryuna 0:182437c14483 174 }
ryuna 1:8e9ee100d854 175 */
ryuna 4:bac44e117fce 176
ryuna 1:8e9ee100d854 177 I2CState = Mbed.receive();
ryuna 1:8e9ee100d854 178 switch(I2CState){
ryuna 0:182437c14483 179 case I2CSlave::ReadAddressed:
ryuna 2:099a6c81a02e 180
ryuna 4:bac44e117fce 181
ryuna 4:bac44e117fce 182 Mbed.write(SendData, 9);
ryuna 0:182437c14483 183 break;
ryuna 1:8e9ee100d854 184 case I2CSlave::WriteGeneral:
ryuna 4:bac44e117fce 185 Mbed.read(SendData, 9);
ryuna 0:182437c14483 186 break;
ryuna 1:8e9ee100d854 187 case I2CSlave::WriteAddressed:
ryuna 0:182437c14483 188 break;
ryuna 0:182437c14483 189
ryuna 0:182437c14483 190 }
ryuna 4:bac44e117fce 191
ryuna 0:182437c14483 192 Led = 0;
ryuna 0:182437c14483 193
ryuna 2:099a6c81a02e 194 //wait(0.5);
ryuna 0:182437c14483 195 }
ryuna 0:182437c14483 196 }