Skip to content

Instantly share code, notes, and snippets.

Show Gist options
  • Save klattimer/151d9cf133d595db7d7517ac07d9f671 to your computer and use it in GitHub Desktop.
Save klattimer/151d9cf133d595db7d7517ac07d9f671 to your computer and use it in GitHub Desktop.
Arduino Single Joystick Car control
// 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);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment