mbed_TANK_PS3 - Based on PS3_BlueUSB with reference to http://…

Users » kenbumono » Code » mbed_TANK_PS3

Yasuhiko YAMAMOTO / Mbed 2 deprecated mbed_TANK_PS3

Based on PS3_BlueUSB with reference to http://blog.goo.ne.jp/roboz80/e/10e7bf38d3a63b996ca2894e9fb5e3b6

Ps3USB.cpp@0:44619612f575, 2011-07-05 (annotated)

Committer:: kenbumono
Date:: Tue Jul 05 08:25:40 2011 +0000
Revision:: 0:44619612f575

Who changed what in which revision?

User	Revision	Line number	New contents of line
kenbumono	0:44619612f575	1	/*
kenbumono	0:44619612f575	2	Copyright (c) 2011 Bart Janssens
kenbumono	0:44619612f575	3
kenbumono	0:44619612f575	4	Permission is hereby granted, free of charge, to any person obtaining a copy
kenbumono	0:44619612f575	5	of this software and associated documentation files (the "Software"), to deal
kenbumono	0:44619612f575	6	in the Software without restriction, including without limitation the rights
kenbumono	0:44619612f575	7	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
kenbumono	0:44619612f575	8	copies of the Software, and to permit persons to whom the Software is
kenbumono	0:44619612f575	9	furnished to do so, subject to the following conditions:
kenbumono	0:44619612f575	10
kenbumono	0:44619612f575	11	The above copyright notice and this permission notice shall be included in
kenbumono	0:44619612f575	12	all copies or substantial portions of the Software.
kenbumono	0:44619612f575	13
kenbumono	0:44619612f575	14	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
kenbumono	0:44619612f575	15	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
kenbumono	0:44619612f575	16	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
kenbumono	0:44619612f575	17	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
kenbumono	0:44619612f575	18	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
kenbumono	0:44619612f575	19	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
kenbumono	0:44619612f575	20	THE SOFTWARE.
kenbumono	0:44619612f575	21	*/
kenbumono	0:44619612f575	22
kenbumono	0:44619612f575	23	#include <stdio.h>
kenbumono	0:44619612f575	24	#include <stdlib.h>
kenbumono	0:44619612f575	25	#include <string.h>
kenbumono	0:44619612f575	26
kenbumono	0:44619612f575	27	#include "USBHost.h"
kenbumono	0:44619612f575	28	#include "hci.h"
kenbumono	0:44619612f575	29	#include "Utils.h"
kenbumono	0:44619612f575	30	#include "ps3.h"
kenbumono	0:44619612f575	31	#include "motordriver.h"
kenbumono	0:44619612f575	32
kenbumono	0:44619612f575	33	#include "mbed.h"
kenbumono	0:44619612f575	34
kenbumono	0:44619612f575	35
kenbumono	0:44619612f575	36
kenbumono	0:44619612f575	37	#define AUTOEVT(_class,_subclass,_protocol) (((_class) << 16) \| ((_subclass) << 8) \| _protocol)
kenbumono	0:44619612f575	38	#define PS3EVT AUTOEVT(CLASS_HID,0,0)
kenbumono	0:44619612f575	39	#define byteswap(x) ((x >> 8) \| (x << 8))
kenbumono	0:44619612f575	40
kenbumono	0:44619612f575	41	u8 ps3_data[48];
kenbumono	0:44619612f575	42
kenbumono	0:44619612f575	43
kenbumono	0:44619612f575	44	Ps3USB::Ps3USB(int device, int configuration, int interfaceNumber)
kenbumono	0:44619612f575	45	{
kenbumono	0:44619612f575	46	printf("Creating new sixaxis \r\n");
kenbumono	0:44619612f575	47	_device = device;
kenbumono	0:44619612f575	48	_configuration = configuration;
kenbumono	0:44619612f575	49	_interfaceNumber = interfaceNumber;
kenbumono	0:44619612f575	50	printf("device = %d configuration = %d interfaceNumber = %d\r\n", device, configuration, interfaceNumber);
kenbumono	0:44619612f575	51	int result;
kenbumono	0:44619612f575	52	int err;
kenbumono	0:44619612f575	53
kenbumono	0:44619612f575	54	_count = 1;
kenbumono	0:44619612f575	55
kenbumono	0:44619612f575	56	u8 abuffer[48] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
kenbumono	0:44619612f575	57	0x00, 0x02, 0xff, 0x27, 0x10, 0x00, 0x32, 0xff,
kenbumono	0:44619612f575	58	0x27, 0x10, 0x00, 0x32, 0xff, 0x27, 0x10, 0x00,
kenbumono	0:44619612f575	59	0x32, 0xff, 0x27, 0x10, 0x00, 0x32, 0x00, 0x00,
kenbumono	0:44619612f575	60	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
kenbumono	0:44619612f575	61	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
kenbumono	0:44619612f575	62	memcpy(ledrumble,abuffer,48);
kenbumono	0:44619612f575	63
kenbumono	0:44619612f575	64	EndpointDescriptor* ep;
kenbumono	0:44619612f575	65
kenbumono	0:44619612f575	66	u8 buffer[255];
kenbumono	0:44619612f575	67	err = GetDescriptor(_device,DESCRIPTOR_TYPE_CONFIGURATION,0,buffer,255);
kenbumono	0:44619612f575	68	if (err < 0)
kenbumono	0:44619612f575	69	printf("Failed to get descriptor\r\n");
kenbumono	0:44619612f575	70
kenbumono	0:44619612f575	71
kenbumono	0:44619612f575	72	int len = buffer[2] \| (buffer[3] << 8);
kenbumono	0:44619612f575	73	u8* d = buffer;
kenbumono	0:44619612f575	74	u8* end = d + len;
kenbumono	0:44619612f575	75	while (d < end)
kenbumono	0:44619612f575	76	{
kenbumono	0:44619612f575	77	if (d[1] == DESCRIPTOR_TYPE_INTERFACE)
kenbumono	0:44619612f575	78	{
kenbumono	0:44619612f575	79	InterfaceDescriptor* id = (InterfaceDescriptor*)d;
kenbumono	0:44619612f575	80	if (id->bInterfaceNumber == _interfaceNumber)
kenbumono	0:44619612f575	81	{
kenbumono	0:44619612f575	82	d += d[0];
kenbumono	0:44619612f575	83	while (d < end && d[1] != DESCRIPTOR_TYPE_INTERFACE)
kenbumono	0:44619612f575	84	{
kenbumono	0:44619612f575	85	if (d[1] == DESCRIPTOR_TYPE_ENDPOINT)
kenbumono	0:44619612f575	86	ep = (EndpointDescriptor*)d;
kenbumono	0:44619612f575	87
kenbumono	0:44619612f575	88	if (ep->bEndpointAddress == 0x02) {
kenbumono	0:44619612f575	89	printf("PS3 input endpoint (0x02) found\r\n");
kenbumono	0:44619612f575	90	input_ep = 0x02;
kenbumono	0:44619612f575	91
kenbumono	0:44619612f575	92	}
kenbumono	0:44619612f575	93	if (ep->bEndpointAddress == 0x81) {
kenbumono	0:44619612f575	94	printf("PS3 output endpoint (0x81) found\r\n");
kenbumono	0:44619612f575	95	output_ep = 0x81;
kenbumono	0:44619612f575	96	//AddAutoEvent(device,id,(EndpointDescriptor*)d);
kenbumono	0:44619612f575	97	}
kenbumono	0:44619612f575	98	d += d[0];
kenbumono	0:44619612f575	99	}
kenbumono	0:44619612f575	100	}
kenbumono	0:44619612f575	101	}
kenbumono	0:44619612f575	102	d += d[0];
kenbumono	0:44619612f575	103	}
kenbumono	0:44619612f575	104
kenbumono	0:44619612f575	105	}
kenbumono	0:44619612f575	106
kenbumono	0:44619612f575	107	int Ps3USB::Enable()
kenbumono	0:44619612f575	108	{
kenbumono	0:44619612f575	109	int err;
kenbumono	0:44619612f575	110
kenbumono	0:44619612f575	111	u8 enable[4] = {0x42,0x0c,0x00,0x00};
kenbumono	0:44619612f575	112
kenbumono	0:44619612f575	113
kenbumono	0:44619612f575	114	err = USBControlTransfer(_device, HOST_TO_DEVICE\|REQUEST_TYPE_CLASS\|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x03f4,0, enable, sizeof(enable), 0, 0 );
kenbumono	0:44619612f575	115	//printf("set report result = %d\r\n", err);
kenbumono	0:44619612f575	116	_count ++;
kenbumono	0:44619612f575	117	if (_count == 25) _count = 1;
kenbumono	0:44619612f575	118
kenbumono	0:44619612f575	119	err = USBInterruptTransfer(_device,output_ep,ps3_data,sizeof(ps3_data),PS3EventCallback,this);
kenbumono	0:44619612f575	120	wait_ms(4);
kenbumono	0:44619612f575	121
kenbumono	0:44619612f575	122	return 0;
kenbumono	0:44619612f575	123
kenbumono	0:44619612f575	124	}
kenbumono	0:44619612f575	125
kenbumono	0:44619612f575	126
kenbumono	0:44619612f575	127
kenbumono	0:44619612f575	128
kenbumono	0:44619612f575	129	int Ps3USB::SetPair(u8* bdAddr)
kenbumono	0:44619612f575	130	{
kenbumono	0:44619612f575	131	int err;
kenbumono	0:44619612f575	132
kenbumono	0:44619612f575	133	u8 buf[8];
kenbumono	0:44619612f575	134	u8 buf2[6];
kenbumono	0:44619612f575	135
kenbumono	0:44619612f575	136	memcpy(buf2,bdAddr,6);
kenbumono	0:44619612f575	137
kenbumono	0:44619612f575	138	buf[0] = 0x01;
kenbumono	0:44619612f575	139	buf[1] = 0x00;
kenbumono	0:44619612f575	140	buf[2] = buf2[0];
kenbumono	0:44619612f575	141	buf[3] = buf2[1];
kenbumono	0:44619612f575	142	buf[4] = buf2[2];
kenbumono	0:44619612f575	143	buf[5] = buf2[3];
kenbumono	0:44619612f575	144	buf[6] = buf2[4];
kenbumono	0:44619612f575	145	buf[7] = buf2[5];
kenbumono	0:44619612f575	146
kenbumono	0:44619612f575	147	//set Mac address
kenbumono	0:44619612f575	148	err = USBControlTransfer(_device, HOST_TO_DEVICE\|REQUEST_TYPE_CLASS\|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x03f5, 0, buf, sizeof(buf), 0, 0 );
kenbumono	0:44619612f575	149	wait_ms(4);
kenbumono	0:44619612f575	150	printf("set Mac address to %02X:%02X:%02X:%02X:%02X:%02X , result = %d\r\n", buf[2], buf[3], buf[4], buf[5], buf[6], buf[7], err);
kenbumono	0:44619612f575	151
kenbumono	0:44619612f575	152	return 0;
kenbumono	0:44619612f575	153	}
kenbumono	0:44619612f575	154
kenbumono	0:44619612f575	155	int Ps3USB::ShowPair()
kenbumono	0:44619612f575	156	{
kenbumono	0:44619612f575	157	int err;
kenbumono	0:44619612f575	158
kenbumono	0:44619612f575	159	u8 buf[8];
kenbumono	0:44619612f575	160	//get Mac address
kenbumono	0:44619612f575	161	err = USBControlTransfer(_device, DEVICE_TO_HOST\|REQUEST_TYPE_CLASS\|RECIPIENT_INTERFACE, HID_REQUEST_GET_REPORT, 0x03f5, 0, buf, sizeof(buf), 0, 0 );
kenbumono	0:44619612f575	162	wait_ms(4);
kenbumono	0:44619612f575	163	printf("Mac address is set to %02X:%02X:%02X:%02X:%02X:%02X , result = %d\r\n",buf[2], buf[3], buf[4], buf[5], buf[6], buf[7], err);
kenbumono	0:44619612f575	164
kenbumono	0:44619612f575	165
kenbumono	0:44619612f575	166	return 0;
kenbumono	0:44619612f575	167	}
kenbumono	0:44619612f575	168
kenbumono	0:44619612f575	169	int Ps3USB::Led(int i)
kenbumono	0:44619612f575	170	{
kenbumono	0:44619612f575	171	int err;
kenbumono	0:44619612f575	172	u8 ledpattern[7] = {0x02, 0x04, 0x08, 0x10, 0x12, 0x14, 0x18 };
kenbumono	0:44619612f575	173	u8 buf[48];
kenbumono	0:44619612f575	174
kenbumono	0:44619612f575	175	if (i < 7) ledrumble[9] = ledpattern[i];
kenbumono	0:44619612f575	176	memcpy(buf, ledrumble, 48);
kenbumono	0:44619612f575	177
kenbumono	0:44619612f575	178	err = USBControlTransfer(_device, HOST_TO_DEVICE\|REQUEST_TYPE_CLASS\|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x0201,0, buf, sizeof(buf), 0, 0 );
kenbumono	0:44619612f575	179	wait_ms(4);
kenbumono	0:44619612f575	180
kenbumono	0:44619612f575	181	return 0;
kenbumono	0:44619612f575	182	}
kenbumono	0:44619612f575	183
kenbumono	0:44619612f575	184	// left and right: duration and power, both from 0 to 255
kenbumono	0:44619612f575	185	int Ps3USB::Rumble(u8 duration_right, u8 power_right, u8 duration_left, u8 power_left)
kenbumono	0:44619612f575	186	{
kenbumono	0:44619612f575	187	int err;
kenbumono	0:44619612f575	188	u8 buf[48];
kenbumono	0:44619612f575	189
kenbumono	0:44619612f575	190	memcpy(buf, ledrumble, 48);
kenbumono	0:44619612f575	191	buf[1] = duration_right;
kenbumono	0:44619612f575	192	buf[2] = power_right;
kenbumono	0:44619612f575	193	buf[3] = duration_left;
kenbumono	0:44619612f575	194	buf[4] = power_left;
kenbumono	0:44619612f575	195
kenbumono	0:44619612f575	196	err = USBControlTransfer(_device, HOST_TO_DEVICE\|REQUEST_TYPE_CLASS\|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x0201,0, buf, sizeof(buf), 0, 0 );
kenbumono	0:44619612f575	197	wait_ms(4);
kenbumono	0:44619612f575	198
kenbumono	0:44619612f575	199	return 0;
kenbumono	0:44619612f575	200	}
kenbumono	0:44619612f575	201
kenbumono	0:44619612f575	202
kenbumono	0:44619612f575	203	void PS3EventCallback(int device, int endpoint, int status, u8* data, int len, void* userData)
kenbumono	0:44619612f575	204	{
kenbumono	0:44619612f575	205	Ps3USB* t = (Ps3USB*)userData;
kenbumono	0:44619612f575	206
kenbumono	0:44619612f575	207	t->_count ++;
kenbumono	0:44619612f575	208	if (t->_count == 25) t->_count = 1;
kenbumono	0:44619612f575	209
kenbumono	0:44619612f575	210	ParsePs3Result(data, sizeof(ps3report),t->_count);
kenbumono	0:44619612f575	211	USBInterruptTransfer(device, endpoint , data, len, PS3EventCallback, userData);
kenbumono	0:44619612f575	212	wait_ms(4);
kenbumono	0:44619612f575	213
kenbumono	0:44619612f575	214	}
kenbumono	0:44619612f575	215
kenbumono	0:44619612f575	216	Motor A(p22, p7, p8, 1); // pwm, fwd, rev, can break
kenbumono	0:44619612f575	217	Motor B(p21, p5, p6, 1); // pwm, fwd, rev, can break
kenbumono	0:44619612f575	218
kenbumono	0:44619612f575	219	int ParsePs3Result(const u8* data,int len,int count)
kenbumono	0:44619612f575	220	{
kenbumono	0:44619612f575	221	float speed;
kenbumono	0:44619612f575	222	ps3report* _ps3report = (ps3report*)data;
kenbumono	0:44619612f575	223
kenbumono	0:44619612f575	224	/* if (count == 24) printf("LSX LSY RSX RSY UPA RPA DPA RPA L2 R2 L1 R1 TRI CIR CRO SQU ACX ACY ACZ GYZ \r\n");
kenbumono	0:44619612f575	225	printf("%3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %4d %4d %4d %4d \r\n",
kenbumono	0:44619612f575	226	_ps3report->LeftStickX,
kenbumono	0:44619612f575	227	_ps3report->LeftStickY,
kenbumono	0:44619612f575	228	_ps3report->RightStickX,
kenbumono	0:44619612f575	229	_ps3report->RightStickY,
kenbumono	0:44619612f575	230	_ps3report->PressureUp,
kenbumono	0:44619612f575	231	_ps3report->PressureRight,
kenbumono	0:44619612f575	232	_ps3report->PressureDown,
kenbumono	0:44619612f575	233	_ps3report->PressureLeft,
kenbumono	0:44619612f575	234	_ps3report->PressureL2,
kenbumono	0:44619612f575	235	_ps3report->PressureR2,
kenbumono	0:44619612f575	236	_ps3report->PressureL1,
kenbumono	0:44619612f575	237	_ps3report->PressureR1,
kenbumono	0:44619612f575	238	_ps3report->PressureTriangle,
kenbumono	0:44619612f575	239	_ps3report->PressureCircle,
kenbumono	0:44619612f575	240	_ps3report->PressureCross,
kenbumono	0:44619612f575	241	_ps3report->PressureSquare,
kenbumono	0:44619612f575	242	(_ps3report->AccelX),
kenbumono	0:44619612f575	243	(_ps3report->AccelY),
kenbumono	0:44619612f575	244	(_ps3report->AccelZ),
kenbumono	0:44619612f575	245	(_ps3report->GyroZ));
kenbumono	0:44619612f575	246
kenbumono	0:44619612f575	247	*/
kenbumono	0:44619612f575	248	//printfBytes("data",data,len);
kenbumono	0:44619612f575	249	if(100 <= _ps3report->LeftStickY && _ps3report->LeftStickY <= 155)
kenbumono	0:44619612f575	250	{
kenbumono	0:44619612f575	251	A.stop(0);
kenbumono	0:44619612f575	252	} else if(0 <= _ps3report->LeftStickY && _ps3report->LeftStickY <= 99) {
kenbumono	0:44619612f575	253	speed = (100 - _ps3report->LeftStickY) * 0.01;
kenbumono	0:44619612f575	254	A.speed(speed);
kenbumono	0:44619612f575	255	printf("A:%f", speed);
kenbumono	0:44619612f575	256	} else {
kenbumono	0:44619612f575	257	speed = (155 - _ps3report->LeftStickY) * 0.01;
kenbumono	0:44619612f575	258	A.speed(speed);
kenbumono	0:44619612f575	259	printf("A:%f", speed);
kenbumono	0:44619612f575	260	}
kenbumono	0:44619612f575	261
kenbumono	0:44619612f575	262	if(100 <= _ps3report->RightStickY && _ps3report->RightStickY <= 155)
kenbumono	0:44619612f575	263	{
kenbumono	0:44619612f575	264	B.stop(0);
kenbumono	0:44619612f575	265	} else if(0 <= _ps3report->RightStickY && _ps3report->RightStickY <= 99) {
kenbumono	0:44619612f575	266	speed = (100 - _ps3report->RightStickY) * 0.01;
kenbumono	0:44619612f575	267	B.speed(speed);
kenbumono	0:44619612f575	268	printf("B:%f", speed);
kenbumono	0:44619612f575	269	} else {
kenbumono	0:44619612f575	270	speed = (155 - _ps3report->RightStickY) * 0.01;
kenbumono	0:44619612f575	271	B.speed(speed);
kenbumono	0:44619612f575	272	printf("B:%f", speed);
kenbumono	0:44619612f575	273	}
kenbumono	0:44619612f575	274	}
kenbumono	0:44619612f575	275
kenbumono	0:44619612f575	276

Repository toolbox

Export to desktop IDE

Build repository

Repository details

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

Ps3USB.cpp@0:44619612f575, 2011-07-05 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning