4wd_omni_simple2 - 中継機能つけた受け取りオムニ

Bチーム » Code » 4wd_omni_simple2

Bチーム / Mbed 2 deprecated 4wd_omni_simple2

中継機能つけた受け取りオムニ

main.cpp@17:3c82070177e5, 2014-09-26 (annotated)

Committer:: Hatter
Date:: Fri Sep 26 09:26:27 2014 +0000
Revision:: 17:3c82070177e5
Parent:: 16:4b502070bea8
Child:: 18:9cbd70819d2d

Who changed what in which revision?

User	Revision	Line number	New contents of line
Hatter	0:fddd018033fb	1	/*
Hatter	0:fddd018033fb	2	* 4WD_OMNI_simple by Tomoki Hokida
Hatter	13:8a9d3855ab96	3	* Simple Type
Hatter	0:fddd018033fb	4	*
Hatter	0:fddd018033fb	5	* motor pins piar
Hatter	13:8a9d3855ab96	6	* motor(p13,p14):motor(p15,p16)
Hatter	13:8a9d3855ab96	7	* motor(p17,p18):motor(p19,p20)
Hatter	13:8a9d3855ab96	8	*
Hatter	13:8a9d3855ab96	9	* Serial Pins
Hatter	15:1c1ae4077900	10	* packet.arm[0]:arm
Hatter	13:8a9d3855ab96	11	* packet.leg[1]:omni
Hatter	0:fddd018033fb	12	* reset 0
Hatter	0:fddd018033fb	13	*/
Hatter	0:fddd018033fb	14
Hatter	0:fddd018033fb	15	#include "mbed.h"
Hatter	6:26e5a520472d	16	#include "MultiSerial.h"
Hatter	0:fddd018033fb	17
Hatter	14:731c52eb333b	18	#define DATA_NUM 2 //DATA_NUM[byte]通信この値はmemcpy使ってるんでxbee_packet構造体のメモリ数といい感じに合わせるようにしてください
Hatter	13:8a9d3855ab96	19	#define XBEE_KEY 0xAA //keycode
Hatter	9:f05f881226a3	20	#define ARM_KEY 0xAA
Hatter	3:1f1498403eec	21
Hatter	17:3c82070177e5	22	#define PWM 0.5
Hatter	15:1c1ae4077900	23
Hatter	17:3c82070177e5	24	#define PWM_L 0.5
Hatter	17:3c82070177e5	25	#define PWM_R 0.5
Hatter	17:3c82070177e5	26	#define PWM_U 0.5
Hatter	17:3c82070177e5	27	#define PWM_D 0.5
Hatter	12:6e2ee1c83ac1	28
Hatter	15:1c1ae4077900	29	#define R 0x4
Hatter	15:1c1ae4077900	30	#define L 0x1
Hatter	15:1c1ae4077900	31	#define U 0x2
Hatter	15:1c1ae4077900	32	#define D 0x8
Hatter	15:1c1ae4077900	33
Hatter	15:1c1ae4077900	34	#define R_t 0x10
Hatter	15:1c1ae4077900	35	#define L_t 0x40
Hatter	15:1c1ae4077900	36
Hatter	15:1c1ae4077900	37	#define TIME 0
Hatter	13:8a9d3855ab96	38
Hatter	9:f05f881226a3	39	BusOut check(LED1,LED2,LED3,LED4);
Hatter	13:8a9d3855ab96	40	BusOut motors(p13,p14,p15,p16,p17,p18,p19,p20);
Hatter	12:6e2ee1c83ac1	41
Hatter	15:1c1ae4077900	42	PwmOut pwm[4]= {p21,p22,p23,p24};
Hatter	0:fddd018033fb	43
Hatter	9:f05f881226a3	44	Serial pc(USBTX,USBRX);
Hatter	0:fddd018033fb	45
Hatter	16:4b502070bea8	46	MultiSerial xbee(p28,p27);
Hatter	16:4b502070bea8	47	MultiSerial armMbed(p9,p10);
Hatter	0:fddd018033fb	48
Hatter	13:8a9d3855ab96	49	/* data structure */
Hatter	15:1c1ae4077900	50	typedef struct {
Hatter	6:26e5a520472d	51
Hatter	6:26e5a520472d	52	uint8_t arm[1];
Hatter	6:26e5a520472d	53	uint8_t leg;
Hatter	4:5da566692b79	54
Hatter	15:1c1ae4077900	55	} xbee_packet;
Hatter	6:26e5a520472d	56
Hatter	6:26e5a520472d	57	xbee_packet packet;
Hatter	6:26e5a520472d	58
Hatter	15:1c1ae4077900	59	uint8_t get_data[DATA_NUM]= {0};
Hatter	13:8a9d3855ab96	60
Hatter	16:4b502070bea8	61	uint8_t check_data[1];
Hatter	16:4b502070bea8	62
Hatter	13:8a9d3855ab96	63	/* Out put PC stdout function */
Hatter	17:3c82070177e5	64	void pc_print()
Hatter	17:3c82070177e5	65	{
Hatter	13:8a9d3855ab96	66	static int count=0;
Hatter	13:8a9d3855ab96	67
Hatter	15:1c1ae4077900	68	if(count==10e3) {
Hatter	13:8a9d3855ab96	69	pc.printf(" arm = %d" ,packet.arm[0]);
Hatter	13:8a9d3855ab96	70	pc.printf(" leg = %d" ,packet.leg);
Hatter	13:8a9d3855ab96	71	count=0;
Hatter	15:1c1ae4077900	72	}
Hatter	13:8a9d3855ab96	73	count++;
Hatter	13:8a9d3855ab96	74	}
Hatter	0:fddd018033fb	75
Hatter	17:3c82070177e5	76	void connect_check(void)
Hatter	17:3c82070177e5	77	{
Hatter	16:4b502070bea8	78	xbee.start_read();
Hatter	16:4b502070bea8	79	xbee.read_data(check_data,XBEE_KEY);
Hatter	17:3c82070177e5	80
Hatter	16:4b502070bea8	81	while(check_data[0]!=0xCC);
Hatter	16:4b502070bea8	82	while(check_data[0]==0xCC);
Hatter	17:3c82070177e5	83
Hatter	16:4b502070bea8	84	xbee.stop_read();
Hatter	16:4b502070bea8	85	xbee.start_write();
Hatter	17:3c82070177e5	86
Hatter	16:4b502070bea8	87	check_data[0]=0xCC;
Hatter	17:3c82070177e5	88
Hatter	16:4b502070bea8	89	xbee.write_data(check_data,XBEE_KEY);
Hatter	17:3c82070177e5	90
Hatter	16:4b502070bea8	91	wait_ms(10);
Hatter	17:3c82070177e5	92
Hatter	16:4b502070bea8	93	xbee.stop_write();
Hatter	17:3c82070177e5	94	}
Hatter	16:4b502070bea8	95
Hatter	15:1c1ae4077900	96	void stop_slowly(int count)
Hatter	15:1c1ae4077900	97	{
Hatter	15:1c1ae4077900	98
Hatter	15:1c1ae4077900	99	for(float i=1.0; i>=0; i-=0.1) {
Hatter	17:3c82070177e5	100
Hatter	15:1c1ae4077900	101	/*for(int j=0; j>=4; j++) {
Hatter	15:1c1ae4077900	102	pwm[j] = i;
Hatter	15:1c1ae4077900	103
Hatter	15:1c1ae4077900	104	}*/
Hatter	17:3c82070177e5	105
Hatter	17:3c82070177e5	106	pwm[0] = 0.1;
Hatter	17:3c82070177e5	107	pwm[1] = 0.1;
Hatter	17:3c82070177e5	108	pwm[2] = 0.1;
Hatter	17:3c82070177e5	109	pwm[3] = 0.1;
Hatter	17:3c82070177e5	110
Hatter	15:1c1ae4077900	111	wait(count10e-3); //count value 0.01 sec
Hatter	15:1c1ae4077900	112	}
Hatter	15:1c1ae4077900	113	check = 0;
Hatter	15:1c1ae4077900	114	}
Hatter	15:1c1ae4077900	115
Hatter	0:fddd018033fb	116	int main()
Hatter	15:1c1ae4077900	117	{
Hatter	13:8a9d3855ab96	118
Hatter	13:8a9d3855ab96	119	/* PWM init */
Hatter	12:6e2ee1c83ac1	120	pwm[0] = PWM;
Hatter	12:6e2ee1c83ac1	121	pwm[1] = PWM;
Hatter	12:6e2ee1c83ac1	122	pwm[2] = PWM;
Hatter	12:6e2ee1c83ac1	123	pwm[3] = PWM;
Hatter	12:6e2ee1c83ac1	124
Hatter	10:5b67d18f30a9	125	xbee_packet *pt_packet=&packet;
Hatter	12:6e2ee1c83ac1	126
Hatter	17:3c82070177e5	127	//connect_check();
Hatter	17:3c82070177e5	128
Hatter	16:4b502070bea8	129	xbee.start_read();
Hatter	13:8a9d3855ab96	130	xbee.read_data(get_data,XBEE_KEY);
Hatter	13:8a9d3855ab96	131
Hatter	15:1c1ae4077900	132	int counter=0;
Hatter	12:6e2ee1c83ac1	133
Hatter	15:1c1ae4077900	134	for(;;) {
Hatter	15:1c1ae4077900	135
Hatter	15:1c1ae4077900	136	wait_ms(1.0);
Hatter	12:6e2ee1c83ac1	137
Hatter	13:8a9d3855ab96	138	//memcpy(&packet, get_data, DATA_NUM);
Hatter	10:5b67d18f30a9	139	packet.arm[0] = get_data[1];
Hatter	15:1c1ae4077900	140	packet.leg = get_data[0];
Hatter	12:6e2ee1c83ac1	141
Hatter	15:1c1ae4077900	142	//check = get_data[1];
Hatter	15:1c1ae4077900	143	armMbed.write_data(pt_packet->arm,ARM_KEY);
Hatter	12:6e2ee1c83ac1	144
Hatter	13:8a9d3855ab96	145	/* Stop */
Hatter	15:1c1ae4077900	146	if(packet.leg==0x0) {
Hatter	15:1c1ae4077900	147
Hatter	15:1c1ae4077900	148	counter /= 10e2; //mirisec -->> sec
Hatter	15:1c1ae4077900	149
Hatter	15:1c1ae4077900	150	if(counter>=TIME) { //wait 2 sec
Hatter	15:1c1ae4077900	151
Hatter	15:1c1ae4077900	152	stop_slowly(counter);
Hatter	15:1c1ae4077900	153	}
Hatter	13:8a9d3855ab96	154
Hatter	15:1c1ae4077900	155	motors = 0;
Hatter	15:1c1ae4077900	156	counter = 0; //cunter init
Hatter	15:1c1ae4077900	157
Hatter	15:1c1ae4077900	158	} else {
Hatter	15:1c1ae4077900	159
Hatter	15:1c1ae4077900	160	counter++;
Hatter	15:1c1ae4077900	161	check = 0xFF;
Hatter	15:1c1ae4077900	162	}
Hatter	15:1c1ae4077900	163
Hatter	15:1c1ae4077900	164	/* PWM */
Hatter	15:1c1ae4077900	165	if((packet.leg>>4)==0) {
Hatter	15:1c1ae4077900	166
Hatter	14:731c52eb333b	167	pwm[0] = PWM_L;
Hatter	14:731c52eb333b	168	pwm[1] = PWM_R;
Hatter	14:731c52eb333b	169	pwm[2] = PWM_U;
Hatter	15:1c1ae4077900	170	pwm[3] = PWM_D;
Hatter	13:8a9d3855ab96	171
Hatter	15:1c1ae4077900	172	} else {
Hatter	14:731c52eb333b	173
Hatter	14:731c52eb333b	174	pwm[0] = PWM;
Hatter	14:731c52eb333b	175	pwm[1] = PWM;
Hatter	14:731c52eb333b	176	pwm[2] = PWM;
Hatter	14:731c52eb333b	177	pwm[3] = PWM;
Hatter	15:1c1ae4077900	178
Hatter	13:8a9d3855ab96	179	}
Hatter	12:6e2ee1c83ac1	180
Hatter	13:8a9d3855ab96	181	/* Direction of movement */
Hatter	15:1c1ae4077900	182	if(packet.leg&R) motors = 0x55; //01010101 R 接続に気をつけること
Hatter	15:1c1ae4077900	183	if(packet.leg&L) motors = 0xAA; //10101010 L 多分モータードライバの信号線をそれぞれ逆にすればOKかな
Hatter	15:1c1ae4077900	184	if(packet.leg&U) motors = 0x5A; //01011010 U
Hatter	15:1c1ae4077900	185	if(packet.leg&D) motors = 0xA5; //10100101 D
Hatter	13:8a9d3855ab96	186
Hatter	13:8a9d3855ab96	187	/* Turn circling */
Hatter	16:4b502070bea8	188	if(packet.leg&L_t) motors = 0x66; //01100110
Hatter	16:4b502070bea8	189	if(packet.leg&R_t) motors = 0x99; //10011001
Hatter	13:8a9d3855ab96	190
Hatter	13:8a9d3855ab96	191	pc_print();
Hatter	6:26e5a520472d	192	}
Hatter	0:fddd018033fb	193	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	19 Aug 2014
Imports:	10
Forks:	0
Commits:	43
Dependents:	0
Dependencies:	2
Followers:	9

The code in this repository is Apache licensed.

main.cpp@17:3c82070177e5, 2014-09-26 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning