mbed_TANK_Kinect - Based on myBlueUSB reference ver. http://mbed.or…

Users » kenbumono » Code » mbed_TANK_Kinect

Yasuhiko YAMAMOTO / Mbed 2 deprecated mbed_TANK_Kinect

Based on myBlueUSB reference ver. http://mbed.org/users/networker/programs/myBlueUSB/lsm1ui

Dependencies: mbed myUSBHost AvailableMemory rfcomm myBlueUSB sdp

TestShell.cpp@0:8d8481ed6d49, 2011-07-05 (annotated)

Committer:: kenbumono
Date:: Tue Jul 05 08:25:59 2011 +0000
Revision:: 0:8d8481ed6d49

Who changed what in which revision?

User	Revision	Line number	New contents of line
kenbumono	0:8d8481ed6d49	1
kenbumono	0:8d8481ed6d49	2	/*
kenbumono	0:8d8481ed6d49	3	Copyright (c) 2010 Peter Barrett
kenbumono	0:8d8481ed6d49	4
kenbumono	0:8d8481ed6d49	5	Permission is hereby granted, free of charge, to any person obtaining a copy
kenbumono	0:8d8481ed6d49	6	of this software and associated documentation files (the "Software"), to deal
kenbumono	0:8d8481ed6d49	7	in the Software without restriction, including without limitation the rights
kenbumono	0:8d8481ed6d49	8	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
kenbumono	0:8d8481ed6d49	9	copies of the Software, and to permit persons to whom the Software is
kenbumono	0:8d8481ed6d49	10	furnished to do so, subject to the following conditions:
kenbumono	0:8d8481ed6d49	11
kenbumono	0:8d8481ed6d49	12	The above copyright notice and this permission notice shall be included in
kenbumono	0:8d8481ed6d49	13	all copies or substantial portions of the Software.
kenbumono	0:8d8481ed6d49	14
kenbumono	0:8d8481ed6d49	15	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
kenbumono	0:8d8481ed6d49	16	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
kenbumono	0:8d8481ed6d49	17	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
kenbumono	0:8d8481ed6d49	18	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
kenbumono	0:8d8481ed6d49	19	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
kenbumono	0:8d8481ed6d49	20	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
kenbumono	0:8d8481ed6d49	21	THE SOFTWARE.
kenbumono	0:8d8481ed6d49	22	*/
kenbumono	0:8d8481ed6d49	23	#include "mbed.h"
kenbumono	0:8d8481ed6d49	24	#include <vector>
kenbumono	0:8d8481ed6d49	25	#include "Utils.h"
kenbumono	0:8d8481ed6d49	26	#include "USBHost.h"
kenbumono	0:8d8481ed6d49	27	#include "hci.h"
kenbumono	0:8d8481ed6d49	28	#include "HCITransportUSB.h"
kenbumono	0:8d8481ed6d49	29	#include "RFCOMM.h"
kenbumono	0:8d8481ed6d49	30	#include "ftclasslibusbdevbt.h"
kenbumono	0:8d8481ed6d49	31	#include "sdp_data.h"
kenbumono	0:8d8481ed6d49	32	#include "sdp.h"
kenbumono	0:8d8481ed6d49	33	#include "btserial.h"
kenbumono	0:8d8481ed6d49	34	#include "neighbourhood.h"
kenbumono	0:8d8481ed6d49	35
kenbumono	0:8d8481ed6d49	36	#include "motordriver.h"
kenbumono	0:8d8481ed6d49	37
kenbumono	0:8d8481ed6d49	38	/************************************************
kenbumono	0:8d8481ed6d49	39	TODO:
kenbumono	0:8d8481ed6d49	40	mtu and credits are completely unhandled - in progress
kenbumono	0:8d8481ed6d49	41	multiple rfcomm sessions should be possible - done
kenbumono	0:8d8481ed6d49	42	SDP would be nice - beta
kenbumono	0:8d8481ed6d49	43	multiple rfcomm channels are untested
kenbumono	0:8d8481ed6d49	44	decoupling of rfcomm and application - much better
kenbumono	0:8d8481ed6d49	45	packets are not reassembled - some are (HCI and ft application level)
kenbumono	0:8d8481ed6d49	46	disconnect and timeouts
kenbumono	0:8d8481ed6d49	47	************************************************/
kenbumono	0:8d8481ed6d49	48	#define DEBUG 1
kenbumono	0:8d8481ed6d49	49	int state = 0;
kenbumono	0:8d8481ed6d49	50
kenbumono	0:8d8481ed6d49	51	//int bulk = 0;
kenbumono	0:8d8481ed6d49	52	void printf(const BD_ADDR* addr) {
kenbumono	0:8d8481ed6d49	53	const u8* a = addr->addr;
kenbumono	0:8d8481ed6d49	54	printf("%02X:%02X:%02X:%02X:%02X:%02X",a[5],a[4],a[3],a[2],a[1],a[0]);
kenbumono	0:8d8481ed6d49	55	}
kenbumono	0:8d8481ed6d49	56
kenbumono	0:8d8481ed6d49	57	const char FtDevClass[3] = {0x00, 0x1F, 0x82 };
kenbumono	0:8d8481ed6d49	58	const char SerDevClass[3] = {4, 1, 0x00};
kenbumono	0:8d8481ed6d49	59	// Instance
kenbumono	0:8d8481ed6d49	60	extern RFCOMMManager rfcomm_manager;
kenbumono	0:8d8481ed6d49	61
kenbumono	0:8d8481ed6d49	62	class application : public HCI {
kenbumono	0:8d8481ed6d49	63	BTDevice* devs[8];
kenbumono	0:8d8481ed6d49	64	int count, i, pending;
kenbumono	0:8d8481ed6d49	65	public:
kenbumono	0:8d8481ed6d49	66	// We have connected to a device
kenbumono	0:8d8481ed6d49	67	void ConnectionComplete(connection_info* info) {
kenbumono	0:8d8481ed6d49	68	printf("ConnectionComplete ");
kenbumono	0:8d8481ed6d49	69	BD_ADDR* a = &info->bdaddr;
kenbumono	0:8d8481ed6d49	70	printf(a);
kenbumono	0:8d8481ed6d49	71	printf("\n");
kenbumono	0:8d8481ed6d49	72	RemoteNameRequest(a);
kenbumono	0:8d8481ed6d49	73	for (i++; i < count; i++) {//find the next device to open
kenbumono	0:8d8481ed6d49	74	printfBytes("DEVICE CLASS",devs[i]->_info.dev_class,3);
kenbumono	0:8d8481ed6d49	75	if (devs[i]->_handle == 0 && memcmp(devs[i]->_info.dev_class, FtDevClass, 3)==0) {//or some other way to connect to RFCOMM devices
kenbumono	0:8d8481ed6d49	76	BD_ADDR* bd = &devs[i]->_info.bdaddr;
kenbumono	0:8d8481ed6d49	77	printf("Connecting to ");
kenbumono	0:8d8481ed6d49	78	printf(bd);
kenbumono	0:8d8481ed6d49	79	printf("\n");
kenbumono	0:8d8481ed6d49	80	pending++;
kenbumono	0:8d8481ed6d49	81	CreateConnection(bd); //some low level connect, just let it happen for now (sets pin, mtu etc.)
kenbumono	0:8d8481ed6d49	82	printf("connection cmd was sent\n");
kenbumono	0:8d8481ed6d49	83	return;
kenbumono	0:8d8481ed6d49	84	}
kenbumono	0:8d8481ed6d49	85	}
kenbumono	0:8d8481ed6d49	86	//state = 1; //start the real application
kenbumono	0:8d8481ed6d49	87	}
kenbumono	0:8d8481ed6d49	88
kenbumono	0:8d8481ed6d49	89	void ConnectDevices() {
kenbumono	0:8d8481ed6d49	90	count = GetDevices(devs,8);//get pointers to all bluetooth devices
kenbumono	0:8d8481ed6d49	91	pending = 0;
kenbumono	0:8d8481ed6d49	92	for (i = 0; i < count; i++) {
kenbumono	0:8d8481ed6d49	93	printfBytes("DEVICE CLASS",devs[i]->_info.dev_class,3);
kenbumono	0:8d8481ed6d49	94	if (devs[i]->_handle == 0 /&& memcmp(devs[i]->_info.dev_class, FtDevClass, 3)==0/) {//or some other way to connect to RFCOMM devices
kenbumono	0:8d8481ed6d49	95	BD_ADDR* bd = &devs[i]->_info.bdaddr;
kenbumono	0:8d8481ed6d49	96	printf("Connecting to ");
kenbumono	0:8d8481ed6d49	97	printf(bd);
kenbumono	0:8d8481ed6d49	98	printf("\n");
kenbumono	0:8d8481ed6d49	99	pending++;
kenbumono	0:8d8481ed6d49	100	CreateConnection(bd); //some low level connect, just let it happen for now (sets pin, mtu etc.)
kenbumono	0:8d8481ed6d49	101	printf("connection cmd was sent\n");
kenbumono	0:8d8481ed6d49	102	return;
kenbumono	0:8d8481ed6d49	103	}
kenbumono	0:8d8481ed6d49	104	}
kenbumono	0:8d8481ed6d49	105	if (pending == 0) state = 1;//for the case when there are no ft devices
kenbumono	0:8d8481ed6d49	106	}
kenbumono	0:8d8481ed6d49	107	virtual void Callback(HCI_CALLBACK_EVENT c, const u8* data, int len);
kenbumono	0:8d8481ed6d49	108	int csr_write_bd_addr(BD_ADDR *bdaddr, bool transient=true);
kenbumono	0:8d8481ed6d49	109	int csr_reset_device(bool transient=true);
kenbumono	0:8d8481ed6d49	110	} App; //application instance
kenbumono	0:8d8481ed6d49	111
kenbumono	0:8d8481ed6d49	112	extern "C" void mbed_reset();
kenbumono	0:8d8481ed6d49	113
kenbumono	0:8d8481ed6d49	114
kenbumono	0:8d8481ed6d49	115	void application::Callback(HCI_CALLBACK_EVENT evt, const u8* data, int len) {//these events are forwarded (in)directly from HCIRecv
kenbumono	0:8d8481ed6d49	116	unsigned char pin[] = "1234";
kenbumono	0:8d8481ed6d49	117	unsigned char newaddr[] = {0x2c, 0x07, 0x54, 0x7b, 0x13, 0x00};//possible ft TX address (ROBO TX-277)
kenbumono	0:8d8481ed6d49	118	// unsigned char newaddr[] = {0x57, 0x0a, 0x3d, 0x83, 0x15, 0x00};//original address of the cheap round BT dongle
kenbumono	0:8d8481ed6d49	119	printf("\x1b[%dm", 33);
kenbumono	0:8d8481ed6d49	120	switch (evt) {
kenbumono	0:8d8481ed6d49	121	case CALLBACK_READY:
kenbumono	0:8d8481ed6d49	122	printf("CALLBACK_READY\n");
kenbumono	0:8d8481ed6d49	123	printf("my address = ");
kenbumono	0:8d8481ed6d49	124	printf((BD_ADDR*)data);
kenbumono	0:8d8481ed6d49	125	if (memcmp(newaddr, data, 6) != 0) { //csr_write_bd_addr((BD_ADDR*)newaddr, false);
kenbumono	0:8d8481ed6d49	126	}
kenbumono	0:8d8481ed6d49	127	Inquiry();//start the second phase of the discovery
kenbumono	0:8d8481ed6d49	128	break;
kenbumono	0:8d8481ed6d49	129
kenbumono	0:8d8481ed6d49	130	case CALLBACK_INQUIRY_RESULT: //optionally build the list of FT devices here
kenbumono	0:8d8481ed6d49	131	printf("CALLBACK_INQUIRY_RESULT ");
kenbumono	0:8d8481ed6d49	132	printf((BD_ADDR*)data);
kenbumono	0:8d8481ed6d49	133	printf("\n");//data points to inquiry_info struct
kenbumono	0:8d8481ed6d49	134	// RemoteNameRequest((BD_ADDR*)data);
kenbumono	0:8d8481ed6d49	135	break;
kenbumono	0:8d8481ed6d49	136
kenbumono	0:8d8481ed6d49	137	case CALLBACK_INQUIRY_DONE:
kenbumono	0:8d8481ed6d49	138	printf("CALLBACK_INQUIRY_DONE\n");
kenbumono	0:8d8481ed6d49	139	neighbors = new neighbourhood(&App);
kenbumono	0:8d8481ed6d49	140	neighbors->read();
kenbumono	0:8d8481ed6d49	141	ConnectDevices();
kenbumono	0:8d8481ed6d49	142	break;
kenbumono	0:8d8481ed6d49	143
kenbumono	0:8d8481ed6d49	144	case CALLBACK_REMOTE_NAME: {
kenbumono	0:8d8481ed6d49	145	BD_ADDR* addr = (BD_ADDR*)data;
kenbumono	0:8d8481ed6d49	146	const char* name = (const char*)(data + 6);
kenbumono	0:8d8481ed6d49	147	printf(addr);
kenbumono	0:8d8481ed6d49	148	printf(" = % s\n",name);
kenbumono	0:8d8481ed6d49	149	pending--;
kenbumono	0:8d8481ed6d49	150	if (pending == 0) state = 1;
kenbumono	0:8d8481ed6d49	151	}
kenbumono	0:8d8481ed6d49	152	break;
kenbumono	0:8d8481ed6d49	153
kenbumono	0:8d8481ed6d49	154	case CALLBACK_CONNECTION_COMPLETE: {
kenbumono	0:8d8481ed6d49	155	connection_info ci = (connection_info)data;
kenbumono	0:8d8481ed6d49	156	if (ci->status>0) {
kenbumono	0:8d8481ed6d49	157	printf("Connection failed, status=0x%02X\n", ci->status);
kenbumono	0:8d8481ed6d49	158	break;
kenbumono	0:8d8481ed6d49	159	}
kenbumono	0:8d8481ed6d49	160	ConnectionComplete(ci);
kenbumono	0:8d8481ed6d49	161	printf("Going to open sdp socket\n");
kenbumono	0:8d8481ed6d49	162	L2CAPAddr addr;
kenbumono	0:8d8481ed6d49	163	memcpy(&addr.bdaddr, &ci->bdaddr, 6);
kenbumono	0:8d8481ed6d49	164	//int s = SDP.Open(&addr.hdr);
kenbumono	0:8d8481ed6d49	165	}
kenbumono	0:8d8481ed6d49	166	break;
kenbumono	0:8d8481ed6d49	167	case CALLBACK_PIN_REQ:
kenbumono	0:8d8481ed6d49	168	printf("Enter PIN for ");
kenbumono	0:8d8481ed6d49	169	printf((BD_ADDR*)data);
kenbumono	0:8d8481ed6d49	170	printf(" : submitting %s\n", pin);
kenbumono	0:8d8481ed6d49	171	//USBLoop(); wait(1.0); USBLoop();
kenbumono	0:8d8481ed6d49	172	//for(int k=0; k<2000000;k++) USBLoop();
kenbumono	0:8d8481ed6d49	173	PinCodeReply(data, pin);
kenbumono	0:8d8481ed6d49	174	break;
kenbumono	0:8d8481ed6d49	175	case CALLBACK_VENDOR:
kenbumono	0:8d8481ed6d49	176	printfBytes("Vendor Reply:", data, len);
kenbumono	0:8d8481ed6d49	177	//mbed_reset();
kenbumono	0:8d8481ed6d49	178	if (data[0] == 0xc2)
kenbumono	0:8d8481ed6d49	179	csr_reset_device(false);
kenbumono	0:8d8481ed6d49	180	break;
kenbumono	0:8d8481ed6d49	181	default:
kenbumono	0:8d8481ed6d49	182	printf("Unhandled HCI Callback %d\n", evt);
kenbumono	0:8d8481ed6d49	183	};
kenbumono	0:8d8481ed6d49	184	printf("\x1b[%dm", 0);
kenbumono	0:8d8481ed6d49	185	}
kenbumono	0:8d8481ed6d49	186
kenbumono	0:8d8481ed6d49	187	#define CSR_WRITE 0xFC00
kenbumono	0:8d8481ed6d49	188
kenbumono	0:8d8481ed6d49	189	int application::csr_write_bd_addr(BD_ADDR *bdaddr, bool transient) {
kenbumono	0:8d8481ed6d49	190	unsigned char cmd[] = { 0xc2,
kenbumono	0:8d8481ed6d49	191	0x02, 0x00, 0x0c, 0x00, 0x11, 0x47, 0x03, 0x70,
kenbumono	0:8d8481ed6d49	192	0x00, 0x00, 0x01, 0x00, 0x04, 0x00, 0x00, 0x00,
kenbumono	0:8d8481ed6d49	193	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
kenbumono	0:8d8481ed6d49	194	};
kenbumono	0:8d8481ed6d49	195
kenbumono	0:8d8481ed6d49	196	if (transient)
kenbumono	0:8d8481ed6d49	197	cmd[15] = 0x08;
kenbumono	0:8d8481ed6d49	198
kenbumono	0:8d8481ed6d49	199	cmd[17] = bdaddr->addr[2];
kenbumono	0:8d8481ed6d49	200	cmd[18] = 0x00;
kenbumono	0:8d8481ed6d49	201	cmd[19] = bdaddr->addr[0];
kenbumono	0:8d8481ed6d49	202	cmd[20] = bdaddr->addr[1];
kenbumono	0:8d8481ed6d49	203	cmd[21] = bdaddr->addr[3];
kenbumono	0:8d8481ed6d49	204	cmd[22] = 0x00;
kenbumono	0:8d8481ed6d49	205	cmd[23] = bdaddr->addr[4];
kenbumono	0:8d8481ed6d49	206	cmd[24] = bdaddr->addr[5];
kenbumono	0:8d8481ed6d49	207
kenbumono	0:8d8481ed6d49	208	return SendCmd(CSR_WRITE, cmd, sizeof(cmd));
kenbumono	0:8d8481ed6d49	209	}
kenbumono	0:8d8481ed6d49	210
kenbumono	0:8d8481ed6d49	211	int application::csr_reset_device(bool transient) {
kenbumono	0:8d8481ed6d49	212	unsigned char cmd[] = { 0xc2, 0x02, 0x00, 0x09, 0x00,
kenbumono	0:8d8481ed6d49	213	0x00, 0x00, 0x01, 0x40, 0x00, 0x00,
kenbumono	0:8d8481ed6d49	214	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
kenbumono	0:8d8481ed6d49	215	};
kenbumono	0:8d8481ed6d49	216
kenbumono	0:8d8481ed6d49	217	if (transient)
kenbumono	0:8d8481ed6d49	218	cmd[7] = 0x02;
kenbumono	0:8d8481ed6d49	219
kenbumono	0:8d8481ed6d49	220	return SendCmd(CSR_WRITE, cmd, sizeof(cmd));
kenbumono	0:8d8481ed6d49	221	}
kenbumono	0:8d8481ed6d49	222
kenbumono	0:8d8481ed6d49	223
kenbumono	0:8d8481ed6d49	224	// these should be placed in the DMA SRAM
kenbumono	0:8d8481ed6d49	225	typedef struct {
kenbumono	0:8d8481ed6d49	226	u8 _hciBuffer[MAX_HCL_SIZE];
kenbumono	0:8d8481ed6d49	227	u8 _aclBuffer[MAX_ACL_SIZE];
kenbumono	0:8d8481ed6d49	228	} SRAMPlacement;
kenbumono	0:8d8481ed6d49	229
kenbumono	0:8d8481ed6d49	230	HCITransportUSB _HCITransportUSB; //use USB as the transport to the radio
kenbumono	0:8d8481ed6d49	231
kenbumono	0:8d8481ed6d49	232	int OnBluetoothInsert(int device) {//install the HCI and start discovery, user callbacks are made to HciCalback
kenbumono	0:8d8481ed6d49	233	printf("Bluetooth inserted of %d\n",device);
kenbumono	0:8d8481ed6d49	234	u32 sramLen;
kenbumono	0:8d8481ed6d49	235	u8* sram = USBGetBuffer(&sramLen);
kenbumono	0:8d8481ed6d49	236	sram = (u8*)(((u32)sram + 1023) & ~1023);
kenbumono	0:8d8481ed6d49	237	SRAMPlacement* s = (SRAMPlacement*)sram;
kenbumono	0:8d8481ed6d49	238	_HCITransportUSB.Open(device,s->_hciBuffer,s->_aclBuffer);//setup buffers for USB host, incoming data goes first to HCIRecv and ACLRecv
kenbumono	0:8d8481ed6d49	239	RegisterSocketHandler(SOCKET_L2CAP,&App); //register the application::hci as handler for L2CAP events
kenbumono	0:8d8481ed6d49	240	RegisterSocketHandler(SOCKET_RFCOM, &rfcomm_manager);//set the RFCOMMManager as the RFCOM socket handler
kenbumono	0:8d8481ed6d49	241	if (RegisterSocketHandler(SOCKET_SDP, &SDP))
kenbumono	0:8d8481ed6d49	242	printf("Could not register SDP socket type\n");
kenbumono	0:8d8481ed6d49	243	App.Open(&_HCITransportUSB);//the callback is virtual
kenbumono	0:8d8481ed6d49	244	App.Inquiry();//start discovery of BT devices phase 0
kenbumono	0:8d8481ed6d49	245	return 0;
kenbumono	0:8d8481ed6d49	246	}
kenbumono	0:8d8481ed6d49	247
kenbumono	0:8d8481ed6d49	248	DigitalOut led(LED1), loop(LED2);
kenbumono	0:8d8481ed6d49	249
kenbumono	0:8d8481ed6d49	250	int comm = 0;
kenbumono	0:8d8481ed6d49	251	btserial incoming, outgoing;
kenbumono	0:8d8481ed6d49	252
kenbumono	0:8d8481ed6d49	253	Motor motorRight(p21, p5, p6, 1); // pwm, fwd, rev, can break
kenbumono	0:8d8481ed6d49	254	Motor motorLeft(p22, p7, p8, 1); // pwm, fwd, rev, can break
kenbumono	0:8d8481ed6d49	255
kenbumono	0:8d8481ed6d49	256	void motorSpeed(Motor& motor, int stick)
kenbumono	0:8d8481ed6d49	257	{
kenbumono	0:8d8481ed6d49	258	float speed;
kenbumono	0:8d8481ed6d49	259	if(100 <= stick && stick <= 155) {
kenbumono	0:8d8481ed6d49	260	motor.stop(0);
kenbumono	0:8d8481ed6d49	261	} else if(0 <= stick && stick <= 99) {
kenbumono	0:8d8481ed6d49	262	speed = (100 - stick) * 0.01;
kenbumono	0:8d8481ed6d49	263	motor.speed(speed);
kenbumono	0:8d8481ed6d49	264	printf("Motor:%f", speed);
kenbumono	0:8d8481ed6d49	265	} else {
kenbumono	0:8d8481ed6d49	266	speed = (155 - stick) * 0.01;
kenbumono	0:8d8481ed6d49	267	motor.speed(speed);
kenbumono	0:8d8481ed6d49	268	printf("Motor:%f", speed);
kenbumono	0:8d8481ed6d49	269	}
kenbumono	0:8d8481ed6d49	270	}
kenbumono	0:8d8481ed6d49	271
kenbumono	0:8d8481ed6d49	272	void echo(int socket, SocketState state, const u8* data, int len, void* userData) {
kenbumono	0:8d8481ed6d49	273	const u8 connack[] = { 0xbe, 0xef, 8, 'C','O','N','N','_','A','C','K', 0x11};
kenbumono	0:8d8481ed6d49	274	printf("Echo: socket %d, state %d, len=%d\n", socket, state, len);
kenbumono	0:8d8481ed6d49	275	if (state==SocketState_Open) {
kenbumono	0:8d8481ed6d49	276	if (len == 0) {
kenbumono	0:8d8481ed6d49	277	printf("Sending CONN_ACK\n");
kenbumono	0:8d8481ed6d49	278	Socket_Send(socket, connack, sizeof(connack));
kenbumono	0:8d8481ed6d49	279	} else {
kenbumono	0:8d8481ed6d49	280	//Socket_Send(socket, data, len);
kenbumono	0:8d8481ed6d49	281	printfBytes("echo:", data, len);
kenbumono	0:8d8481ed6d49	282
kenbumono	0:8d8481ed6d49	283	if(len >= 4) {
kenbumono	0:8d8481ed6d49	284	char temp[5] = {0};
kenbumono	0:8d8481ed6d49	285	memcpy(temp, data, 4);
kenbumono	0:8d8481ed6d49	286	int num = strtol(temp, NULL, 16);
kenbumono	0:8d8481ed6d49	287
kenbumono	0:8d8481ed6d49	288	int LeftStickY = num / 256;
kenbumono	0:8d8481ed6d49	289	int RightStickY = num - (LeftStickY * 256);
kenbumono	0:8d8481ed6d49	290
kenbumono	0:8d8481ed6d49	291	motorSpeed(motorLeft, LeftStickY);
kenbumono	0:8d8481ed6d49	292	motorSpeed(motorRight, RightStickY);
kenbumono	0:8d8481ed6d49	293	}
kenbumono	0:8d8481ed6d49	294	}
kenbumono	0:8d8481ed6d49	295	}
kenbumono	0:8d8481ed6d49	296	}
kenbumono	0:8d8481ed6d49	297
kenbumono	0:8d8481ed6d49	298	void TestShell() {
kenbumono	0:8d8481ed6d49	299	int n=0;
kenbumono	0:8d8481ed6d49	300	USBInit();
kenbumono	0:8d8481ed6d49	301	for (;;) {
kenbumono	0:8d8481ed6d49	302	switch (state) {
kenbumono	0:8d8481ed6d49	303	case 0: //inquiry and low-level connection
kenbumono	0:8d8481ed6d49	304	break;
kenbumono	0:8d8481ed6d49	305	case 1: {//initialisation
kenbumono	0:8d8481ed6d49	306	printf("Ready to open ports\n");
kenbumono	0:8d8481ed6d49	307	InitFtBtDeviceList();
kenbumono	0:8d8481ed6d49	308	int n = GetNrOfFtBtDevices();
kenbumono	0:8d8481ed6d49	309	printf("%d ft BT devices have been found\n", n);
kenbumono	0:8d8481ed6d49	310	if (n > 0) {
kenbumono	0:8d8481ed6d49	311	ftbtdev *d = GetFtUsbDeviceHandle(0);
kenbumono	0:8d8481ed6d49	312	if (d==0) printf("could not get device handle\n");
kenbumono	0:8d8481ed6d49	313	//int sock = OpenFtBtDevice(d);
kenbumono	0:8d8481ed6d49	314	}
kenbumono	0:8d8481ed6d49	315	state = 2;
kenbumono	0:8d8481ed6d49	316	comm = Socket_Listen(SOCKET_RFCOM, 1, echo, 0);
kenbumono	0:8d8481ed6d49	317	//incoming = new btserial(1);
kenbumono	0:8d8481ed6d49	318	}
kenbumono	0:8d8481ed6d49	319	break;
kenbumono	0:8d8481ed6d49	320	case 2://main loop
kenbumono	0:8d8481ed6d49	321	/* if (incoming->readable()>0){
kenbumono	0:8d8481ed6d49	322	int c= incoming->getc();
kenbumono	0:8d8481ed6d49	323	putc(c, stderr);
kenbumono	0:8d8481ed6d49	324	incoming->putc(c);
kenbumono	0:8d8481ed6d49	325	}
kenbumono	0:8d8481ed6d49	326	else if (incoming->readable()<0){
kenbumono	0:8d8481ed6d49	327	state = 3;
kenbumono	0:8d8481ed6d49	328	printf("end of session");
kenbumono	0:8d8481ed6d49	329	delete incoming;
kenbumono	0:8d8481ed6d49	330	}*/
kenbumono	0:8d8481ed6d49	331	break;
kenbumono	0:8d8481ed6d49	332	default:
kenbumono	0:8d8481ed6d49	333	break;
kenbumono	0:8d8481ed6d49	334	}
kenbumono	0:8d8481ed6d49	335	loop=1;
kenbumono	0:8d8481ed6d49	336	USBLoop();
kenbumono	0:8d8481ed6d49	337	loop=0;
kenbumono	0:8d8481ed6d49	338	n++;
kenbumono	0:8d8481ed6d49	339	if (n>=500000) {
kenbumono	0:8d8481ed6d49	340	n=0;
kenbumono	0:8d8481ed6d49	341	led = !led;
kenbumono	0:8d8481ed6d49	342	}
kenbumono	0:8d8481ed6d49	343	}
kenbumono	0:8d8481ed6d49	344	//printf("Dropped out of main loop!\n");
kenbumono	0:8d8481ed6d49	345	}
kenbumono	0:8d8481ed6d49	346
kenbumono	0:8d8481ed6d49	347	//********************************************************************************************************************************

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	05 Jul 2011
Imports:	15
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	6
Followers:	1

TestShell.cpp@0:8d8481ed6d49, 2011-07-05 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning