Connect USB Joystick to Commodore C64

/*
 Arduino sketch to connect 2 USB joysticks to Commodore C64
 */

/*
 Basis for this sketch: 
 Example sketch for the PS3 USB library - developed by Kristian Lauszus
 For more information visit my blog: http://blog.tkjelectronics.dk/ or
 send me an e-mail:  kristianl@tkjelectronics.com
 */

 /*
  Mapping Competition Pro to --> PS3:
  Front Left Button --> Triangle
  Front Right Button --> Circle
  Rear Left Button (triangle shaped) --> Cross
  Rear Right Button (triangle shaped) --> Square
  Forward --> LeftHatY = 0
  Backward --> LeftHatY = 255
  Left --> LeftHatX = 0
  Right --> LeftHatY = 255
  * 
  */

#include <PS3USB.h>
#include <usbhub.h>
#include <UHS2_gpio.h>

// Satisfy the IDE, which needs to see the include statment in the ino too.
#ifdef dobogusinclude
#include <spi4teensy3.h>
#endif
#include <SPI.h>

#define PRINTREPORT

#define LEFT1   4 //via GPIO_Out4 on USB Host Shield
#define RIGHT1  5 //via GPIO_Out5 on USB Host Shield
#define FORWARD1     2
#define BACKWARD1   3
#define BUTTON1 0   //via GPIO_Out0 on USB Host Shield
#define LEFT2   4
#define RIGHT2  5
#define FORWARD2     6
#define BACKWARD2   7
#define BUTTON2 1   //via GPIO_Out1 on USB Host Shield
#define LED_PORT1   2
#define LED_PORT2   3
#define SWAPBUTTON  0 // via GPIO_In0 on USB Host Shield
#define STATE_BUTTON  0
#define STATE_LEFT  1
#define STATE_RIGHT 2
#define STATE_FORWARD 3
#define STATE_BACKWARD  4


#define JOYSTICK_COUNT  2
#define JOYSTICK_STATES 5

#define DEBOUNCETIME  50


USB Usb;
USBHub     Hub(&Usb);
PS3USB PS3(&Usb); // This will just create the instance
PS3USB PS3_1(&Usb); // This will just create the instance
UHS2_GPIO Gpio(&Usb); // Create a GPIO object

bool joystickState[JOYSTICK_COUNT][JOYSTICK_STATES];
bool joystickSwap = LOW;

void setup() {
  Serial.begin(115200);
  #if !defined(__MIPSEL__)
    while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
  #endif
  
  if (Usb.Init() == -1) {
    Serial.print(F("\r\nOSC did not start"));
    while (1); //halt
  }
  Serial.print(F("\r\nPS3 USB Library Started"));
  
  //initialize joystick states for all joysticks
  for (unsigned char i=0; i<JOYSTICK_COUNT; i++){
    for (unsigned char j=0; j<JOYSTICK_STATES; j++){
      joystickState[i][j] = 0;
    }
  }

  //initialize Output pins
  Gpio.digitalWrite(LEFT1, LOW);
  Gpio.digitalWrite(RIGHT1, LOW);
  digitalWrite(FORWARD1, LOW);
  digitalWrite(BACKWARD1, LOW);
  digitalWrite(LEFT2, LOW);
  digitalWrite(RIGHT2, LOW);
  digitalWrite(FORWARD2, LOW);
  digitalWrite(BACKWARD2, LOW);
  pinMode(LEFT1, OUTPUT);
  pinMode(RIGHT1, OUTPUT);
  pinMode(FORWARD1, OUTPUT);
  pinMode(BACKWARD1, OUTPUT);
  pinMode(LEFT2, OUTPUT);
  pinMode(RIGHT2, OUTPUT);
  pinMode(FORWARD2, OUTPUT);
  pinMode(BACKWARD2, OUTPUT);
  Gpio.digitalWrite(BUTTON1, LOW);
  Gpio.digitalWrite(BUTTON2, LOW);
}

void loop() {
  unsigned char joystickMapping[JOYSTICK_COUNT];
  static bool joystickButtonState[JOYSTICK_COUNT];
  Usb.Task();
  PS3.attachOnInit(USBonInit1);
  PS3_1.attachOnInit(USBonInit2);

 // Shall Joysticks be swapped?
 joystickSwap = ToggleButton(Gpio.digitalRead(SWAPBUTTON));
 
 if(joystickSwap){
  joystickMapping[0] = 1;
  joystickMapping[1] = 0;
  Gpio.digitalWrite(LED_PORT1, LOW);
  Gpio.digitalWrite(LED_PORT2, HIGH);
 }
 else{
  joystickMapping[0] = 0;
  joystickMapping[1] = 1;
  Gpio.digitalWrite(LED_PORT1, HIGH);
  Gpio.digitalWrite(LED_PORT2, LOW);
 }


 // Joystick 1
  if (PS3.PS3Connected || PS3.PS3NavigationConnected) {
    if (PS3.getAnalogHat(LeftHatX) == 0){
      joystickState[joystickMapping[0]][STATE_LEFT] = true;
      Serial.print(F("\r\nLeft1")); 
    }
    else{
      joystickState[joystickMapping[0]][STATE_LEFT] = false; 
    }

    if (PS3.getAnalogHat(LeftHatX) == 255){
      joystickState[joystickMapping[0]][STATE_RIGHT] = true;
      Serial.print(F("\r\nRight1")); 
    }
    else{
      joystickState[joystickMapping[0]][STATE_RIGHT] = false; 
    }

    if (PS3.getAnalogHat(LeftHatY) == 0){
      joystickState[joystickMapping[0]][STATE_FORWARD] = true;
      Serial.print(F("\r\nForward1")); 
    }
    else{
      joystickState[joystickMapping[0]][STATE_FORWARD] = false; 
    }   

    if (PS3.getAnalogHat(LeftHatY) == 255){
      joystickState[joystickMapping[0]][STATE_BACKWARD] = true;
      Serial.print(F("\r\nBackward1")); 
    }
    else{
      joystickState[joystickMapping[0]][STATE_BACKWARD] = false; 
    }   
  
   
    if (PS3.getButtonPress(TRIANGLE) || PS3.getButtonPress(CIRCLE) || PS3.getButtonPress(CROSS) || PS3.getButtonPress(SQUARE)){
      Serial.print(F("\r\nButton1_on"));   
      joystickState[joystickMapping[0]][STATE_BUTTON] = true;  
    }
    else{
      joystickState[joystickMapping[0]][STATE_BUTTON] = false;  
    }
  }

// Joystick 2
if (PS3_1.PS3Connected || PS3_1.PS3NavigationConnected) {
    if (PS3_1.getAnalogHat(LeftHatX) == 0){
      joystickState[joystickMapping[1]][STATE_LEFT] = true;
      Serial.print(F("\r\nLeft2")); 
    }
    else{
      joystickState[joystickMapping[1]][STATE_LEFT] = false; 
    }

    if (PS3_1.getAnalogHat(LeftHatX) == 255){
      joystickState[joystickMapping[1]][STATE_RIGHT] = true;
      Serial.print(F("\r\nRight2")); 
    }
    else{
      joystickState[joystickMapping[1]][STATE_RIGHT] = false; 
    }

    if (PS3_1.getAnalogHat(LeftHatY) == 0){
      joystickState[joystickMapping[1]][STATE_FORWARD] = true;
      Serial.print(F("\r\nForward2")); 
    }
    else{
      joystickState[joystickMapping[1]][STATE_FORWARD] = false; 
    }   

    if (PS3_1.getAnalogHat(LeftHatY) == 255){
      joystickState[joystickMapping[1]][STATE_BACKWARD] = true;
      Serial.print(F("\r\nBackward2")); 
    }
    else{
      joystickState[joystickMapping[1]][STATE_BACKWARD] = false; 
    }
    
  if (PS3_1.getButtonPress(TRIANGLE) || PS3_1.getButtonPress(CIRCLE) || PS3_1.getButtonPress(CROSS) || PS3_1.getButtonPress(SQUARE)){
      Serial.print(F("\r\nButton2_on"));   
      joystickState[joystickMapping[1]][STATE_BUTTON] = true;  
    }
    else{
      joystickState[joystickMapping[1]][STATE_BUTTON] = false;  
    }
  }
  Gpio.digitalWrite(LEFT1, joystickState[0][STATE_LEFT]);
  Gpio.digitalWrite(RIGHT1, joystickState[0][STATE_RIGHT]);
  digitalWrite(FORWARD1, joystickState[0][STATE_FORWARD]);
  digitalWrite(BACKWARD1, joystickState[0][STATE_BACKWARD]);
  digitalWrite(LEFT2, joystickState[1][STATE_LEFT]);
  digitalWrite(RIGHT2, joystickState[1][STATE_RIGHT]);
  digitalWrite(FORWARD2, joystickState[1][STATE_FORWARD]);
  digitalWrite(BACKWARD2, joystickState[1][STATE_BACKWARD]);
  Gpio.digitalWrite(BUTTON1, joystickState[0][STATE_BUTTON]);
  Gpio.digitalWrite(BUTTON2, joystickState[1][STATE_BUTTON]);
}

void USBonInit1(){ //runs once at moment of USB controller initialization
  Serial.println(" USB 1 Connected ");
}

void USBonInit2(){ //runs once at moment of USB controller initialization
  Serial.println(" USB 2 Connected ");
}


bool DebounceButton(bool reading){
  static unsigned int lastDebounceTime;
  static bool lastButtonState;
  static bool buttonState;
      
  if (reading != lastButtonState) {
    // reset the debouncing timer
    lastDebounceTime = millis();
  }  
  
  if ((millis() - lastDebounceTime) > DEBOUNCETIME) {
    // whatever the reading is at, it's been there for longer
    // than the debounce delay, so take it as the actual current state:
      buttonState = reading;
   }
  
  lastButtonState = reading;
  return buttonState;
}


bool ToggleButton(bool reading){
    static bool toggleState = LOW;
    static bool lastButtonState = LOW;
    bool buttonState;

    buttonState = DebounceButton(reading);
    
    if((buttonState != lastButtonState) && (buttonState == HIGH)){
      if (toggleState == LOW){
        toggleState = HIGH;
        Serial.println(" Toggle High");
      }
      else{
        toggleState = LOW;
        Serial.println(" Toggle Low");
      }
    }
    lastButtonState = buttonState;
  return toggleState;
}