CatPot 2015-2016
右側のほうのセンサLPC1114FN28 fumiya版
Dependencies: IRM2121_2 Ping mbed HMC6352 Watchdog
Fork of
CatPot_SensorRight
by 
CatPot 2015-2016
Revision 11:735a59ebc63f, committed 2015-03-05
- Comitter:
- lilac0112_1
- Date:
- Thu Mar 05 08:26:28 2015 +0000
- Parent:
- 9:c992dfaa77ce
- Child:
- 12:4bf564955c53
- Commit message:
- I2CtoSerial
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
| sub.txt | Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Mar 02 23:53:34 2015 +0000
+++ b/main.cpp Thu Mar 05 08:26:28 2015 +0000
@@ -4,47 +4,69 @@
*現在は両方が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 //
-
-
-
+#define I2C_ADDRESS 0xAA//
+
+
+
IRM2121 Ir[6] = {dp17,dp18,dp24,dp25,dp26,dp28};
-
-Ping Ping1(dp1,dp2);
-Ping Ping2(dp4,dp6);
+
+Ping Ping1(dp2,dp1);
+Ping Ping2(dp6,dp4);
DigitalOut Led(LED1);
-
+
I2CSlave Mbed(dp5,dp27);//sda,scl
Serial pc(USBTX,USBRX);
-
-void IrSort(int input[], uint8_t output[]){
+/*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 IrSort(unsigned int input[], char output[]){
+
/*
*1,2位の値とその場所の番号をアウトプットに格納する.
- *値が0だとボールを検知してないことになる。
+ *値が255以上だとボールを検知してないことになる。
*/
- int near[] = {1000, 1000};//rank1, rank2
- uint8_t position[] = {12, 0}, i;
+ unsigned int near[2] = {1000,1000};
+ uint8_t num[2] = {12,0}, i;
- for(i=0; i < 6; i++){
-
- if(input[i] == 0){
+ for(i = 0; i < 6; i++ ){
+ if(!input[i]){
continue;
}
@@ -53,30 +75,32 @@
}
if(near[0] < input[i]){
near[1] = input[i];
- position[1] = i;
+ num[1] = i;
continue;
}
-
near[1] = near[0];
- position[1] = position[0];
+ num [1] = num[0];
near[0] = input[i];
- position[0] = i;
+ num [0] = i;
}
-
- if(position[0] == 12){//not found
+
+ if(num[0] == 12){//not found
output[0] = 125;
output[1] = 125;
output[2] = 125;
return;
}
-
- output[0] = position[0]*8 | position[1];
- output[1] = near[0]/10;
- output[2] = near[1]/10;
+ if(num[1] == 12){
+ num[1] = 0;
+ }
+ output[0] = num[0]*6 + num[1];
+ output[1] = near[0] /10;
+ output[2] = near[1] /10;
}
-
-
+
+
+
int main() {
/*begin SetUp*/
@@ -86,105 +110,124 @@
g.kick(1.0f);//1秒でWDT発生
/*end Setup*/
+
/*Count*/
uint8_t PingCk = 0;
- int i;
/*Data*/
uint8_t PingData[2] = {0};
- int ir__data[6] = {0};
- uint8_t ir_data[6] = {0};
+ char IrData[3] = {0};
+ unsigned int IrBase[6] = {0};
/*State(operate_value etc...)*/
int I2CState;
/* I2Cdata*/
- char SendData[6]={0};
+ char SendData[9]={0};
char AddressData[1] = {0};
-
-
+
+
while(1) {
-
- /*Ping(圧縮1/1)*/
- /*if(PingCk >= PING_COUNT){
+ Led = 1;//周回の初め
+
+ //通信していないとき
+ if(!AddressData[0])
+ {
+ PingCk ++;
+
+ IrBase[0] = Ir[0].Read();
+ IrBase[1] = Ir[1].Read();
+ IrBase[2] = Ir[2].Read();
+ IrBase[3] = Ir[3].Read();
+ IrBase[4] = Ir[4].Read();
+ IrBase[5] = Ir[5].Read();
+
+ /*平均化
- Ping1.Send();
- wait_ms(30);
- PingData[0] = Ping1.Read_cm();
+ 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]);
+
+ */
+
+
+ IrSort(IrBase,IrData);
- if(I2C_ADDRESS==0xAA){
+ if(PingCk == 2){
+
+ Ping1.Send();
+ wait_ms(30);
+ PingData[0] = Ping1.Read_cm();
+ if(PingData[0]>125) PingData[0]=125;
+ }
+
+
+ if(PingCk >= PING_COUNT){
+
Ping2.Send();
wait_ms(30);
PingData[1] = Ping2.Read_cm();
- }
+ if(PingData[1]>125) PingData[1]=125;
+ PingCk = 0;
+
+ //pc.printf("%d \t %d \t %d \n",PingData[0],PingData[1],IrBase[0]);
- for(i=0; i < 2; i++){
-
- if(PingData[i] > 125) PingData[i] = 125;
-
}
- PingCk = 0;
- }*/
- /*Ir(圧縮1/10)*/
- for(i=0; i < 6; i++){
-
- ir__data[i] = Ir[i].Read();
-
- if(ir__data[i] > 1250) ir__data[i] = 1250;
}
-
- IrSort(ir__data, ir_data);
-
-
-
I2CState = Mbed.receive();
- switch(I2CState){
-
+ 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 0x00:
- case 0x02://IR
-
- SendData[0] = (char)ir_data[0];
- SendData[1] = (char)ir_data[1];
- SendData[2] = (char)ir_data[2];
-
- /*for(i=0; i<3; i++){
- pc.printf("%d ", (char)SendData[i]);
- }
- pc.printf("\n");*/
-
- Mbed.write(SendData, 3);
-
- break;
- default:
- break;
- }
- }
+ 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://BALL
+ 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;
+ }else{
+ SendData[0] = (char)IrData[0];
+ SendData[1] = (char)IrData[1];
+ SendData[2] = (char)IrData[2];
+ SendData[3] = (char)PingData[0];
+ SendData[4] = (char)PingData[1];
+ Mbed.write(SendData, 5);
+ }
- AddressData[0] = 0x00;
- break;
- case I2CSlave::WriteGeneral:
- Mbed.read(AddressData,1);
break;
case I2CSlave::WriteAddressed:
Mbed.read(AddressData,1);
break;
+
+ case I2CSlave::WriteGeneral:
+
+ break;
}
Led = 0;
+ /*
+ pc.printf("%d \t %d \t %d ",IrBase[0],IrBase[1],IrBase[2]);
- PingCk ++;
-
+ pc.printf("\t %d \t %d \t %d \n",IrBase[3],IrBase[4],IrBase[5]);
+ */
+
+ //wait(0.1);
}
}
\ No newline at end of file
--- a/sub.txt Mon Mar 02 23:53:34 2015 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,66 +0,0 @@
-
-/*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 ++;
-
-}
\ No newline at end of file