IkedoMiniV2_PS2Controller.ino

/*******************************************************************************
 * THIS SOFTWARE IS PROVIDED IN AN "AS IS" CONDITION. NO WARRANTY AND SUPPORT
 * IS APPLICABLE TO THIS SOFTWARE IN ANY FORM. CYTRON TECHNOLOGIES SHALL NOT,
 * IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL OR CONSEQUENTIAL
 * DAMAGES, FOR ANY REASON WHATSOEVER.
 ********************************************************************************
 * DESCRIPTION:
 *
 * This example is for controlling Ikedo Mini V2 using PS2 controller.
 *
 * AUTHOR   : Idris Zainal Abidin
 * COMPANY  : Cytron Technologies Sdn Bhd
 * WEBSITE  : my.cytron.io
 * EMAIL    : support@cytron.io
 *
 *******************************************************************************/

#include "CytronMakerSumo.h"
#include <PS2X_lib.h>

PS2X ps2x;
// Right now, the library does NOT support hot-pluggable controllers, meaning
// you must always either restart your Arduino after you connect the controller,
// or call config_gamepad(pins) again after connecting the controller.

// Define note and duration for melody.
int melodyPitch[] = {NOTE_C4, NOTE_G4};
int melodyDuration[] = {8, 8};

int error = 0;
byte type = 0;
byte vibrate = 0;

void setup() {
  Serial.begin(115200);
  MakerSumo.begin();

  // GamePad(clock, command, attention, data, pressures?, rumble?)
  error = ps2x.config_gamepad(5, 7, 6, 2, false, false);
  if (error == 0) {
    Serial.println("Found Controller, configured successful");
    Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
    Serial.println("holding L1 or R1 will print out the analog stick values.");
    Serial.println("Go to www.billporter.info for updates and to report bugs.");
  }
  else if (error == 1) {
    Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
  }
  else if (error == 2) {
    Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");
  }
  else if (error == 3) {
    Serial.println("Controller refusing to enter Pressures mode, may not support it. ");
  }

  type = ps2x.readType();
  switch (type) {
    case 0:
      Serial.println("Unknown Controller type");
      break;
    case 1:
      Serial.println("DualShock Controller Found");
      break;
    case 2:
      Serial.println("GuitarHero Controller Found");
      break;
  }

  // Play the melody once.
  MakerSumo.playMelody(melodyPitch, melodyDuration, 2);
}

int MAX_SPEED = 255;
//int MAX_SPEED = 180; // For Novamax 800rpm
signed int speed = 0;
signed int speed_left = 0;
signed int speed_right = 0;

void loop() {
  if (error == 1)
    return;

  else {
    ps2x.read_gamepad(false, vibrate);

    // Will be TRUE as long as button is pressed
    if (ps2x.Button(PSB_R2)) {
      if (speed < MAX_SPEED) {
        speed += 5;
      }
    }
    else if (!ps2x.Button(PSB_R2)) {
      if (speed < MAX_SPEED * 0.5) {
        speed += 5;
      }
      else if (speed > MAX_SPEED * 0.5) {
        speed -= 5;
      }
    }

    int button_pressed = 0;

    if (ps2x.Button(PSB_PAD_UP)) {
      button_pressed += 1;
    }
    if (ps2x.Button(PSB_PAD_RIGHT)) {
      button_pressed += 2;
    }
    if (ps2x.Button(PSB_PAD_DOWN)) {
      button_pressed += 4;
    }
    if (ps2x.Button(PSB_PAD_LEFT)) {
      button_pressed += 8;
    }

    if (button_pressed == 1) {
      speed_left = speed;
      speed_right = speed;
    }
    else if (button_pressed == 3) {
      speed_left = speed;
      speed_right = 0;
    }
    else if (button_pressed == 2) {
      speed_left = speed;
      speed_right = -speed;
    }
    else if (button_pressed == 6) {
      speed_left = -speed;
      speed_right = 0;
    }
    else if (button_pressed == 4) {
      speed_left = -speed;
      speed_right = -speed;
    }
    else if (button_pressed == 12) {
      speed_left = 0;
      speed_right = -speed;
    }
    else if (button_pressed == 8) {
      speed_left = -speed;
      speed_right = speed;
    }
    else if (button_pressed == 9) {
      speed_left = 0;
      speed_right = speed;
    }
    else if (button_pressed == 0) {
      if (speed_left > 20) {
        speed_left -= 5;
      }
      else if (speed_left > 0) {
        speed_left -= 1;
      }
      else if (speed_left < -20) {
        speed_left += 5;
      }
      else if (speed_left < 0) {
        speed_left += 1;
      }

      if (speed_right > 20) {
        speed_right -= 5;
      }
      else if (speed_right > 0) {
        speed_right -= 1;
      }
      else if (speed_right < -20) {
        speed_right += 5;
      }
      else if (speed_right < 0) {
        speed_right += 1;
      }
      speed = 0;
    }

    MakerSumo.setMotorSpeed(MOTOR_L, speed_left);
    MakerSumo.setMotorSpeed(MOTOR_R, speed_right);
  }
}