File content as of revision 13:d7c1aae2b48f:

#include "mbed.h"
#include "USBHostXpad.h"
#include "stm32f4xx_flash.h"

DigitalOut myled(LED1);
Serial pc(USBTX, USBRX); // tx, rx
DigitalInOut data(PA_8);
DigitalIn button(PC_13); // eventually code to set controls

/**
@namespace AXYB
@brief Integer for storing the hex of the A X Y B buttons
@brief XPad returns a 4 digit hex for all buttons- AXYB buttons are stored in first value
@param A - given as a 1
@param B - given as a 2
@param X - given as a 4
@param Y - given as a 8
*/
uint8_t AXYB=0x0;
/**
@namespace XLBRB
@brief Integer for storing the hex of the LB,RB and center X buttons
@brief XPad returns a 4 digit hex for all buttons- XLBRB buttons are stored in second value
@param LB - given as a 1
@param R - given as a 2
@param X - given as a 4
*/
uint8_t XLBRB=0x0;
 
/**
@namespace bkStrtLCRC
@brief Integer for storing the hex of the Left analog button,Right analog button,back and start buttons
@brief XPad returns a 4 digit hex for all buttons- bkStrtLCRC buttons are stored in third value
@param start - given as a 1
@param back - given as a 2
@param LC - given as a 4
@param RC - given as a 8
*/
uint8_t bkStrtLCRC=0x0;
/**
@namespace DPad
@brief Integer for storing the hex of the Directional buttons
@brief XPad returns a 4 digit hex for all buttons- DPad buttons are stored in fourth value
@param Up - given as a 1
@param Down - given as a 2
@param Left - given as a 4
@param Right - given as a 8
*/
uint8_t DPad=0x0;
/**
@namespace LSY
@brief float for storing the value of the Left Analogue Stick's Y axis
@brief XPad returns a value between -32768(down) and 32767(up)
@there is a deadzone between around -4000 and 4000 where the value returned is not consistent when in the fixed position(assummed 0,0 point)
*/
char LSY=0x0;
/**
@namespace LSX
@brief float for storing the value of the Left Analogue Stick's X axis
@brief XPad returns a value between -32768(left) and 32767(right)
@there is a deadzone between around -4000 and 4000 where the value returned is not consistent when in the fixed position(assummed 0,0 point)
*/
char LSX=0x0;
/**
@namespace RSY
@brief float for storing the value of the Right Analogue Stick's Y axis
@brief XPad returns a value between -32768() and 32767(up)
@there is a deadzone between around -4000 and 4000 where the value returned is not consistent when in the fixed position(assummed 0,0 point)
*/
float RSY=0x0;
/**
@namespace RSX
@brief float for storing the value of the Right Analogue Stick's X axis
@brief XPad returns a value between -32768(left) and 32767(right)
@there is a deadzone between around -4000 and 4000 where the value returned is not consistent when in the fixed position(assummed 0,0 point)
*/
float RSX=0x0;
/**
@namespace sN
@brief float for storing the stick Normalising value
@brief makes the range of the sticks -80 to 80
*/
const float sN=0.00244140625;//(80/32768)
/**
@namespace Lt
@brief float for storing the value of the Left trigger
@brief XPad returns a value between 0(not pressed) and 255(fully pressed)
@
*/
float Lt=0x0;
/**
@namespace Rt
@brief float for storing the value of the Left trigger
@brief XPad returns a value between 0(not pressed) and 255(fully pressed)
@
*/
float Rt=0x0;
/**
@namespace tN
@brief float for storing the trigger Normalising value
@brief makes the range of the triggers 0 to 10
*/
const float tN=0.03921568627;//(10/255)
const int dead_zone = 5;
const int sensitivity = 66;
const int TRIGGER_THRESHOLD = 5;

char reverse(char b) 
{
   b = (b & 0xF0) >> 4 | (b & 0x0F) << 4;
   b = (b & 0xCC) >> 2 | (b & 0x33) << 2;
   b = (b & 0xAA) >> 1 | (b & 0x55) << 1;
   return b;
}

enum STATE {NORMAL=0, DPAD_UP, DPAD_DOWN, DPAD_LEFT, DPAD_RIGHT, BUTTON_START, BUTTON_B, BUTTON_A, C_UP, C_DOWN, C_LEFT, C_RIGHT, BUTTON_L, BUTTON_R, BUTTON_Z};
enum XPAD_BUTTON {XPAD_DUP, XPAD_DDOWN, XPAD_DLEFT, XPAD_DRIGHT, XPAD_X, XPAD_Y, XPAD_A, XPAD_B, XPAD_LB, XPAD_RB, XPAD_LT, XPAD_RT, XPAD_BACK, XPAD_START, XPAD_LAB, XPAD_RAB};

uint8_t state = NORMAL;
bool XpadButtonPressed = false;

const uint64_t      A_MASK      = 0x00001000,
                    B_MASK      = 0x00002000,
                    X_MASK      = 0x00004000,
                    Y_MASK      = 0x00008000,
                    LB_MASK     = 0x00000100,
                    RB_MASK     = 0x00000200,
                    START_MASK  = 0x00000010,
                    BACK_MASK   = 0x00000020,
                    LAB_MASK    = 0x00000040,
                    RAB_MASK    = 0x00000080,
                    DUP_MASK    = 0x00000001,
                    DDOWN_MASK  = 0x00000002,
                    DLEFT_MASK  = 0x00000004,
                    DRIGHT_MASK = 0x00000008,
                    // the next two masks are special extensions to support triggers
                    LT_MASK     = 0x00010000,
                    RT_MASK     = 0x00020000;

extern "C" void my_wait_us_asm (int n);
void LoadControls();
void SaveControls();

struct __attribute__((packed)) N64ControllerData // all bits are in the correct order... except for the analog
{
    unsigned int a : 1; // 1 bit wide
    unsigned int b : 1;
    unsigned int z : 1;
    unsigned int start : 1;
    unsigned int up : 1;
    unsigned int down : 1;
    unsigned int left : 1;
    unsigned int right : 1;
    
    unsigned int dummy1 : 1;
    unsigned int dummy2 : 1;
    unsigned int l : 1;
    unsigned int r : 1;
    unsigned int c_up : 1;
    unsigned int c_down : 1;
    unsigned int c_left : 1;
    unsigned int c_right : 1;
    
    char x_axis;
    
    char y_axis;

} n64_data;

struct __attribute__((packed)) XpadControls
{
    uint64_t a;
    uint64_t b;
    uint64_t z;
    uint64_t start;
    uint64_t up;
    uint64_t down;
    uint64_t left;
    uint64_t right;
    uint64_t l;
    uint64_t r;
    uint64_t c_up;
    uint64_t c_down;
    uint64_t c_left;
    uint64_t c_right;
    
    XpadControls()
    {
        LoadControls();
    }

    void PrintControls()
    {
        pc.printf("The mask for a is: 0x%X\r\n",a);
        pc.printf("The mask for start is: 0x%X\r\n",start);
    }
} xpc;

const int SAVE_ADDR = 0x0800C000; // sector 3
XpadControls* saveData = (XpadControls*)SAVE_ADDR; 

// linear search, find first button pressed
// if the user pressed 2 buttons, its the user's fault
uint64_t DetectButton()
{
    if(AXYB != 0)
    {
        if(AXYB & 0x01) // a
        {
            return A_MASK;
        }
        else if((AXYB >> 1) & 0x01) // b
        {
            return B_MASK;
        }
        else if((AXYB >> 2) & 0x01) // x
        {
            return X_MASK;
        }
        else if((AXYB >> 3) & 0x01) // y
        {
            return Y_MASK;
        }
    }
    else if(XLBRB != 0)
    {
        if((XLBRB >> 1) & 0x01) // right bumper
        {
            return RB_MASK;
        }
        else if(XLBRB & 0x01) // left bumper
        {
            return LB_MASK;
        }
        // the Xbox ("X") button is ignored in this firmware
    }
    else if(bkStrtLCRC != 0)
    {
        if(bkStrtLCRC & 0x01) // start
        {
            return START_MASK;
        }
        else if(bkStrtLCRC & 0x02) // back
        {
            return BACK_MASK;
        }
        else if(bkStrtLCRC & 0x04) // L analog button
        {
            return LAB_MASK;
        }
        else if(bkStrtLCRC & 0x08) // R analog button
        {
            return RAB_MASK;
        }
    }
    else if(DPad != 0)
    {
        if(DPad & 0x01) // DPad Up
        {
            return DUP_MASK;
        }
        else if(DPad & 0x02) // DPad Down
        {
            return DDOWN_MASK;
        }
        else if(DPad & 0x04) // DPad Left
        {
            return DLEFT_MASK;
        }
        else if(DPad & 0x08) // DPad Right
        {
            return DRIGHT_MASK;
        }
    }
    else if(Lt > TRIGGER_THRESHOLD)
    {
        return LT_MASK;
    }
    else if(Rt > TRIGGER_THRESHOLD)
    {
        return RT_MASK;
    }
    
    return 0; // no button was pressed
}

void ChangeButtonMapping(uint64_t bt)
{
    // analog settings must be hardcoded, cannot change on the fly
    
    if(state == DPAD_UP) // state = 1 --> dpad up
    {
        xpc.up = bt;
    }
    else if(state == DPAD_DOWN) // state = 2 --> dpad down
    {
        xpc.down = bt;
    }
    else if(state == DPAD_LEFT) // state = 3 --> dpad left
    {
        xpc.left = bt;
    }
    else if(state == DPAD_RIGHT) // state = 4 --> dpad right
    {
        xpc.right = bt;
    }
    else if(state == BUTTON_START) // state = 5 --> start
    {
        xpc.start = bt;
    }
    else if(state == BUTTON_B) // state = 6 --> B
    {
        xpc.b = bt;
    }
    else if(state == BUTTON_A) // state = 7 --> A
    {
        xpc.a = bt;
    }
    else if(state == C_UP) // state = 8 --> c up
    {
        xpc.c_up = bt;
    }
    else if(state == C_DOWN) // state = 9 --> c down
    {
        xpc.c_down = bt;
    }
    else if(state == C_LEFT) // state = 10 --> c left
    {
        xpc.c_left = bt;
    }
    else if(state == C_RIGHT) // state = 11 --> c right
    {
        xpc.c_right = bt;
    }
    else if(state == BUTTON_L) // state = 12 --> L
    {
        xpc.l = bt;
    }
    else if(state == BUTTON_R) // state = 13 --> R
    {
        xpc.r = bt;
    }
    else if(state == BUTTON_Z) // state = 14 --> Z
    {
        xpc.z = bt;
    }
}

void SaveControls()
{
    FLASH_Unlock(); //unlock flash writing
    FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | 
                  FLASH_FLAG_PGAERR | FLASH_FLAG_PGPERR|FLASH_FLAG_PGSERR);
    FLASH_EraseSector(FLASH_Sector_3,VoltageRange_3); // 0x0800C000 - 0x0800FFFF
    
    uint32_t* data = (uint32_t*)&xpc;
    
    // Total size is 112 bytes
    // Each word is 4 bytes, so the total size is 28 words
    // Note: ProgramDoubleWord requires a higher voltage, so we must do one word at a time
    for(int ct = 0;ct < 28;ct++)
    {
        FLASH_ProgramWord(SAVE_ADDR+(ct*4),*data); //each SAVE_ADDR+4 is 4 bytes because it is a memory address
        data++; // each data+1 is 4 bytes because it is a 32 bit data type
    }
        
    FLASH_Lock(); // lock it back up
}

void LoadControls()
{
    memcpy(&xpc,saveData,sizeof(XpadControls));
    pc.printf("Controls have been loaded!\r\n");
}

void AdvanceState()
{
    state++;
    if(state >= 15) // we're done mapping the controls
    {
        SaveControls(); // write directly to flash
        state = NORMAL; // back to normal controller operation
    }
}

void onXpadEvent (int buttons, int stick_lx, int stick_ly, int stick_rx, int stick_ry, int trigger_l, int trigger_r)
{
    //pc.printf("A button was pressed!\r\n",state); // DEBUG
    AXYB=buttons>>12;
    XLBRB=(buttons&0x0f00)>>8;
    bkStrtLCRC=(buttons&0x00f0)>>4;
    DPad=buttons&0x000f;
    
    //pc.printf("AXYB: %u, XLBRB, %u, bkStrtLCRC %u, DPad, %u\r\n",AXYB,XLBRB,bkStrtLCRC,DPad);
    
    // normalize the analog stick values to be 80 max
    //LSY=(char)((int)(stick_ly*sN));
    //LSX=(char)((int)(stick_lx*sN));
    //RSY=stick_ry*sN;
    //RSX=stick_rx*sN;
    
    // normalize the trigger values to be 10 max
    Lt=trigger_l*tN;
    Rt=trigger_r*tN;
    
    if(state == 0)
    {
        memset(&n64_data,0,4); // clear controller state
        
        uint64_t buttons_and_triggers = buttons;
        
        if(Lt > TRIGGER_THRESHOLD)
        {
            buttons_and_triggers |= LT_MASK;
        }
        if(Rt > TRIGGER_THRESHOLD)
        {
            buttons_and_triggers |= RT_MASK;
        }
        
        if(buttons_and_triggers & xpc.up)
        {
            n64_data.up = 1;
        }
        if(buttons_and_triggers & xpc.down)
        {
            n64_data.down = 1;
        }
        if(buttons_and_triggers & xpc.left)
        {
            n64_data.left = 1;
        }
        if(buttons_and_triggers & xpc.right)
        {
            n64_data.right = 1;
        }
        if(buttons_and_triggers & xpc.c_up)
        {
            n64_data.c_up = 1;
        }
        if(buttons_and_triggers & xpc.c_down)
        {
            n64_data.c_down = 1;
        }
        if(buttons_and_triggers & xpc.c_left)
        {
            n64_data.c_left = 1;
        }
        if(buttons_and_triggers & xpc.c_right)
        {
            n64_data.c_right = 1;
        }
        if(buttons_and_triggers & xpc.l)
        {
            n64_data.l = 1;
        }
        if(buttons_and_triggers & xpc.r)
        {
            n64_data.r = 1;
        }
        if(buttons_and_triggers & xpc.z)
        {
            n64_data.z = 1;
        }
        if(buttons_and_triggers & xpc.a)
        {
            n64_data.a = 1;
        }
        if(buttons_and_triggers & xpc.b)
        {
            n64_data.b = 1;
        }
        if(buttons_and_triggers & xpc.start)
        {
            n64_data.start = 1;
        }
        
        // LD code, val is the x_axis value from -32k to +32k
        // seems pretty inefficient but hopefully its fast enough
        // ----- begin LD analog code -----
        /*int val = stick_lx;
        float X1_norm;
        if(val > 0)
            X1_norm = val / 32767.f;
        else X1_norm = val / -32767.f;
        
        X1_norm = (X1_norm - dead_zone / 100.0) * 100.0 / (100.0 - dead_zone);
        if (X1_norm < 0) X1_norm = 0;

        char ans = (char)(127 * X1_norm * sensitivity / 100);
        if (val < 0) ans = -ans;
        n64_data.x_axis = reverse(ans);
        
        // repeat for y values
        val = stick_ly;
        if(val > 0)
            X1_norm = val / 32767.f;
        else X1_norm = val / -32767.f;
        
        X1_norm = (X1_norm - dead_zone / 100.0) * 100.0 / (100.0 - dead_zone);
        if (X1_norm < 0) X1_norm = 0;

        ans = (char)(127 * X1_norm * sensitivity / 100);
        if (val < 0) ans = -ans;
        n64_data.y_axis = reverse(ans);*/
        // ----- end of LD analog code -----
        
        // ----- begin nrage replication analog code -----
        const float XPAD_MAX = 32768;
        const float N64_MAX = (sensitivity > 0) ? 127*(sensitivity/100.0f) : 0;
        //const float N64_MAX = 127;
        float deadzoneValue = (dead_zone/100.0f) * XPAD_MAX;
        float deadzoneRelation = XPAD_MAX / (XPAD_MAX - deadzoneValue);
        
        LSX = LSY = 0; // -128 to +127...
        float unscaled_result = 0;
        
        if(stick_lx >= deadzoneValue) // positive = right
        {
            unscaled_result = (stick_lx - deadzoneValue) * deadzoneRelation;
            LSX = (char)(unscaled_result * (N64_MAX / XPAD_MAX));
        }
        else if(stick_lx <= (-deadzoneValue)) // negative = left
        {
            stick_lx = -stick_lx; // compute as positive, then negate at the end
            unscaled_result = (stick_lx - deadzoneValue) * deadzoneRelation;
            LSX = (char)(unscaled_result * (N64_MAX / XPAD_MAX));
            LSX = -LSX;
        }
        
        if(stick_ly >= deadzoneValue) // positive = up
        {
            unscaled_result = (stick_ly - deadzoneValue) * deadzoneRelation;
            LSY = (char)(unscaled_result * (N64_MAX / XPAD_MAX));
        }
        else if(stick_ly <= (-deadzoneValue)) // negative = down
        {
            stick_ly = -stick_ly; // compute as positive, then negate at the end
            unscaled_result = (stick_ly - deadzoneValue) * deadzoneRelation;
            LSY = (char)(unscaled_result * (N64_MAX / XPAD_MAX));
            LSY = -LSY;
        }
        n64_data.x_axis = reverse(LSX);
        n64_data.y_axis = reverse(LSY);
        
        // ----- end nrage replication analog code -----
        
        // Generic analog stick code
        /*if(LSX > dead_zone)
        {
            n64_data.x_axis = reverse(LSX);
        }
        if(LSY > dead_zone)
        {
            n64_data.y_axis = reverse(LSY);
        }*/
        // end of general code
    }
    else // state > 0 so we are in the process of changing controls
    {
        uint64_t b = DetectButton(); // read for button presses (just do linear search)
        if(b != 0) /*button was actually is pressed*/
        {
            //pc.printf("PRESSED\r\n",state); // DEBUG
            if(XpadButtonPressed == false)
            {
                XpadButtonPressed = true;
                ChangeButtonMapping(b);
                AdvanceState();
            }
        }
        else
        {
            //pc.printf("NADA\r\n",state); // DEBUG
            XpadButtonPressed = false;
        }
    }
}

// 0 is 3 microseconds low followed by 1 microsecond high
// 1 is 1 microsecond low followed by 3 microseconds high
unsigned int GetMiddleOfPulse()
{
    // wait for line to go high
    while(1)
    {
        if(data.read() == 1) break;
    }
    
    // wait for line to go low
    while(1)
    {
        if(data.read() == 0) break;
    }
    
    // now we have the falling edge
    // wait 2 microseconds to be in the middle of the pulse, and read. high --> 1.  low --> 0. 
    my_wait_us_asm(2);
    return (unsigned int) data.read();
}

// continuously read bits until at least 9 are read, confirm valid command, return without stop bit
unsigned int readCommand()
{
    unsigned int command = GetMiddleOfPulse(), bits_read = 1;
    
    while(1) // read at least 9 bits (2 bytes + stop bit)
    {
        //my_wait_us_asm(4);
        command = command << 1; // make room for the new bit
        //command += data.read(); // place the new bit into the command
        command += GetMiddleOfPulse();
        command &= 0x1FF; // remove all except the last 9 bits
        
        bits_read++;
        
        if(bits_read >= 9) // only consider when at least a whole command's length has been read
        {
            if(command == 0x3 || command == 0x1 || command == 0x1FF || command == 0x5 || command == 0x7)
            {
                // 0x3 = 0x1 + stop bit --> get controller state
                // 0x1 = 0x0 + stop bit --> who are you?
                // 0x1FF = 0xFF + stop bit --> reset signal
                // 0x5 = 0x10 + stop bit --> read
                // 0x7 = 0x11 + stop bit --> write
                command = command >> 1; // get rid of the stop bit
                return command;
            }
        }
    }
}

void write_1()
{
    data = 0;
    my_wait_us_asm(1);
    data = 1;
    my_wait_us_asm(3);
    //pc.printf("1");
}

void write_0()
{
    data = 0;
    my_wait_us_asm(3);
    data = 1;
    my_wait_us_asm(1);
    //pc.printf("0");
}


void SendStop()
{
    data = 0;
    my_wait_us_asm(1);
    data = 1;
}

// send a byte from LSB to MSB (proper serialization)
void SendByte(unsigned char b)
{
    for(int i = 0;i < 8;i++) // send all 8 bits, one at a time
    {
        if((b >> i) & 1)
        {
            write_1();
        }
        else
        {
            write_0();
        }
    }
}

void SendIdentity()
{
    // reply 0x05, 0x00, 0x02
    SendByte(0x05);
    SendByte(0x00);
    SendByte(0x02);
    SendStop();
}

void SendControllerData()
{
    unsigned long data = *(unsigned long*)&n64_data;
    unsigned int size = sizeof(data) * 8; // should be 4 bytes * 8 = 32 bits
    
    for(unsigned int i = 0;i < size;i++)
    {
        if((data >> i) & 1)
        {
            write_1();
        }
        else
        {
            write_0();
        }
    }
    
    SendStop();
}

int main()
{
    bool buttonPressed = false;
    
    pc.printf("\r\nNow loaded! SystemCoreClock = %d Hz\r\n", SystemCoreClock);

    USBHostXpad xpad;
    if (!xpad.connect()) {
        pc.printf("Error: XBox controller not found.\n");
    }
    
    xpad.attachEvent(onXpadEvent);
    xpad.led(USBHostXpad::LED1_ON);
    
    while(1)
    {
        if(state == NORMAL)
        {
            if(!button) // user wants to change controls
            {
                if(!buttonPressed) // make sure it's a separate button press
                {
                    myled = true;
                    buttonPressed = true;
                    state++;
                    continue;
                }
            }
            else
            {
                buttonPressed = false;
            }
            
            // Set pin mode to input
            data.input();
            
            USBHost::poll();
            
            __disable_irq();    // Disable Interrupts
            
            // Read 64 command
            unsigned int cmd = readCommand();
            
            my_wait_us_asm(2); // wait a small amount of time before replying
     
            //-------- SEND RESPONSE
            // Set pin mode to output
            data.output();
            
            //pc.printf("cmd = 0x%02X\r\n",cmd);
            
            switch(cmd)
            {
                case 0x00: // identity
                    //pc.printf("I got an 0x00!\r\n");
                    SendIdentity();
                    break;
                case 0xFF: // reset
                    //pc.printf("I got an 0xFF!\r\n");
                    SendIdentity();
                    break;
                case 0x01: // poll for state
                    SendControllerData();
                    break;
                default:
                    // we do not process the read and write commands (memory pack)
                    break;
            }
            __enable_irq();    // Enable Interrupts
            //-------- DONE SENDING RESPOSE
        }
        else
        {
            if(!button) // user wants to cancel and return to regular mode
            {
                if(!buttonPressed) // make sure it's a separate button press
                {
                    state = NORMAL;
                    myled = false;
                    buttonPressed = true;
                    continue;
                }
            }
            else
            {
                buttonPressed = false;
            }
            
            //pc.printf("State = %d\r\n",state); // DEBUG
            
            
            USBHost::poll();
            //wait(0.1); // too long of a wait. caused xbone controller issues due to timeouts
            
            if(state == NORMAL) // about to return to normal operation, make sure the LED turns off
            {
                myled = false;
                XpadButtonPressed = false;
            }
        }
    }
}