CatPot 2015-2016
CatPot for defence on RoboCup in 2015 winter
Dependencies: HMC6352 Ping IRM2121_2 mbed MultiSerial Watchdog
Diff: main.cpp
- Revision:
- 0:94ced082e13a
- Child:
- 1:5b2e1ee5e63e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Jan 24 05:47:18 2015 +0000
@@ -0,0 +1,274 @@
+/*
+ *メインと通信するLPC1114のプログラム()
+ *
+ *現在は両方がmainと通信します．
+ *
+ *mainの通信回数が多いためそれに耐えうる速度で通信しなければならないのではという懸念。
+ *試してみないとわからないが、RTOSにより常に通信のみを監視するのもありかもしれない。
+ *それでダメなら詰む。
+ */
+/***********************************
+ *RoboCupJunior Soccer B Open 2014
+ *Koshinestu progrum
+ *
+ * このプログラムでは以下の処理をする．
+ *
+ * 主にセンサーデータの作成を行う．
+ *
+ * IRM2121を6(12/2)個用いてボールの位置を読み取る．
+ *
+ * フォトダイオードを4(8/2)個用いてラインを読み取る．
+ *
+ * 超音波センサを2個用いて周囲の壁との距離を読み取る．
+ *
+ * LPC1768へI2Cを用いて各種センサーデータを送信する．
+ *
+ *************************
+ *
+ *Pin Map
+ *
+ * dp17,dp18,dp24,dp25,dp26,NC >> IRM2121
+ *
+ * dp1,dp2 >> Ping1
+ *
+ * dp4,dp6 >> Ping2
+ *
+ * dp9,dp10,dp11,dp13 >> PhotoDiode
+ *
+ * dp5,dp27 >> I2C >> LPC1768
+ *
+ ******************************/
+
+#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"
+
+#define PING_COUNT 5 //5回に1回にPing処理
+#define LINE_LEVEL (1-0.55) //しきい値を設定 0.55がしきい値
+#define IR_LEVEL 400 //irのしきい値
+
+#define I2C_ADDRESS 0xAA // another (0xAC)
+
+
+
+IRM2121 Ir[6] = {dp17,dp18,dp24,dp25,dp26,NC};
+Ping Ping1(dp1,dp2);
+Ping Ping2(dp4,dp6);
+AnalogIn Line[4] = {dp9,dp10,dp11,dp13};
+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;
+ }
+ /*
+ for(i = 0; i < 6; i++){
+
+ if(nigh1 > input[i]){
+ nigh1 = input[i];
+ ni1 = i;
+ }
+ if((nigh1<input[i])&&(nigh2>input[i])){
+ nigh2 = input[i];
+ ni2 = i;
+ }
+ }*/
+ if(ni1 == 12) { //ボールがないとき
+ output[0] = 0;
+ output[1] = 255;
+ output[2] = 255;
+ return;
+ }
+
+ output[0] = ni1*6 + ni2;
+ output[1] = nigh1;
+ output[2] = nigh2;
+
+}
+/*周回の初期動作*/
+void ResetSet(uint8_t *PingCk)
+{
+
+ Led = 1;//周回の初め
+ PingCk ++;
+
+}
+
+
+
+int main()
+{
+
+ //default Mbed.frequency();
+ Mbed.address(I2C_ADDRESS);
+ /*Data*/
+ int IrMemory[6] = {0};//IRのデータ保管
+ uint8_t PingMemory[2] = {0};
+ unsigned int LineMemory[4] = {0};
+
+ int IrTemp[6] = {0};//仮置き場
+ uint8_t IrSendData[3];//[0]:どのirを指すのかアドレスで
+
+ /*Flag*/
+ uint8_t LineFlag;//配置 - - - - Line[3] Line[2] Line[1] Line[0]
+
+ /*State(operate_value etc...)*/
+ int I2CState;
+
+ /*Count*/
+ uint8_t PingCk = 0;
+
+ /* I2Cdata*/
+ char SendData[9]= {0};
+ char AddressData = 0;
+
+ /*Debug valiable*/
+ bool valid = 0,busy = 0;
+
+ while(1) {
+
+ ResetSet(&PingCk);
+ Ir[0].Set();
+ Ir[1].Set();
+ Ir[2].Set();
+ Ir[3].Set();
+ Ir[4].Set();
+ //Ir[5].Set();
+
+ wait_ms(50);//wait値は最適値を探す必要がある．
+
+ IrTemp[0] = IR_LEVEL - Ir[0].Read();
+ IrTemp[1] = IR_LEVEL - Ir[1].Read();
+ IrTemp[2] = IR_LEVEL - Ir[2].Read();
+ IrTemp[3] = IR_LEVEL - Ir[3].Read();
+ IrTemp[4] = IR_LEVEL - Ir[4].Read();
+ //IrTemp[5] = IR_LEVEL - Ir[5].Read();
+
+ Ir[0].ReturnVB(&valid,&busy);
+ pc.printf("valid %d, busy %d \n\r",valid, busy);
+ /*
+ IrMemory[0] = IrMoving_ave(0,IrTemp[0]);
+ IrMemory[1] = IrMoving_ave(1,IrTemp[1]);
+ IrMemory[2] = IrMoving_ave(2,IrTemp[2]);
+ IrMemory[3] = IrMoving_ave(3,IrTemp[3]);
+ IrMemory[4] = IrMoving_ave(4,IrTemp[4]);
+ //IrMemory[5] = IrMoving_ave(5,IrTemp[5]);
+ */
+
+ /*小さい(大きい)二つだけでいい気がする．値をみて小さかったらそうしょう*/
+ /**/
+ //IrDataSelect(IrMemory,IrSendData);
+ /**/
+ /*
+ if(PingCk >= PING_COUNT){
+ Ping1.Send();
+ Ping2.Send();
+ wait_ms(30);
+ PingMemory[0] = Ping1.Read_cm();
+ PingMemory[1] = Ping2.Read_cm();
+ PingCk = 0;
+ }
+ */
+ LineMemory[0] = Line[0];
+ LineMemory[1] = Line[1];
+ LineMemory[2] = Line[2];
+ LineMemory[3] = Line[3];
+
+ LineFlag = (((int)(LineMemory[3]+LINE_LEVEL)) | ((int)(LineMemory[2]+LINE_LEVEL)))<<1 +
+ (((int)(LineMemory[1]+LINE_LEVEL)) | ((int)(LineMemory[0]+LINE_LEVEL))); //まとめてる
+ /*
+ I2CState = Mbed.receive();
+
+ switch(I2CState){
+ case I2CSlave::ReadAddressed:
+ if(AddressData){
+ switch(AddressData){
+ case 0x01://IR
+ SendData[0] = (char)IrSendData[0];
+ SendData[1] = (char)IrSendData[1];
+ SendData[2] = (char)IrSendData[2];
+ Mbed.write(SendData, 3);
+ break;
+
+ case 0x02://ping
+ SendData[0] = (char)PingMemory[0];
+ SendData[1] = (char)PingMemory[1];
+ Mbed.write(SendData, 2);
+ break;
+ default:
+ break;
+ }
+ }else{//line
+ SendData[0] = (char)LineFlag;
+ Mbed.write(SendData, 1);
+ }
+ AddressData = 0;
+ break;
+ case I2CSlave::WriteGeneral:
+ AddressData = Mbed.read();
+ break;
+ case I2CSlave::WriteAddressed:
+ AddressData = Mbed.read();
+ break;
+
+ }
+ */
+ pc.printf("%d %d %d %d %d %d\n\r",IrTemp[0],IrTemp[1],IrTemp[2],IrTemp[3],IrTemp[4],IrTemp[5]);
+
+ Led = 0;
+
+ }
+}