File content as of revision 2:2c4daaf8b15c:

/*
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 <TextLCD.h>//for orange borad
#include "C12832_lcd.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 M_1_CW_MIN 63
#define M_2_CW_MIN 191
#define M_1_CCW_MIN 65
#define M_2_CCW_MIN 193

#define M_1_CW_MAX 1
#define M_2_CW_MAX 128
#define M_1_CCW_MAX 127
#define M_2_CCW_MAX 255


#define M_1_STOP 64
#define M_2_STOP 192




u8 ps3_data[48];
Serial sabertooth1(p13,p14);
Serial sabertooth2(p28,p27);
Serial sabertoothArm(p9,p10);

Serial pro(USBTX, USBRX);
//TextLCD lcd(p24,p26,p27,p28,p29,p30);//for orange board
C12832_LCD lcd;


PwmOut rgb(p23);

AnalogIn pot1(p19);

DigitalOut AIN1(p15);
DigitalOut AIN2(p16);
DigitalOut BIN2(p17);
DigitalOut BIN1(p18);
DigitalOut STBY(p12);
PwmOut PWMA(p21);
PwmOut PWMB(p22);


int map(int val, int b_min, int b_max, int a_min, int a_max)
{
    return (val - b_max) * (a_min - a_max) / (b_min - b_max) + a_max;
}

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);

}

int ParsePs3Result(const u8* data,int len,int count)
{
    ps3report* _ps3report = (ps3report*)data;

    //int pot=pot2.read();
    float pot =pot1.read()*100.0;
    int pot_int=(int)pot+1;
    
    sabertooth1.baud(19200);
    sabertooth2.baud(19200);
    sabertoothArm.baud(19200);

    u8 P_UP   =_ps3report->PressureUp;
    u8 P_DOWN =_ps3report->PressureDown;
    u8 P_LEFT =_ps3report->PressureLeft;
    u8 P_RIGHT=_ps3report->PressureRight;

    u8 L2=_ps3report->PressureL2;
    u8 R2=_ps3report->PressureR2;

    u8 L1=_ps3report->PressureL1;
    u8 R1=_ps3report->PressureR1;
    
    
    u8 Cir=_ps3report->PressureCircle;
    u8 Tri=_ps3report->PressureTriangle;
    u8 Cro=_ps3report->PressureCross;
    u8 Squ=_ps3report->PressureSquare;
    

    int sb1_1=0;
    int sb1_2=0;
    int sb2_1=0;
    int sb2_2=0;
    
    int arm_val=0;

    u8 M_1_CW_BIG = M_1_CW_MAX;
    u8 M_2_CW_BIG = M_2_CW_MAX;
    u8 M_1_CCW_BIG = M_1_CCW_MAX;
    u8 M_2_CCW_BIG = M_2_CCW_MAX;

    bool up_lock=false;
    bool down_lock=false;
    bool left_lock=false;
    bool right_lock=false;
    bool clockwise_lock=false;
    bool c_clockwise_lock=false;
    bool left_stick_lock=false;

    M_1_CW_BIG=map(pot_int,0,100,62,1);
    M_2_CW_BIG=map(pot_int,0,100,190,128);
    M_1_CCW_BIG=map(pot_int,0,100,66,127);
    M_2_CCW_BIG=map(pot_int,0,100,194,255);

    int inPin1=0;
    int inPin2=0;
    int inSTBY=0;



    if(P_UP==0&&P_DOWN==0&&P_LEFT==0&&P_RIGHT==0) {
//        up_lock=false;
//        down_lock=false;
//        left_lock=false;
//        right_lock=false;
//        clockwise_lock=false;
//        c_clockwise_lock=false;

        sb1_1=M_1_STOP;
        sb1_2=M_2_STOP;
        sb2_1=M_1_STOP;
        sb2_2=M_2_STOP;

        sabertooth1.putc(sb1_1);
        sabertooth1.putc(sb1_2);

        sabertooth2.putc(sb2_1);
        sabertooth2.putc(sb2_2);


    }
    if(P_UP!=0 && !down_lock && !left_lock && !right_lock && !clockwise_lock && !c_clockwise_lock && !left_stick_lock) {
//        up_lock=true;

        sb1_1=map(P_UP,0,255,M_1_CW_MIN,M_1_CW_BIG);
        sb1_2=map(P_UP,0,255,M_2_CW_MIN,M_2_CW_BIG);
        sb2_1=map(P_UP,0,255,M_1_CW_MIN,M_1_CW_BIG);
        sb2_2=map(P_UP,0,255,M_2_CW_MIN,M_2_CW_BIG);

        sabertooth1.putc(sb1_1);
        sabertooth1.putc(sb1_2);

        sabertooth2.putc(sb2_1);
        sabertooth2.putc(sb2_2);
    }
    if(P_DOWN!=0 && !up_lock && !left_lock && !right_lock && !clockwise_lock && !c_clockwise_lock && !left_stick_lock) {
//        down_lock=true;

        sb1_1=map(P_DOWN,0,255,M_1_CCW_MIN,M_1_CCW_BIG);
        sb1_2=map(P_DOWN,0,255,M_2_CCW_MIN,M_2_CCW_BIG);
        sb2_1=map(P_DOWN,0,255,M_1_CCW_MIN,M_1_CCW_BIG);
        sb2_2=map(P_DOWN,0,255,M_2_CCW_MIN,M_2_CCW_BIG);

        sabertooth1.putc(sb1_1);
        sabertooth1.putc(sb1_2);

        sabertooth2.putc(sb2_1);
        sabertooth2.putc(sb2_2);
    }
    if(P_LEFT!=0 && !up_lock && !down_lock && !right_lock && !clockwise_lock && !c_clockwise_lock && !left_stick_lock) {
//        left_lock=true;

        sb1_1=map(P_LEFT,0,255,M_1_CCW_MIN,M_1_CCW_BIG);
        sb1_2=map(P_LEFT,0,255,M_2_CW_MIN,M_2_CW_BIG);
        sb2_1=map(P_LEFT,0,255,M_1_CW_MIN,M_1_CW_BIG);
        sb2_2=map(P_LEFT,0,255,M_2_CCW_MIN,M_2_CCW_BIG);

        sabertooth1.putc(sb1_1);
        sabertooth1.putc(sb1_2);

        sabertooth2.putc(sb2_1);
        sabertooth2.putc(sb2_2);
    }
    if(P_RIGHT!=0 && !up_lock && !down_lock && !left_lock && !clockwise_lock && !c_clockwise_lock && !left_stick_lock) {
//        right_lock=true;

        sb1_1=map(P_RIGHT,0,255,M_1_CW_MIN,M_1_CW_BIG);
        sb1_2=map(P_RIGHT,0,255,M_2_CCW_MIN,M_2_CCW_BIG);
        sb2_1=map(P_RIGHT,0,255,M_1_CCW_MIN,M_1_CCW_BIG);
        sb2_2=map(P_RIGHT,0,255,M_2_CW_MIN,M_2_CW_BIG);

        sabertooth1.putc(sb1_1);
        sabertooth1.putc(sb1_2);

        sabertooth2.putc(sb2_1);
        sabertooth2.putc(sb2_2);
    }
    if(L1!=0 && !up_lock && !down_lock && !left_lock && !right_lock && !clockwise_lock && !left_stick_lock) {
//        c_clockwise_lock=true;

        sb1_1=map(L1,0,255,M_1_CCW_MIN,M_1_CCW_BIG);
        sb1_2=map(L1,0,255,M_2_CCW_MIN,M_2_CCW_BIG);
        sb2_1=map(L1,0,255,M_1_CW_MIN,M_1_CW_BIG);
        sb2_2=map(L1,0,255,M_2_CW_MIN,M_2_CW_BIG);

        sabertooth1.putc(sb1_1);
        sabertooth1.putc(sb1_2);

        sabertooth2.putc(sb2_1);
        sabertooth2.putc(sb2_2);
    }
    if(R1!=0 && !up_lock && !down_lock && !left_lock && !right_lock && !c_clockwise_lock &&!left_stick_lock) {
//        clockwise_lock=true;

        sb1_1=map(R1,0,255,M_1_CW_MIN,M_1_CW_BIG);
        sb1_2=map(R1,0,255,M_2_CW_MIN,M_2_CW_BIG);
        sb2_1=map(R1,0,255,M_1_CCW_MIN,M_1_CCW_BIG);
        sb2_2=map(R1,0,255,M_2_CCW_MIN,M_2_CCW_BIG);

        sabertooth1.putc(sb1_1);
        sabertooth1.putc(sb1_2);

        sabertooth2.putc(sb2_1);
        sabertooth2.putc(sb2_2);
    }
    if(L2==255) {
        sb1_1=M_1_STOP;
        sb1_2=M_2_STOP;

        sabertooth1.putc(sb1_1);
        sabertooth1.putc(sb1_2);
    }
    if(R2==255) {
        sb2_1=M_1_STOP;
        sb2_2=M_2_STOP;

        sabertooth2.putc(sb2_1);
        sabertooth2.putc(sb2_2);
    }
    if(P_UP!=0 && P_LEFT!=0) {
        sb1_1=M_1_STOP;
        sb1_2=map(P_UP+P_LEFT,0,510,M_2_CW_MIN,M_2_CW_BIG);
        sb2_1=map(P_UP+P_LEFT,0,510,M_1_CW_MIN,M_1_CW_BIG);
        sb2_2=M_2_STOP;

        sabertooth1.putc(sb1_1);
        sabertooth1.putc(sb1_2);

        sabertooth2.putc(sb2_1);
        sabertooth2.putc(sb2_2);
    }
    if(P_DOWN!=0 && P_LEFT!=0) {
        sb1_1=map(P_DOWN+P_LEFT,0,510,M_1_CCW_MIN,M_1_CCW_BIG);
        sb1_2=M_2_STOP;
        sb2_1=M_1_STOP;
        sb2_2=map(P_DOWN+P_LEFT,0,510,M_2_CCW_MIN,M_2_CCW_BIG);

        sabertooth1.putc(sb1_1);
        sabertooth1.putc(sb1_2);

        sabertooth2.putc(sb2_1);
        sabertooth2.putc(sb2_2);
    }
    if(P_UP!=0 && P_RIGHT!=0) {
        sb1_1=map(P_UP+P_RIGHT,0,510,M_1_CW_MIN,M_1_CW_BIG);
        sb1_2=M_2_STOP;
        sb2_1=M_1_STOP;
        sb2_2=map(P_UP+P_RIGHT,0,510,M_2_CW_MIN,M_2_CW_BIG);

        sabertooth1.putc(sb1_1);
        sabertooth1.putc(sb1_2);

        sabertooth2.putc(sb2_1);
        sabertooth2.putc(sb2_2);
    }
    if(P_DOWN!=0 && P_RIGHT!=0) {
        sb1_1=M_1_STOP;
        sb1_2=map(P_DOWN+P_RIGHT,0,510,M_2_CCW_MIN,M_2_CCW_BIG);
        sb2_1=map(P_DOWN+P_RIGHT,0,510,M_1_CCW_MIN,M_1_CCW_BIG);
        sb2_2=M_2_STOP;

        sabertooth1.putc(sb1_1);
        sabertooth1.putc(sb1_2);

        sabertooth2.putc(sb2_1);
        sabertooth2.putc(sb2_2);
    }
    if(Tri!=0){
        arm_val=map(Tri,0,255,M_1_CCW_MIN,M_1_CCW_MAX);
        sabertoothArm.putc(arm_val);
    }
    if(Cro!=0){
        arm_val=map(Cro,0,255,M_1_CW_MIN,M_1_CW_MAX);
        sabertoothArm.putc(arm_val);
    }
    if(Tri==0 && Cro==0){
        arm_val=M_1_STOP;
        sabertoothArm.putc(arm_val);
    }
    if(Cir!=0){
        inSTBY=1;
        STBY=inSTBY;
        AIN1=1;
        AIN2=0;
        BIN1=0;
        BIN2=0;
        PWMA=Cir;
    }
    if(Squ!=0){
        inSTBY=1;
        STBY=inSTBY;
        BIN1=1;
        BIN2=0;
        AIN1=0;
        AIN2=0;
        PWMB=Squ;
    }
    if(Cir==0 && Squ==0){
        inSTBY=0;
        STBY=inSTBY;
    }
    if(Cir!=0 && Squ!=0){
        inSTBY=0;
        STBY=inSTBY;
    }
    
    lcd.locate(1,1);
//    lcd.printf("1_1=%u 1_2=%u 2_1=%u \n 2_2=%u \n L2=%u R2=%u L1=%u R1=%u",sb1_1,sb1_2,sb2_1,sb2_2,L2,R2,L1,R1);
    lcd.printf("speed=%d\n",pot_int);
    if(pro.writeable()>0) {
        pro.printf("%u,%u,%u,%u,%u,%u,%u,%u,%d\n",sb1_1,sb1_2,sb2_1,sb2_2,L2,R2,L1,R1,pot_int);
    }
//        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);
}