Ps3USB.cpp

00001 /*
00002 Copyright (c) 2011 Bart Janssens
00003 
00004 Permission is hereby granted, free of charge, to any person obtaining a copy
00005 of this software and associated documentation files (the "Software"), to deal
00006 in the Software without restriction, including without limitation the rights
00007 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00008 copies of the Software, and to permit persons to whom the Software is
00009 furnished to do so, subject to the following conditions:
00010 
00011 The above copyright notice and this permission notice shall be included in
00012 all copies or substantial portions of the Software.
00013 
00014 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00015 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00016 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00017 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00018 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00019 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00020 THE SOFTWARE.
00021 */
00022 
00023 #include <stdio.h>
00024 #include <stdlib.h>
00025 #include <string.h>
00026 
00027 #include "USBHost.h"
00028 #include "hci.h"
00029 #include "Utils.h"
00030 #include "ps3.h"
00031 #include "motordriver.h"
00032 
00033 #include "mbed.h"
00034 
00035 
00036 
00037 #define AUTOEVT(_class,_subclass,_protocol) (((_class) << 16) | ((_subclass) << 8) | _protocol)
00038 #define PS3EVT AUTOEVT(CLASS_HID,0,0)
00039 #define byteswap(x) ((x >> 8) | (x << 8))
00040 
00041 u8 ps3_data[48];
00042 
00043 
00044 Ps3USB::Ps3USB(int device, int configuration, int interfaceNumber)
00045 {
00046         printf("Creating new sixaxis \r\n");
00047         _device = device;
00048         _configuration = configuration;
00049         _interfaceNumber = interfaceNumber;
00050         printf("device = %d configuration = %d interfaceNumber = %d\r\n", device, configuration, interfaceNumber);
00051         int result;
00052         int err;
00053         
00054         _count = 1;
00055         
00056         u8 abuffer[48] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00057                           0x00, 0x02, 0xff, 0x27, 0x10, 0x00, 0x32, 0xff,
00058                           0x27, 0x10, 0x00, 0x32, 0xff, 0x27, 0x10, 0x00,
00059                           0x32, 0xff, 0x27, 0x10, 0x00, 0x32, 0x00, 0x00,
00060                           0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00061                           0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
00062         memcpy(ledrumble,abuffer,48);
00063         
00064         EndpointDescriptor* ep;
00065                 
00066         u8 buffer[255];
00067         err = GetDescriptor(_device,DESCRIPTOR_TYPE_CONFIGURATION,0,buffer,255);
00068         if (err < 0)
00069             printf("Failed to get descriptor\r\n");
00070             
00071 
00072     int len = buffer[2] | (buffer[3] << 8);
00073     u8* d = buffer;
00074     u8* end = d + len;
00075     while (d < end)
00076     {
00077         if (d[1] == DESCRIPTOR_TYPE_INTERFACE)
00078         {
00079             InterfaceDescriptor* id = (InterfaceDescriptor*)d;
00080             if (id->bInterfaceNumber == _interfaceNumber)
00081             {
00082                  d += d[0];
00083                 while (d < end && d[1] != DESCRIPTOR_TYPE_INTERFACE)
00084                 {
00085                     if (d[1] == DESCRIPTOR_TYPE_ENDPOINT)
00086                         ep = (EndpointDescriptor*)d;
00087                         
00088                         if (ep->bEndpointAddress == 0x02)  {
00089                             printf("PS3 input endpoint (0x02) found\r\n");
00090                             input_ep = 0x02;
00091                             
00092                         }
00093                         if (ep->bEndpointAddress == 0x81)  {
00094                             printf("PS3 output endpoint (0x81) found\r\n");
00095                             output_ep = 0x81;
00096                             //AddAutoEvent(device,id,(EndpointDescriptor*)d);
00097                         }
00098                     d += d[0];
00099                 }
00100             }
00101         }
00102         d += d[0];
00103     }
00104     
00105     }
00106     
00107     int Ps3USB::Enable()
00108     {
00109         int err;
00110         
00111         u8 enable[4] = {0x42,0x0c,0x00,0x00};
00112 
00113         
00114         err = USBControlTransfer(_device, HOST_TO_DEVICE|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x03f4,0, enable, sizeof(enable), 0, 0 );
00115         //printf("set report result = %d\r\n", err);
00116         _count ++;
00117         if (_count == 25) _count = 1;
00118         
00119         err = USBInterruptTransfer(_device,output_ep,ps3_data,sizeof(ps3_data),PS3EventCallback,this);
00120         wait_ms(4);
00121         
00122         return 0;
00123     
00124     }
00125     
00126     
00127    
00128     
00129     int Ps3USB::SetPair(u8* bdAddr)
00130     {
00131         int err;
00132         
00133         u8 buf[8];
00134         u8 buf2[6];
00135       
00136         memcpy(buf2,bdAddr,6);
00137       
00138         buf[0] = 0x01;
00139         buf[1] = 0x00;
00140         buf[2] = buf2[0];
00141         buf[3] = buf2[1];
00142         buf[4] = buf2[2];
00143         buf[5] = buf2[3];
00144         buf[6] = buf2[4];
00145         buf[7] = buf2[5];
00146       
00147         //set Mac address    
00148         err = USBControlTransfer(_device, HOST_TO_DEVICE|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x03f5, 0, buf, sizeof(buf), 0, 0 );
00149         wait_ms(4);
00150         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);
00151 
00152         return 0;
00153     }
00154     
00155     int Ps3USB::ShowPair()
00156     {
00157         int err;
00158         
00159         u8 buf[8];
00160         //get Mac address    
00161         err = USBControlTransfer(_device, DEVICE_TO_HOST|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_GET_REPORT, 0x03f5, 0, buf, sizeof(buf), 0, 0 );
00162         wait_ms(4);
00163         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);
00164 
00165         
00166         return 0;
00167     }
00168     
00169     int Ps3USB::Led(int i)
00170     {
00171         int err;
00172         u8 ledpattern[7] = {0x02, 0x04, 0x08, 0x10, 0x12, 0x14, 0x18 };
00173         u8 buf[48];
00174 
00175         if (i < 7) ledrumble[9] = ledpattern[i];
00176         memcpy(buf, ledrumble, 48);
00177         
00178         err = USBControlTransfer(_device, HOST_TO_DEVICE|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x0201,0, buf, sizeof(buf), 0, 0 );
00179         wait_ms(4);
00180       
00181         return 0;
00182     }
00183     
00184     // left and right: duration and power, both from 0 to 255
00185     int Ps3USB::Rumble(u8 duration_right, u8 power_right, u8 duration_left, u8 power_left)
00186     {
00187         int err;
00188         u8 buf[48];
00189         
00190         memcpy(buf, ledrumble, 48);
00191         buf[1] = duration_right;
00192         buf[2] = power_right;
00193         buf[3] = duration_left;
00194         buf[4] = power_left;
00195          
00196         err = USBControlTransfer(_device, HOST_TO_DEVICE|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x0201,0, buf, sizeof(buf), 0, 0 );
00197         wait_ms(4);
00198         
00199         return 0;
00200     }
00201 
00202 
00203     void PS3EventCallback(int device, int endpoint, int status, u8* data, int len, void* userData)
00204     {
00205         Ps3USB* t = (Ps3USB*)userData;
00206 
00207         t->_count ++;
00208         if (t->_count == 25) t->_count = 1;
00209         
00210         ParsePs3Result(data, sizeof(ps3report),t->_count);
00211         USBInterruptTransfer(device, endpoint , data, len, PS3EventCallback, userData);
00212         wait_ms(4);
00213 
00214     }
00215 
00216         Motor A(p22, p7, p8, 1); // pwm, fwd, rev, can break
00217         Motor B(p21, p5, p6, 1); // pwm, fwd, rev, can break
00218 
00219     int ParsePs3Result(const u8* data,int len,int count)
00220     {
00221         float speed;
00222         ps3report* _ps3report = (ps3report*)data;
00223             
00224 /*        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");
00225         printf("%3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %4d %4d %4d %4d \r\n",
00226                             _ps3report->LeftStickX,
00227                             _ps3report->LeftStickY,
00228                             _ps3report->RightStickX,
00229                             _ps3report->RightStickY,
00230                             _ps3report->PressureUp,
00231                             _ps3report->PressureRight,
00232                             _ps3report->PressureDown,
00233                             _ps3report->PressureLeft,
00234                             _ps3report->PressureL2,
00235                             _ps3report->PressureR2,
00236                             _ps3report->PressureL1,
00237                             _ps3report->PressureR1,
00238                             _ps3report->PressureTriangle,
00239                             _ps3report->PressureCircle,
00240                             _ps3report->PressureCross,
00241                             _ps3report->PressureSquare,
00242                             (_ps3report->AccelX),
00243                             (_ps3report->AccelY),
00244                             (_ps3report->AccelZ),
00245                             (_ps3report->GyroZ));
00246                             
00247 */
00248         //printfBytes("data",data,len);
00249         if(100 <= _ps3report->LeftStickY && _ps3report->LeftStickY <= 155)
00250         {
00251             A.stop(0);
00252         } else if(0 <= _ps3report->LeftStickY && _ps3report->LeftStickY <= 99) {
00253             speed = (100 - _ps3report->LeftStickY) * 0.01;
00254             A.speed(speed);
00255             printf("A:%f", speed);
00256         } else {
00257             speed = (155 - _ps3report->LeftStickY) * 0.01;
00258             A.speed(speed);
00259             printf("A:%f", speed);
00260         }
00261         
00262         if(100 <= _ps3report->RightStickY && _ps3report->RightStickY <= 155)
00263         {
00264             B.stop(0);
00265         } else if(0 <= _ps3report->RightStickY && _ps3report->RightStickY <= 99) {
00266             speed = (100 - _ps3report->RightStickY) * 0.01;
00267             B.speed(speed);
00268             printf("B:%f", speed);
00269         } else {
00270             speed = (155 - _ps3report->RightStickY) * 0.01;
00271             B.speed(speed);
00272             printf("B:%f", speed);
00273         }
00274     }
00275 
00276