CatPot 2015-2016
CatPot for defence on RoboCup in 2015 winter
Dependencies: HMC6352 Ping IRM2121_2 mbed MultiSerial Watchdog
Diff: main.cpp
- Revision:
- 1:5b2e1ee5e63e
- Parent:
- 0:94ced082e13a
--- a/main.cpp Sat Jan 24 05:47:18 2015 +0000
+++ b/main.cpp Wed Mar 11 01:11:40 2015 +0000
@@ -1,82 +1,50 @@
/*
- *メインと通信するLPC1114のプログラム()
- *
- *現在は両方がmainと通信します．
- *
- *mainの通信回数が多いためそれに耐えうる速度で通信しなければならないのではという懸念。
- *試してみないとわからないが、RTOSにより常に通信のみを監視するのもありかもしれない。
- *それでダメなら詰む。
- */
-/***********************************
- *RoboCupJunior Soccer B Open 2014
- *Koshinestu progrum
- *
- * このプログラムでは以下の処理をする．
- *
- * 主にセンサーデータの作成を行う．
- *
- * IRM2121を6(12/2)個用いてボールの位置を読み取る．
- *
- * フォトダイオードを4(8/2)個用いてラインを読み取る．
- *
- * 超音波センサを2個用いて周囲の壁との距離を読み取る．
- *
- * LPC1768へI2Cを用いて各種センサーデータを送信する．
+ *main用LPC1768と通信するSensor用LPC1768のプログラム()
*
- *************************
+ *現在は2つのLPC1768でSerial通信します．
*
- *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"
-
+#include "MultiSerial.h"
+#include "HMC6352.h"
+#include "Watchdog.h"
+
#define PING_COUNT 5 //5回に1回にPing処理
-#define LINE_LEVEL (1-0.55) //しきい値を設定 0.55がしきい値
#define IR_LEVEL 400 //irのしきい値
+#define ADDRESS 0xAA//
+#define DATA_NUM 9
+
+uint8_t tx_data[DATA_NUM]={0};
-#define I2C_ADDRESS 0xAA // another (0xAC)
-
-
+IRM2121 Ir[12] = {p20,p19,p18,p17,p16,p15, p12,p11, p8,p7,p6,p5};
-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);
+Ping Ping1(p22, p21);
+Ping Ping2(p24, p23);
+Ping Ping3(p26, p25);
+Ping Ping4(p30, p29);
+
+DigitalOut Led[4] = {LED1, LED2, LED3, LED4};
-I2CSlave Mbed(dp5,dp27);//sda,scl
-Serial pc(USBTX,USBRX);
+HMC6352 hmc6352(p28, p27);
+
+MultiSerial Mbed(p13, p14);
+//MultiSerial mbed(p9, p10);
+Serial pc(USBTX,USBRX);
+
/*irm2121平均化*/
-uint8_t IrMoving_ave(uint8_t num, int data)
-{
+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] -= temp[num][9];
sum[num] += data;
-
-
+
+
temp[num][9] = temp[num][8];
temp[num][8] = temp[num][7];
temp[num][7] = temp[num][6];
@@ -87,188 +55,186 @@
temp[num][2] = temp[num][1];
temp[num][1] = temp[num][0];
temp[num][0] = data;
-
- return sum[num]/10;
+
+ return sum[num]/10;
}
-
-void IrDataSelect(int input[], uint8_t output[])
-{
+
+void IrSort(unsigned 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]) {
+ unsigned int near[2] = {1000,1000};
+ uint8_t num[2] = {12,0}, i;
+ static int sub[12]={0};
+
+ for(i = 0; i < 12; i++ ){
+ if(!input[i]){
continue;
}
- if(nigh1 < input[i]) {
- nigh2 = input[i];
- ni2 = i;
+
+ if(near[1] < input[i]+sub[i]){
continue;
}
- nigh2 = nigh1;
- ni2 = ni1;
- nigh1 = input[i];
- ni1 = i;
+ if(near[0] < input[i]+sub[i]){
+ near[1] = input[i]+sub[i];
+ num[1] = i;
+ continue;
+ }
+ near[1] = near[0];
+ num [1] = num[0];
+ near[0] = input[i]+sub[i];
+ num [0] = 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;
+
+ if(num[0] == 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)
-{
-
- Led = 1;//周回の初め
- PingCk ++;
-
+ if(num[1] == 12){
+ num[1] = 0;
+ }
+ /*
+ for(i = 0; i < 12; i++ ){
+
+ if(i == num[0]){
+
+ sub[i]++;
+ continue;
+ }
+ if(sub[i]>0){
+
+ sub[i]--;
+ }
+ }
+ */
+ output[0] = num[0]*12 + num[1];//rank1*12 + rank2
+ output[1] = near[0] /10;//rank1
+ output[2] = near[1] /10;//rank2
+
}
-
-
-
-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;
-
+
+
+
+int main() {
+
+ /*begin SetUp*/
+ Watchdog g;
+ g.Configure(1.0f);//1秒でWDT発生
+ /*end Setup*/
+
+
/*Count*/
uint8_t PingCk = 0;
-
- /* I2Cdata*/
- char SendData[9]= {0};
- char AddressData = 0;
-
- /*Debug valiable*/
- bool valid = 0,busy = 0;
-
+
+ /*Data*/
+ unsigned int PingData[4] = {0};
+ unsigned int IrBase[12] = {0};
+ uint8_t IrData[3] = {0};
+ uint16_t Compass=0;
+
+ /*Serial*/
+
+
+ Mbed.start_write();
+ Mbed.write_data(tx_data, ADDRESS, DATA_NUM);
+
+ //Continuous mode, periodic set/reset, 20Hz measurement rate.
+ hmc6352.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+
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]);
+
+ g.Service();
+
+ PingCk ++;
+
+ Led[0] = 1;//周回の初め
+
+
+
+ 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();
+ IrBase[6] = Ir[6].Read();
+ IrBase[7] = Ir[7].Read();
+ IrBase[8] = Ir[8].Read();
+ IrBase[9] = Ir[9].Read();
+ IrBase[10] = Ir[10].Read();
+ IrBase[11] = Ir[11].Read();
+
+ /*平均化
+ 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(IrMemory,IrSendData);
- /**/
- /*
+
+ IrSort(IrBase,IrData);
+
+ if(PingCk == 2){
+
+ Ping1.Send();
+ wait_ms(30);
+ PingData[0] = Ping1.Read_cm();
+ if(PingData[0]>0xFF) PingData[0]=0xFF;
+ }
+ if(PingCk == 3){
+
+ Ping2.Send();
+ wait_ms(30);
+ PingData[1] = Ping2.Read_cm();
+ if(PingData[1]>0xFF) PingData[1]=0xFF;
+ }
+ if(PingCk == 4){
+
+ Ping3.Send();
+ wait_ms(30);
+ PingData[2] = Ping3.Read_cm();
+ if(PingData[2]>0xFF) PingData[2]=0xFF;
+ }
if(PingCk >= PING_COUNT){
- Ping1.Send();
- Ping2.Send();
+
+ Ping4.Send();
wait_ms(30);
- PingMemory[0] = Ping1.Read_cm();
- PingMemory[1] = Ping2.Read_cm();
+ PingData[3] = Ping4.Read_cm();
+ if(PingData[3]>0xFF) PingData[3]=0xFF;
+
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;
-
+
+ Compass = hmc6352.sample();
+
+ //ex.
+ tx_data[0] = IrData[0];
+ tx_data[1] = IrData[1];
+ tx_data[2] = IrData[2];
+ tx_data[3] = PingData[0];
+ tx_data[4] = PingData[1];
+ tx_data[5] = PingData[2];
+ tx_data[6] = PingData[3];
+ tx_data[7] = (Compass & 0x00FF)>>0;
+ tx_data[8] = (Compass & 0xFF00)>>8;
+
+
+ for(int i=0; i<7; i++){
+ pc.printf("%d\t", tx_data[i]);
}
- */
- pc.printf("%d %d %d %d %d %d\n\r",IrTemp[0],IrTemp[1],IrTemp[2],IrTemp[3],IrTemp[4],IrTemp[5]);
-
- Led = 0;
-
+
+ pc.printf("%d\t", Compass);
+
+ pc.printf("\n");
+
+
+ Led[0] = 0;//周回の終わり
+
+ wait_ms(1);
}
-}
+}
\ No newline at end of file