CatPot_SensorRight_T - CatPot for defence on RoboCup in 2015 winter

CatPot 2015-2016 » Code » CatPot_SensorRight_T

CatPot 2015-2016 / Mbed 2 deprecated CatPot_SensorRight_T

CatPot for defence on RoboCup in 2015 winter

Dependencies: HMC6352 Ping IRM2121_2 mbed MultiSerial Watchdog

main.cpp@0:94ced082e13a, 2015-01-24 (annotated)

Committer:: lilac0112_1
Date:: Sat Jan 24 05:47:18 2015 +0000
Revision:: 0:94ced082e13a
Child:: 1:5b2e1ee5e63e

Who changed what in which revision?

User	Revision	Line number	New contents of line
lilac0112_1	0:94ced082e13a	1	/*
lilac0112_1	0:94ced082e13a	2	*メインと通信するLPC1114のプログラム()
lilac0112_1	0:94ced082e13a	3	*
lilac0112_1	0:94ced082e13a	4	*現在は両方がmainと通信します．
lilac0112_1	0:94ced082e13a	5	*
lilac0112_1	0:94ced082e13a	6	*mainの通信回数が多いためそれに耐えうる速度で通信しなければならないのではという懸念。
lilac0112_1	0:94ced082e13a	7	*試してみないとわからないが、RTOSにより常に通信のみを監視するのもありかもしれない。
lilac0112_1	0:94ced082e13a	8	*それでダメなら詰む。
lilac0112_1	0:94ced082e13a	9	*/
lilac0112_1	0:94ced082e13a	10	/***********************************
lilac0112_1	0:94ced082e13a	11	*RoboCupJunior Soccer B Open 2014
lilac0112_1	0:94ced082e13a	12	*Koshinestu progrum
lilac0112_1	0:94ced082e13a	13	*
lilac0112_1	0:94ced082e13a	14	* このプログラムでは以下の処理をする．
lilac0112_1	0:94ced082e13a	15	*
lilac0112_1	0:94ced082e13a	16	*　主にセンサーデータの作成を行う．
lilac0112_1	0:94ced082e13a	17	*
lilac0112_1	0:94ced082e13a	18	*　IRM2121を6(12/2)個用いてボールの位置を読み取る．
lilac0112_1	0:94ced082e13a	19	*
lilac0112_1	0:94ced082e13a	20	*　フォトダイオードを4(8/2)個用いてラインを読み取る．
lilac0112_1	0:94ced082e13a	21	*
lilac0112_1	0:94ced082e13a	22	*　超音波センサを2個用いて周囲の壁との距離を読み取る．
lilac0112_1	0:94ced082e13a	23	*
lilac0112_1	0:94ced082e13a	24	* LPC1768へI2Cを用いて各種センサーデータを送信する．
lilac0112_1	0:94ced082e13a	25	*
lilac0112_1	0:94ced082e13a	26	*************************
lilac0112_1	0:94ced082e13a	27	*
lilac0112_1	0:94ced082e13a	28	*Pin Map
lilac0112_1	0:94ced082e13a	29	*
lilac0112_1	0:94ced082e13a	30	* dp17,dp18,dp24,dp25,dp26,NC >> IRM2121
lilac0112_1	0:94ced082e13a	31	*
lilac0112_1	0:94ced082e13a	32	* dp1,dp2 >> Ping1
lilac0112_1	0:94ced082e13a	33	*
lilac0112_1	0:94ced082e13a	34	* dp4,dp6 >> Ping2
lilac0112_1	0:94ced082e13a	35	*
lilac0112_1	0:94ced082e13a	36	* dp9,dp10,dp11,dp13 >> PhotoDiode
lilac0112_1	0:94ced082e13a	37	*
lilac0112_1	0:94ced082e13a	38	* dp5,dp27 >> I2C >> LPC1768
lilac0112_1	0:94ced082e13a	39	*
lilac0112_1	0:94ced082e13a	40	******************************/
lilac0112_1	0:94ced082e13a	41
lilac0112_1	0:94ced082e13a	42	#include "mbed.h"
lilac0112_1	0:94ced082e13a	43	/*
lilac0112_1	0:94ced082e13a	44	#define SYSAHBCLKDIV_Val 0x00000001 // Reset: 0x001
lilac0112_1	0:94ced082e13a	45	#define CLOCK_SETUP 1 // デフォルト=1
lilac0112_1	0:94ced082e13a	46	#define SYSPLLCTRL_Val 0x00000023 // デフォルト=23 Reset: 0x000
lilac0112_1	0:94ced082e13a	47	#define SYSPLLCLKSEL_Val 0x00000001 // デフォルト=0 Reset: 0x000
lilac0112_1	0:94ced082e13a	48	#define MAINCLKSEL_Val 0x00000003 // デフォルト=3 Reset: 0x000
lilac0112_1	0:94ced082e13a	49	*/
lilac0112_1	0:94ced082e13a	50	#include "IRM2121.h"
lilac0112_1	0:94ced082e13a	51	#include "Ping.h"
lilac0112_1	0:94ced082e13a	52
lilac0112_1	0:94ced082e13a	53	#define PING_COUNT 5 //5回に1回にPing処理
lilac0112_1	0:94ced082e13a	54	#define LINE_LEVEL (1-0.55) //しきい値を設定　0.55がしきい値
lilac0112_1	0:94ced082e13a	55	#define IR_LEVEL 400 //irのしきい値
lilac0112_1	0:94ced082e13a	56
lilac0112_1	0:94ced082e13a	57	#define I2C_ADDRESS 0xAA // another (0xAC)
lilac0112_1	0:94ced082e13a	58
lilac0112_1	0:94ced082e13a	59
lilac0112_1	0:94ced082e13a	60
lilac0112_1	0:94ced082e13a	61	IRM2121 Ir[6] = {dp17,dp18,dp24,dp25,dp26,NC};
lilac0112_1	0:94ced082e13a	62	Ping Ping1(dp1,dp2);
lilac0112_1	0:94ced082e13a	63	Ping Ping2(dp4,dp6);
lilac0112_1	0:94ced082e13a	64	AnalogIn Line[4] = {dp9,dp10,dp11,dp13};
lilac0112_1	0:94ced082e13a	65	DigitalOut Led(LED1);
lilac0112_1	0:94ced082e13a	66
lilac0112_1	0:94ced082e13a	67	I2CSlave Mbed(dp5,dp27);//sda,scl
lilac0112_1	0:94ced082e13a	68	Serial pc(USBTX,USBRX);
lilac0112_1	0:94ced082e13a	69
lilac0112_1	0:94ced082e13a	70	/irm2121平均化/
lilac0112_1	0:94ced082e13a	71	uint8_t IrMoving_ave(uint8_t num, int data)
lilac0112_1	0:94ced082e13a	72	{
lilac0112_1	0:94ced082e13a	73	static unsigned int sum[10];
lilac0112_1	0:94ced082e13a	74	static unsigned temp[6][10];
lilac0112_1	0:94ced082e13a	75
lilac0112_1	0:94ced082e13a	76	sum[num] -= temp[num][9];
lilac0112_1	0:94ced082e13a	77	sum[num] += data;
lilac0112_1	0:94ced082e13a	78
lilac0112_1	0:94ced082e13a	79
lilac0112_1	0:94ced082e13a	80	temp[num][9] = temp[num][8];
lilac0112_1	0:94ced082e13a	81	temp[num][8] = temp[num][7];
lilac0112_1	0:94ced082e13a	82	temp[num][7] = temp[num][6];
lilac0112_1	0:94ced082e13a	83	temp[num][6] = temp[num][5];
lilac0112_1	0:94ced082e13a	84	temp[num][5] = temp[num][4];
lilac0112_1	0:94ced082e13a	85	temp[num][4] = temp[num][3];
lilac0112_1	0:94ced082e13a	86	temp[num][3] = temp[num][2];
lilac0112_1	0:94ced082e13a	87	temp[num][2] = temp[num][1];
lilac0112_1	0:94ced082e13a	88	temp[num][1] = temp[num][0];
lilac0112_1	0:94ced082e13a	89	temp[num][0] = data;
lilac0112_1	0:94ced082e13a	90
lilac0112_1	0:94ced082e13a	91	return sum[num]/10;
lilac0112_1	0:94ced082e13a	92	}
lilac0112_1	0:94ced082e13a	93
lilac0112_1	0:94ced082e13a	94	void IrDataSelect(int input[], uint8_t output[])
lilac0112_1	0:94ced082e13a	95	{
lilac0112_1	0:94ced082e13a	96	/*
lilac0112_1	0:94ced082e13a	97	*1,2位の値とその場所の番号をアウトプットに格納する.
lilac0112_1	0:94ced082e13a	98	*値が255以上だとボールを検知してないことになる。
lilac0112_1	0:94ced082e13a	99	*/
lilac0112_1	0:94ced082e13a	100	uint8_t nigh1 = 255, nigh2 = 255;
lilac0112_1	0:94ced082e13a	101	uint8_t ni1=12, ni2 = 12, i;
lilac0112_1	0:94ced082e13a	102
lilac0112_1	0:94ced082e13a	103	for(i = 6; i > 0; i-- ) {
lilac0112_1	0:94ced082e13a	104	if(nigh2 < input[i]) {
lilac0112_1	0:94ced082e13a	105	continue;
lilac0112_1	0:94ced082e13a	106	}
lilac0112_1	0:94ced082e13a	107	if(nigh1 < input[i]) {
lilac0112_1	0:94ced082e13a	108	nigh2 = input[i];
lilac0112_1	0:94ced082e13a	109	ni2 = i;
lilac0112_1	0:94ced082e13a	110	continue;
lilac0112_1	0:94ced082e13a	111	}
lilac0112_1	0:94ced082e13a	112	nigh2 = nigh1;
lilac0112_1	0:94ced082e13a	113	ni2 = ni1;
lilac0112_1	0:94ced082e13a	114	nigh1 = input[i];
lilac0112_1	0:94ced082e13a	115	ni1 = i;
lilac0112_1	0:94ced082e13a	116	}
lilac0112_1	0:94ced082e13a	117	/*
lilac0112_1	0:94ced082e13a	118	for(i = 0; i < 6; i++){
lilac0112_1	0:94ced082e13a	119
lilac0112_1	0:94ced082e13a	120	if(nigh1 > input[i]){
lilac0112_1	0:94ced082e13a	121	nigh1 = input[i];
lilac0112_1	0:94ced082e13a	122	ni1 = i;
lilac0112_1	0:94ced082e13a	123	}
lilac0112_1	0:94ced082e13a	124	if((nigh1<input[i])&&(nigh2>input[i])){
lilac0112_1	0:94ced082e13a	125	nigh2 = input[i];
lilac0112_1	0:94ced082e13a	126	ni2 = i;
lilac0112_1	0:94ced082e13a	127	}
lilac0112_1	0:94ced082e13a	128	}*/
lilac0112_1	0:94ced082e13a	129	if(ni1 == 12) { //ボールがないとき
lilac0112_1	0:94ced082e13a	130	output[0] = 0;
lilac0112_1	0:94ced082e13a	131	output[1] = 255;
lilac0112_1	0:94ced082e13a	132	output[2] = 255;
lilac0112_1	0:94ced082e13a	133	return;
lilac0112_1	0:94ced082e13a	134	}
lilac0112_1	0:94ced082e13a	135
lilac0112_1	0:94ced082e13a	136	output[0] = ni1*6 + ni2;
lilac0112_1	0:94ced082e13a	137	output[1] = nigh1;
lilac0112_1	0:94ced082e13a	138	output[2] = nigh2;
lilac0112_1	0:94ced082e13a	139
lilac0112_1	0:94ced082e13a	140	}
lilac0112_1	0:94ced082e13a	141	/周回の初期動作/
lilac0112_1	0:94ced082e13a	142	void ResetSet(uint8_t *PingCk)
lilac0112_1	0:94ced082e13a	143	{
lilac0112_1	0:94ced082e13a	144
lilac0112_1	0:94ced082e13a	145	Led = 1;//周回の初め
lilac0112_1	0:94ced082e13a	146	PingCk ++;
lilac0112_1	0:94ced082e13a	147
lilac0112_1	0:94ced082e13a	148	}
lilac0112_1	0:94ced082e13a	149
lilac0112_1	0:94ced082e13a	150
lilac0112_1	0:94ced082e13a	151
lilac0112_1	0:94ced082e13a	152	int main()
lilac0112_1	0:94ced082e13a	153	{
lilac0112_1	0:94ced082e13a	154
lilac0112_1	0:94ced082e13a	155	//default Mbed.frequency();
lilac0112_1	0:94ced082e13a	156	Mbed.address(I2C_ADDRESS);
lilac0112_1	0:94ced082e13a	157	/Data/
lilac0112_1	0:94ced082e13a	158	int IrMemory[6] = {0};//IRのデータ保管
lilac0112_1	0:94ced082e13a	159	uint8_t PingMemory[2] = {0};
lilac0112_1	0:94ced082e13a	160	unsigned int LineMemory[4] = {0};
lilac0112_1	0:94ced082e13a	161
lilac0112_1	0:94ced082e13a	162	int IrTemp[6] = {0};//仮置き場
lilac0112_1	0:94ced082e13a	163	uint8_t IrSendData[3];//[0]:どのirを指すのかアドレスで
lilac0112_1	0:94ced082e13a	164
lilac0112_1	0:94ced082e13a	165	/Flag/
lilac0112_1	0:94ced082e13a	166	uint8_t LineFlag;//配置 - - - - Line[3] Line[2] Line[1] Line[0]
lilac0112_1	0:94ced082e13a	167
lilac0112_1	0:94ced082e13a	168	/State(operate_value etc...)/
lilac0112_1	0:94ced082e13a	169	int I2CState;
lilac0112_1	0:94ced082e13a	170
lilac0112_1	0:94ced082e13a	171	/Count/
lilac0112_1	0:94ced082e13a	172	uint8_t PingCk = 0;
lilac0112_1	0:94ced082e13a	173
lilac0112_1	0:94ced082e13a	174	/* I2Cdata*/
lilac0112_1	0:94ced082e13a	175	char SendData[9]= {0};
lilac0112_1	0:94ced082e13a	176	char AddressData = 0;
lilac0112_1	0:94ced082e13a	177
lilac0112_1	0:94ced082e13a	178	/Debug valiable/
lilac0112_1	0:94ced082e13a	179	bool valid = 0,busy = 0;
lilac0112_1	0:94ced082e13a	180
lilac0112_1	0:94ced082e13a	181	while(1) {
lilac0112_1	0:94ced082e13a	182
lilac0112_1	0:94ced082e13a	183	ResetSet(&PingCk);
lilac0112_1	0:94ced082e13a	184	Ir[0].Set();
lilac0112_1	0:94ced082e13a	185	Ir[1].Set();
lilac0112_1	0:94ced082e13a	186	Ir[2].Set();
lilac0112_1	0:94ced082e13a	187	Ir[3].Set();
lilac0112_1	0:94ced082e13a	188	Ir[4].Set();
lilac0112_1	0:94ced082e13a	189	//Ir[5].Set();
lilac0112_1	0:94ced082e13a	190
lilac0112_1	0:94ced082e13a	191	wait_ms(50);//wait値は最適値を探す必要がある．
lilac0112_1	0:94ced082e13a	192
lilac0112_1	0:94ced082e13a	193	IrTemp[0] = IR_LEVEL - Ir[0].Read();
lilac0112_1	0:94ced082e13a	194	IrTemp[1] = IR_LEVEL - Ir[1].Read();
lilac0112_1	0:94ced082e13a	195	IrTemp[2] = IR_LEVEL - Ir[2].Read();
lilac0112_1	0:94ced082e13a	196	IrTemp[3] = IR_LEVEL - Ir[3].Read();
lilac0112_1	0:94ced082e13a	197	IrTemp[4] = IR_LEVEL - Ir[4].Read();
lilac0112_1	0:94ced082e13a	198	//IrTemp[5] = IR_LEVEL - Ir[5].Read();
lilac0112_1	0:94ced082e13a	199
lilac0112_1	0:94ced082e13a	200	Ir[0].ReturnVB(&valid,&busy);
lilac0112_1	0:94ced082e13a	201	pc.printf("valid %d, busy %d \n\r",valid, busy);
lilac0112_1	0:94ced082e13a	202	/*
lilac0112_1	0:94ced082e13a	203	IrMemory[0] = IrMoving_ave(0,IrTemp[0]);
lilac0112_1	0:94ced082e13a	204	IrMemory[1] = IrMoving_ave(1,IrTemp[1]);
lilac0112_1	0:94ced082e13a	205	IrMemory[2] = IrMoving_ave(2,IrTemp[2]);
lilac0112_1	0:94ced082e13a	206	IrMemory[3] = IrMoving_ave(3,IrTemp[3]);
lilac0112_1	0:94ced082e13a	207	IrMemory[4] = IrMoving_ave(4,IrTemp[4]);
lilac0112_1	0:94ced082e13a	208	//IrMemory[5] = IrMoving_ave(5,IrTemp[5]);
lilac0112_1	0:94ced082e13a	209	*/
lilac0112_1	0:94ced082e13a	210
lilac0112_1	0:94ced082e13a	211	/小さい(大きい)二つだけでいい気がする．値をみて小さかったらそうしょう/
lilac0112_1	0:94ced082e13a	212	/**/
lilac0112_1	0:94ced082e13a	213	//IrDataSelect(IrMemory,IrSendData);
lilac0112_1	0:94ced082e13a	214	/**/
lilac0112_1	0:94ced082e13a	215	/*
lilac0112_1	0:94ced082e13a	216	if(PingCk >= PING_COUNT){
lilac0112_1	0:94ced082e13a	217	Ping1.Send();
lilac0112_1	0:94ced082e13a	218	Ping2.Send();
lilac0112_1	0:94ced082e13a	219	wait_ms(30);
lilac0112_1	0:94ced082e13a	220	PingMemory[0] = Ping1.Read_cm();
lilac0112_1	0:94ced082e13a	221	PingMemory[1] = Ping2.Read_cm();
lilac0112_1	0:94ced082e13a	222	PingCk = 0;
lilac0112_1	0:94ced082e13a	223	}
lilac0112_1	0:94ced082e13a	224	*/
lilac0112_1	0:94ced082e13a	225	LineMemory[0] = Line[0];
lilac0112_1	0:94ced082e13a	226	LineMemory[1] = Line[1];
lilac0112_1	0:94ced082e13a	227	LineMemory[2] = Line[2];
lilac0112_1	0:94ced082e13a	228	LineMemory[3] = Line[3];
lilac0112_1	0:94ced082e13a	229
lilac0112_1	0:94ced082e13a	230	LineFlag = (((int)(LineMemory[3]+LINE_LEVEL)) \| ((int)(LineMemory[2]+LINE_LEVEL)))<<1 +
lilac0112_1	0:94ced082e13a	231	(((int)(LineMemory[1]+LINE_LEVEL)) \| ((int)(LineMemory[0]+LINE_LEVEL))); //まとめてる
lilac0112_1	0:94ced082e13a	232	/*
lilac0112_1	0:94ced082e13a	233	I2CState = Mbed.receive();
lilac0112_1	0:94ced082e13a	234
lilac0112_1	0:94ced082e13a	235	switch(I2CState){
lilac0112_1	0:94ced082e13a	236	case I2CSlave::ReadAddressed:
lilac0112_1	0:94ced082e13a	237	if(AddressData){
lilac0112_1	0:94ced082e13a	238	switch(AddressData){
lilac0112_1	0:94ced082e13a	239	case 0x01://IR
lilac0112_1	0:94ced082e13a	240	SendData[0] = (char)IrSendData[0];
lilac0112_1	0:94ced082e13a	241	SendData[1] = (char)IrSendData[1];
lilac0112_1	0:94ced082e13a	242	SendData[2] = (char)IrSendData[2];
lilac0112_1	0:94ced082e13a	243	Mbed.write(SendData, 3);
lilac0112_1	0:94ced082e13a	244	break;
lilac0112_1	0:94ced082e13a	245
lilac0112_1	0:94ced082e13a	246	case 0x02://ping
lilac0112_1	0:94ced082e13a	247	SendData[0] = (char)PingMemory[0];
lilac0112_1	0:94ced082e13a	248	SendData[1] = (char)PingMemory[1];
lilac0112_1	0:94ced082e13a	249	Mbed.write(SendData, 2);
lilac0112_1	0:94ced082e13a	250	break;
lilac0112_1	0:94ced082e13a	251	default:
lilac0112_1	0:94ced082e13a	252	break;
lilac0112_1	0:94ced082e13a	253	}
lilac0112_1	0:94ced082e13a	254	}else{//line
lilac0112_1	0:94ced082e13a	255	SendData[0] = (char)LineFlag;
lilac0112_1	0:94ced082e13a	256	Mbed.write(SendData, 1);
lilac0112_1	0:94ced082e13a	257	}
lilac0112_1	0:94ced082e13a	258	AddressData = 0;
lilac0112_1	0:94ced082e13a	259	break;
lilac0112_1	0:94ced082e13a	260	case I2CSlave::WriteGeneral:
lilac0112_1	0:94ced082e13a	261	AddressData = Mbed.read();
lilac0112_1	0:94ced082e13a	262	break;
lilac0112_1	0:94ced082e13a	263	case I2CSlave::WriteAddressed:
lilac0112_1	0:94ced082e13a	264	AddressData = Mbed.read();
lilac0112_1	0:94ced082e13a	265	break;
lilac0112_1	0:94ced082e13a	266
lilac0112_1	0:94ced082e13a	267	}
lilac0112_1	0:94ced082e13a	268	*/
lilac0112_1	0:94ced082e13a	269	pc.printf("%d %d %d %d %d %d\n\r",IrTemp[0],IrTemp[1],IrTemp[2],IrTemp[3],IrTemp[4],IrTemp[5]);
lilac0112_1	0:94ced082e13a	270
lilac0112_1	0:94ced082e13a	271	Led = 0;
lilac0112_1	0:94ced082e13a	272
lilac0112_1	0:94ced082e13a	273	}
lilac0112_1	0:94ced082e13a	274	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	24 Jan 2015
Imports:	2
Forks:	0
Commits:	2
Dependents:	0
Dependencies:	6
Followers:	3

main.cpp@0:94ced082e13a, 2015-01-24 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning