File content as of revision 1:b2063ffa4927:

/*
Copyright (c) 2011 Bart Janssens

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "USBHost.h"
#include "hci.h"
#include "Utils.h"
#include "ps3.h"

#include "mbed.h"



#define AUTOEVT(_class,_subclass,_protocol) (((_class) << 16) | ((_subclass) << 8) | _protocol)
#define PS3EVT AUTOEVT(CLASS_HID,0,0)
#define byteswap(x) ((x >> 8) | (x << 8))
#define device_baud 115200      //上のマイコンにするバンド数
#define send_device_rate 0.03   //割り込みレート


u8 ps3_data[48];
Serial order_device(p28,p27);//上のマイコンに反応させるための初期化宣言


//*************追加項目ピン設定
PwmOut led1(LED1);
PwmOut led2(LED2);
PwmOut led3(LED3);
PwmOut led4(LED4);
int joyL_x  =0;
int joyL_y  =0;
int joyR_x  =0;
int joyR_y  =0;
int L1      =0;
int R1      =0;
int up      =0; 
int down    =0;
int right   =0;
int left    =0;
int Xmove       =0;
int Ymove       =0;
int angle_omni  =0;

int speed_mode    =1;

void omni_move(double x_speed,double y_speed,double angle); 
char Buf[8];


//**************

//以下項目上への指令を送るためのBuf
/*
void send_device(){
    char Buf[8];
    Buf[0] = up;
    Buf[1] = down;
    Buf[2] = right;
    Buf[3] = left;
    Buf[4] = 0;
    Buf[5] = 0;
    order_device.putc(Buf[0]);
    order_device.putc(Buf[1]);
    order_device.putc(Buf[2]);
    order_device.putc(Buf[3]);
    order_device.putc(Buf[4]);
    order_device.putc(Buf[5]);
}
*/
//*********まで

Ps3USB::Ps3USB(int device, int configuration, int interfaceNumber)
{
        printf("Creating new sixaxis \r\n");
        _device = device;
        _configuration = configuration;
        _interfaceNumber = interfaceNumber;
        printf("device = %d configuration = %d interfaceNumber = %d\r\n", device, configuration, interfaceNumber);
        int result;
        int err;
        
        _count = 1;
        
        u8 abuffer[48] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
                          0x00, 0x02, 0xff, 0x27, 0x10, 0x00, 0x32, 0xff,
                          0x27, 0x10, 0x00, 0x32, 0xff, 0x27, 0x10, 0x00,
                          0x32, 0xff, 0x27, 0x10, 0x00, 0x32, 0x00, 0x00,
                          0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
                          0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
        memcpy(ledrumble,abuffer,48);
        
        EndpointDescriptor* ep;
                
        u8 buffer[255];
        err = GetDescriptor(_device,DESCRIPTOR_TYPE_CONFIGURATION,0,buffer,255);
        if (err < 0)
            printf("Failed to get descriptor\r\n");
            

    int len = buffer[2] | (buffer[3] << 8);
    u8* d = buffer;
    u8* end = d + len;
    while (d < end)
    {
        if (d[1] == DESCRIPTOR_TYPE_INTERFACE)
        {
            InterfaceDescriptor* id = (InterfaceDescriptor*)d;
            if (id->bInterfaceNumber == _interfaceNumber)
            {
                 d += d[0];
                while (d < end && d[1] != DESCRIPTOR_TYPE_INTERFACE)
                {
                    if (d[1] == DESCRIPTOR_TYPE_ENDPOINT)
                        ep = (EndpointDescriptor*)d;
                        
                        if (ep->bEndpointAddress == 0x02)  {
                            printf("PS3 input endpoint (0x02) found\r\n");
                            input_ep = 0x02;
                            
                        }
                        if (ep->bEndpointAddress == 0x81)  {
                            printf("PS3 output endpoint (0x81) found\r\n");
                            output_ep = 0x81;
                            //AddAutoEvent(device,id,(EndpointDescriptor*)d);
                        }
                    d += d[0];
                }
            }
        }
        d += d[0];
    }
    
    }
    
    int Ps3USB::Enable()
    {
        int err;
        
        u8 enable[4] = {0x42,0x0c,0x00,0x00};

        
        err = USBControlTransfer(_device, HOST_TO_DEVICE|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x03f4,0, enable, sizeof(enable), 0, 0 );
        //printf("set report result = %d\r\n", err);
        _count ++;
        if (_count == 25) _count = 1;
        
        err = USBInterruptTransfer(_device,output_ep,ps3_data,sizeof(ps3_data),PS3EventCallback,this);
        wait_ms(4);
        
        return 0;
    
    }
    
    
   
    
    int Ps3USB::SetPair(u8* bdAddr)
    {
        int err;
        
        u8 buf[8];
        u8 buf2[6];
      
        memcpy(buf2,bdAddr,6);
      
        buf[0] = 0x01;
        buf[1] = 0x00;
        buf[2] = buf2[0];
        buf[3] = buf2[1];
        buf[4] = buf2[2];
        buf[5] = buf2[3];
        buf[6] = buf2[4];
        buf[7] = buf2[5];
      
        //set Mac address    
        err = USBControlTransfer(_device, HOST_TO_DEVICE|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x03f5, 0, buf, sizeof(buf), 0, 0 );
        wait_ms(4);
        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);

        return 0;
    }
    
    int Ps3USB::ShowPair()
    {
        int err;
        
        u8 buf[8];
        //get Mac address    
        err = USBControlTransfer(_device, DEVICE_TO_HOST|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_GET_REPORT, 0x03f5, 0, buf, sizeof(buf), 0, 0 );
        wait_ms(4);
        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);

        
        return 0;
    }
    
    int Ps3USB::Led(int i)
    {
        int err;
        u8 ledpattern[7] = {0x02, 0x04, 0x08, 0x10, 0x12, 0x14, 0x18 };
        u8 buf[48];

        if (i < 7) ledrumble[9] = ledpattern[i];
        memcpy(buf, ledrumble, 48);
        
        err = USBControlTransfer(_device, HOST_TO_DEVICE|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x0201,0, buf, sizeof(buf), 0, 0 );
        wait_ms(4);
      
        return 0;
    }
    
    // left and right: duration and power, both from 0 to 255
    int Ps3USB::Rumble(u8 duration_right, u8 power_right, u8 duration_left, u8 power_left)
    {
        int err;
        u8 buf[48];
        
        memcpy(buf, ledrumble, 48);
        buf[1] = duration_right;
        buf[2] = power_right;
        buf[3] = duration_left;
        buf[4] = power_left;
         
        err = USBControlTransfer(_device, HOST_TO_DEVICE|REQUEST_TYPE_CLASS|RECIPIENT_INTERFACE, HID_REQUEST_SET_REPORT, 0x0201,0, buf, sizeof(buf), 0, 0 );
        wait_ms(4);
        
        return 0;
    }


    void PS3EventCallback(int device, int endpoint, int status, u8* data, int len, void* userData)
    {
        Ps3USB* t = (Ps3USB*)userData;

        t->_count ++;
        if (t->_count == 25) t->_count = 1;
        
        ParsePs3Result(data, sizeof(ps3report),t->_count);
        USBInterruptTransfer(device, endpoint , data, len, PS3EventCallback, userData);
        wait_ms(4);

    }
    /*
    void Connect(){
        order_device.baud(device_baud);//上のマイコンとのbaud指定
        tick_device.attach(&send_device,send_device_rate);//割り込み
        }
    */
    
    
    
    int ParsePs3Result(const u8* data,int len,int count)
    {   
        order_device.baud(device_baud);
        ps3report* _ps3report = (ps3report*)data;    
        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");
        printf("%3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %3d %4d %4d %4d %4d \r\n",
                            _ps3report->LeftStickX,
                            _ps3report->LeftStickY,
                            _ps3report->RightStickX,
                            _ps3report->RightStickY,
                            _ps3report->PressureUp,
                            _ps3report->PressureRight,
                            _ps3report->PressureDown,
                            _ps3report->PressureLeft,
                            _ps3report->PressureL2,
                            _ps3report->PressureR2,
                            _ps3report->PressureL1,
                            _ps3report->PressureR1,
                            _ps3report->PressureTriangle,
                            _ps3report->PressureCircle,
                            _ps3report->PressureCross,
                            _ps3report->PressureSquare,
                            (_ps3report->AccelX),
                            (_ps3report->AccelY),
                            (_ps3report->AccelZ),
                            (_ps3report->GyroZ));
        //printfBytes("data",data,len);
        joyL_x  =_ps3report->LeftStickX;
        joyL_y  =_ps3report->LeftStickY;
        joyR_x  =_ps3report->RightStickY;
        joyR_y  =_ps3report->RightStickX;
        L1      =_ps3report->PressureL1;
        R1      =_ps3report->PressureR1;
        up      =_ps3report->PressureUp;
        down    =_ps3report->PressureDown;
        right   =_ps3report->PressureRight;
        left    =_ps3report->PressureLeft;
        
        
        //デットバンドの設定
        if(120<=joyL_x&&joyL_x<=129)
        {
            joyL_x=126;
            }
        if(120<=joyL_y&&joyL_y<=129)
        {
            joyL_y=126;
            }
        if(120<=joyR_x&&joyR_x<=129)
        {
            joyR_x=126;
            }
        if(120<=joyR_y&&joyR_y<=129)
        {
            joyR_y=126;
            }
        
        
        //低速モードと高速モード設定
        if(L1>=100){
            speed_mode=2;
            }
        else{
            speed_mode=20;
            }
        
        
        order_device.printf("%d,%d,%d,%d\n", up, down, right, left);
        
        
        Xmove       =(joyL_x-126)*speed_mode;
        Ymove       =(joyL_y-126)*speed_mode;
        angle_omni  =(joyR_y-126)*speed_mode;
        
        omni_move(angle_omni,Ymove,Xmove);
        led1=joyL_x/255.0;
        led2=joyL_y/255.0;
        led3=joyR_y/255.0;
        
        printf("joyL_x:%d joyL_y:%d angle_omni:%d \r\n" , Xmove, Ymove, angle_omni);
                    }