右側のほうのセンサLPC1114FN28 fumiya版
Dependencies: IRM2121_2 Ping mbed HMC6352 Watchdog
Fork of CatPot_SensorRight by
main.cpp
- Committer:
- ryuna
- Date:
- 2015-02-26
- Revision:
- 6:f64989ad4746
- Parent:
- 5:a41471351ae3
- Child:
- 8:52f8023cdb8f
- Child:
- 10:2ebbea0bcc6e
File content as of revision 6:f64989ad4746:
/* *メインと通信するLPC1114のプログラム() * *現在は両方がmainと通信します. * */ #include "mbed.h" #define SYSAHBCLKDIV_Val 0x00000001 // Reset: 0x001 #define CLOCK_SETUP 1 // デフォルト=1 #define SYSPLLCTRL_Val 0x00000023 // デフォルト=23 Reset: 0x000 #define SYSPLLCLKSEL_Val 0x00000001 // デフォルト=0 Reset: 0x000 #define MAINCLKSEL_Val 0x00000003 // デフォルト=3 Reset: 0x000 #include "IRM2121.h" #include "Ping.h" #include "WatchDogTimer.h" #define PING_COUNT 5 //5回に1回にPing処理 #define IR_LEVEL 400 //irのしきい値 #define I2C_ADDRESS 0xAA // IRM2121 Ir[6] = {dp17,dp18,dp24,dp25,dp26,dp28}; Ping Ping1(dp1,dp2); Ping Ping2(dp4,dp6); DigitalOut Led(LED1); I2CSlave Mbed(dp5,dp27);//sda,scl Serial pc(USBTX,USBRX); /*irm2121平均化*/ uint8_t IrMoving_ave(uint8_t num, int data){ static unsigned int sum[10]; static unsigned temp[6][10]; sum[num] -= temp[num][9]; sum[num] += data; temp[num][9] = temp[num][8]; temp[num][8] = temp[num][7]; temp[num][7] = temp[num][6]; temp[num][6] = temp[num][5]; temp[num][5] = temp[num][4]; temp[num][4] = temp[num][3]; temp[num][3] = temp[num][2]; temp[num][2] = temp[num][1]; temp[num][1] = temp[num][0]; temp[num][0] = data; return sum[num]/10; } void IrDataSelect(int input[], uint8_t output[]){ /* *1,2位の値とその場所の番号をアウトプットに格納する. *値が255以上だとボールを検知してないことになる。 */ uint8_t nigh1 = 255, nigh2 = 255; uint8_t ni1=12, ni2 = 12, i; for(i = 6; i > 0; i-- ){ if(nigh2 < input[i]){ continue; } if(nigh1 < input[i]){ nigh2 = input[i]; ni2 = i; continue; } nigh2 = nigh1; ni2 = ni1; nigh1 = input[i]; ni1 = i; } if(ni1 == 12){//not found output[0] = 255; output[1] = 255; output[2] = 255; return; } output[0] = ni1*6 + ni2; output[1] = nigh1; output[2] = nigh2; } /*周回の初期動作*/ void ResetSet(uint8_t *PingCk, uint8_t *IrCk){ Led = 1;//周回の初め //PingCk ++; } int main() { /*begin SetUp*/ //default Mbed.frequency(); Mbed.address(I2C_ADDRESS); Watchdog g; g.kick(1.0f);//1秒でWDT発生 /*end Setup*/ /*Count*/ uint8_t PingCk = 0; uint8_t IrCk = 0; /*Data*/ uint8_t PingData[2] = {0}; unsigned long IrTemp[6] = {0}; //仮置き場 uint8_t IrData[3]; // irのデータ unsigned int ir__data[6] = {0}; /*State(operate_value etc...)*/ int I2CState; /* I2Cdata*/ char SendData[9]={0}; char AddressData[1] = {0}; /*Debug valiable*/ bool valid = 0,busy = 0; while(1) { ResetSet(&PingCk,&IrCk); /* IrData[0] = (IR_LEVEL - Ir[0].Read())*255/400; IrData[1] = (IR_LEVEL - Ir[1].Read())*255/400; IrData[2] = (IR_LEVEL - Ir[2].Read())*255/400; IrTemp[3] = (IR_LEVEL - Ir[3].Read())*255/400; IrTemp[4] = (IR_LEVEL - Ir[4].Read())*255/400; IrTemp[5] = (IR_LEVEL - Ir[5].Read())*255/400; */ /*平均化 IrTemp[0] = IrMoving_ave(0,IrTemp[0]); IrTemp[1] = IrMoving_ave(1,IrTemp[1]); IrTemp[2] = IrMoving_ave(2,IrTemp[2]); IrTemp[3] = IrMoving_ave(3,IrTemp[3]); IrTemp[4] = IrMoving_ave(4,IrTemp[4]); IrTemp[5] = IrMoving_ave(5,IrTemp[5]); */ //IrDataSelect(IrTemp,IrData); /* if(PingCk >= PING_COUNT){ Ping1.Send(); Ping2.Send(); wait_ms(30); PingData[0] = Ping1.Read_cm(); PingData[1] = Ping2.Read_cm(); PingCk = 0; } */ ir__data[0] = Ir[0].Read(); ir__data[1] = Ir[1].Read(); ir__data[2] = Ir[2].Read(); ir__data[3] = Ir[3].Read(); ir__data[4] = Ir[4].Read(); ir__data[5] = Ir[5].Read(); I2CState = Mbed.receive(); switch(I2CState){ case I2CSlave::ReadAddressed: if(AddressData[0]){ switch(AddressData[0]){ case 0x01://ping SendData[0] = (char)PingData[0]; SendData[1] = (char)PingData[1]; Mbed.write(SendData, 2); break; case 0x02://IR SendData[0] = (char)IrData[0]; SendData[1] = (char)IrData[1]; SendData[2] = (char)IrData[2]; Mbed.write(SendData, 3); break; default: break; } } AddressData[0] = 0; break; case I2CSlave::WriteGeneral: Mbed.read(AddressData,1); break; case I2CSlave::WriteAddressed: break; } Led = 0; pc.printf("%d \t %d \t %d ",ir__data[0],ir__data[1],ir__data[2]); pc.printf("%d \t %d \t %d \n",ir__data[3],ir__data[4],ir__data[5]); /* IrCk ++; if(IrCk > 5){ IrCk = 0; } */ //wait(0.1); } }