Arduino Single Joystick Car control

gistfile1.txt

// Single analogue joystick control for dual motors
// 
// Applications include
//  - Accessible control for single handed people
//  - free up the second analogue stick for adding weapons or camera control
//  - improving on the arduino battle bot PS2 code

// PS2X lib is buggy, especially with my cheapo controllers which have a few bits flipping 
// randomly, other than that, and a little bit of a pain with a poor connection, it seems 
// to work reliably enough most of the time. YMMV

// The robot that this code powers is a simple setup of a wireless PS2 controller, a couple 18650s 
// a power regulator board set to 11V and an L298 dual h-bridge. Nothing special really. 
// the motors are standard arduino yellow geared motors. 

// Total cost of the device is less than 20 quid. 
// Case is made out of corex sheet, hotglue, designed in blender, exported as a paper model
// then scored, cut and glued together.

#include <PS2X_lib.h>

#define PS2_DAT        12
#define PS2_CMD        11
#define PS2_SEL        10
#define PS2_CLK        13

PS2X ps2x;

int error = 0;
byte type = 0;

#define IN1  7   //K1、K2 motor direction
#define IN2  8   //K1、K2 motor direction
#define IN3  9   //K3、K4 motor direction
#define IN4  4   //K3、K4 motor direction
#define ENA  5   // Needs to be a PWM pin to be able to control motor speed ENA
#define ENB  6    // Needs to be a PWM pin to be able to control motor speed ENA


void setup() {
  Serial.begin(57600);
  delay(300);
  // Configure the gamepad
  error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, false, false);
  if(error == 0){
    Serial.print("Found Controller, configured successful ");
    Serial.print("pressures = ");
  }
  type = ps2x.readType(); 
  switch(type) {
    case 0:
      Serial.print("Unknown Controller type found ");
      break;
    case 1:
      Serial.print("DualShock Controller found ");
      break;
    case 2:
      Serial.print("GuitarHero Controller found ");
      break;
    case 3:
      Serial.print("Wireless Sony DualShock Controller found ");
      break;
   }
   Serial.println();
   // Set up the pin modes for the L298
   pinMode(IN1, OUTPUT); 
   pinMode(IN2, OUTPUT); 
   pinMode(IN3, OUTPUT);
   pinMode(IN4, OUTPUT); 
   pinMode(ENA, OUTPUT);  
   pinMode(ENB, OUTPUT);
}

// This is an nthroot implementation for the arduino, it's slow, inaccurate, slightly clunky but 
// it's enough to improve our square -1 to 1 input mapping to a true circle... poorly :)
// based on a wikipedia implementation from a Newtonian estimation.
float nthroot(float number, float n) {
  if (n == 0) return NAN;
  if (number > 0) return pow(number, 1.0 / n);
  if (number == 0) return 0;
  if (number < 0 && float(n) == n && (int(n) & 1)) return -pow(-number, 1.0f / n);
  return 0;
}

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

  // Get the input state
  ps2x.read_gamepad(false, 0);
  float y = ((ps2x.Analog(PSS_LY) - 128) * -1.0f) / 128.0f;
  float x = (ps2x.Analog(PSS_LX) - 128) / 128.0f;

  // Make a note of +/- position of joystick (mostly because arduino's suck at math)
  float ix = 1.0f;
  float iy = 1.0f;

  if (x < 0) {
    ix = -1.0f;
    x = x * -1.0f;
  }
  if (y < 0) {
    iy = -1.0f;
    y = y * -1.0f;
  }
  
  int left;
  int right;

  // Map the input position to a circle (not perfect, lots of precision lost in the arduino)
  float xc = nthroot(1.0f - (pow(y, 2.0f)/2.0f), x) * ix * x;
  float yc = nthroot(1.0f - (pow(x, 2.0f)/2.0f), y) * iy * y;

  // Correct NaN cases
  if (isnan(xc)) {
    xc = 0;
  }
  if (isnan(yc)) {
    yc = 0;
  }

  // From the mapped input position, calculate the strength and angle of the position
  // we lose more precision here due to the arduino's floating point unit being weak. 
  float d = sqrt(pow(xc, 2) + pow(yc, 2));
  float a = atan(xc/yc);

  // Another nan case to fix.
  if (isnan(a)) {
    a = 0;
  }

  // Correct the angle so that it goes +/- 180 rather than weird flips
  if (iy < 0 && ix < 0) {
    a = (PI - a) * -1;
  } else if (iy < 0) {
    a = (PI + a);
  }
  a = abs(a);

  // Compute the amount of turn, this is a blend shape algorithm and it
  // was worked out using a spreadsheet and a bit of trial and error
  // essentially we want 45 degrees to turn on one motor, but 90 degrees 
  // to have the motors spinning in opposite directions, 0 and 180 both 
  // motors full in the direction of travel. 
  // It's near perfect, but not quite... best I could do inside of a few hours. 
  float l = 0;
  // Left motor less than 180 degrees
  if (a < PI) {
    l = (cos(a)+0.5f)*2.0f;
    if (l > 1) l = 1;
  } else {
    l = (cos(a)-0.5f) * 2.0f;
    if (l < -1) l = -1;
  }

  float r = 0;
  if (a > PI) {
    r = (cos(a) + 0.5f) * 2.0f;
    if (r > 1) r = 1;
  } else {
    r = (cos(a)-0.5f) * 2.0f;
    if (r < -1) r = -1;
  }
  if (ix < 0) {
    float v = l;
    l = r;
    r = v;
  } 

  // Multiply out the speed of the motors by the strength upto 255 to drive the motors. 
  left = l * 255 * abs(d);
  right = r * 255 * abs(d);


  // Apply the motor speeds
  if (left < 0) {
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
  } else {
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
  }
  analogWrite(ENA, abs(left));


  if (right < 0) {
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, HIGH);
  } else {
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
  }
  analogWrite(ENB, abs(right));
  
  delay(1);
}