Go Down

Topic: Joystick differential steer robot (Read 2671 times) previous topic - next topic

slomobile

In the process of building a robot today I needed some quick code to test the drive platform under joystick control.  A quick search didn't immediately turn up what I was looking for so I wrote this.  I put it here in hopes that it will be useful to someone.  I'd like to hear if you like it, hate it, used it, or if you find something wrong with it.  It seems longer than it needs to be.  No attempt was made to optimize.  It would be in the playground but I don't know what category it would fit.
Code: [Select]
/*
  Joystick controlled differential steer mobile robot.
  Author: Dustin Maki
  Date: Sept 1 2011
  GPL License.  Free to use, just give credit where due.
  This program scales the joystick reading to start at 1 just outside the deadband so you do not have a large jump in speed.
  It adjusts to the common case where the centered joystick does not rest at quite the center of the range.
  This was first used with
  Teensy++ v.1 http://www.pjrc.com/teensy/pinout.html
  Solarbotics LM298D H bridge http://www.solarbotics.com/products/k_cmd/
  with transistor on each side to switch the direction as in datasheet.
  Joystick http://www.sparkfun.com/products/9032 
     
*/
#define START_BUTTON 21 // not a momentary switch, a toggle switch will work
#define JOYSTICK_BUTTON_PIN 10 // hold joystick with 'tail' toward you.  side with holes on sparkfun breakout board
#define JOYSTICK_X_PIN A0 //full left is 1023, full right is 0
#define JOYSTICK_Y_PIN A1//full forward is 1023, full backward is 0
#define LEFT_PWM_PIN 14
#define LEFT_DIRECTION_PIN 13
#define RIGHT_PWM_PIN 24
#define RIGHT_DIRECTION_PIN 23
#define HALF_DEADBAND 3 //adjust till desired deadband is achieved
#define XRAW_CENTERED 525 // set to whatever "x joystick value is: " on the serial monitor
#define YRAW_CENTERED 493 // set to whatever "y joystick value is: " on the serial monitor
#define DEBUG true // set to false to stop serial messages

void setup()
{
  if (DEBUG)             
  Serial.begin(38400);
 
  pinMode(LEFT_PWM_PIN, OUTPUT);
  pinMode(RIGHT_PWM_PIN, OUTPUT); 
  pinMode(LEFT_DIRECTION_PIN, INPUT);// sets high impedance on pin, brake
  pinMode(RIGHT_DIRECTION_PIN, INPUT);// sets high impedance on pin, brake
  digitalWrite(LEFT_PWM_PIN, LOW); 
  digitalWrite(RIGHT_PWM_PIN, LOW);
}

int throttle, steering;
int xraw, yraw, leftSpd, rightSpd;

void loop()                     
{
  // start button is a push on, push off type. grounded when on
  // when start button is released, program just runs main loop
  while(digitalRead(START_BUTTON) == LOW)
  {
    if (digitalRead(JOYSTICK_BUTTON_PIN) == LOW)//brake requested
    {
      pinMode(LEFT_DIRECTION_PIN, INPUT);// sets high impedance on pin, brake
      pinMode(RIGHT_DIRECTION_PIN, INPUT);// sets high impedance on pin, brake
   
      if (DEBUG)
      {
        Serial.println("brake applied");
      }
      continue;// will not continue main loop, only while
    }
   
  else //start button pressed and not braked
  { 
    // capture joystick position
    yraw = analogRead(JOYSTICK_Y_PIN);
    xraw = analogRead(JOYSTICK_X_PIN);
   
    // scale x and y axis so that both are zeroed at natural center of potentiometer, implement deadband
    if(yraw > YRAW_CENTERED + HALF_DEADBAND)//forward
    {
      throttle = map(yraw, YRAW_CENTERED + HALF_DEADBAND, 1023, 0, 255);     
      if(xraw > XRAW_CENTERED + HALF_DEADBAND)//left
      {
        steering = map(xraw, XRAW_CENTERED + HALF_DEADBAND, 1023, -0, -255);// reverse pot coordinates to agree with cartesian       
      }
      else if(xraw < XRAW_CENTERED - HALF_DEADBAND)// right
      {
        steering = map(xraw, XRAW_CENTERED - HALF_DEADBAND, 0, 0, 255);// reverse pot coordinates to agree with cartesian       
      }
      else // in X deadband
      {
        steering = 0;
      }
    }
   
    else if(yraw < YRAW_CENTERED - HALF_DEADBAND)//reverse
    {
      throttle = map(yraw, YRAW_CENTERED - HALF_DEADBAND, 0, -0, -255);     
      if(xraw > XRAW_CENTERED + HALF_DEADBAND)//left
      {
        steering = map(xraw, XRAW_CENTERED + HALF_DEADBAND, 1023, -0, -255);// reverse pot coordinates to agree with cartesian       
      }
      else if(xraw < XRAW_CENTERED - HALF_DEADBAND)// right
      {
        steering = map(xraw, XRAW_CENTERED - HALF_DEADBAND, 0, 0, 255);// reverse pot coordinates to agree with cartesian       
      }
      else// in X deadband
      {
        steering = 0;
      }
    }
    else //in Y deadband
    {
      throttle = 0;
      if(xraw > XRAW_CENTERED + HALF_DEADBAND)//left
      {
        steering = map(xraw, XRAW_CENTERED + HALF_DEADBAND, 1023, -0, -255);// reverse pot coordinates to agree with cartesian       
      }
      else if(xraw < XRAW_CENTERED - HALF_DEADBAND)// right
      {
        steering = map(xraw, XRAW_CENTERED - HALF_DEADBAND, 0, 0, 255);// reverse pot coordinates to agree with cartesian       
      }
      else// in X deadband
      {
        steering = 0;
      }
    }
   
  //transfer from x,y to l,r
  leftSpd = throttle + steering;
  rightSpd = throttle - steering;
  //leftSpd and rightSpd will contain a number from -255 to 255 that gets updated each time the loop runs.
  //-255 equates to full reverse. 0 equates to anywhere within the deadband.  255 is full forward.
 
  // The next 2 sections can be replaced to use with servos, stepper motors, BLDCs or whatever drive you like.
 
  //write direction pin
  if (leftSpd < 0)
  digitalWrite(LEFT_DIRECTION_PIN, LOW);
  else if (leftSpd > 0)
  digitalWrite(LEFT_DIRECTION_PIN, HIGH); 
  if (rightSpd < 0)
  digitalWrite(RIGHT_DIRECTION_PIN, LOW);
  else if (rightSpd > 0)
  digitalWrite(RIGHT_DIRECTION_PIN, HIGH);
 
  //write PWM pin
  analogWrite(LEFT_PWM_PIN, leftSpd);
  analogWrite(RIGHT_PWM_PIN, rightSpd);
 

  if (DEBUG)
  {
  Serial.print("x joystick value is: ");
  Serial.println(xraw);
  Serial.print("y joystick value is: ");
  Serial.println(yraw);
  Serial.print("throttle is: ");
  Serial.println(throttle);
  Serial.print("steering is: ");
  Serial.println(steering);
  Serial.print("leftSpd is: ");
  Serial.println(leftSpd);
  Serial.print("rightSpd is: ");
  Serial.println(rightSpd);
 
  delay(250);
  }
}
}
}



slomobile

Code: [Select]
//write PWM pin
  analogWrite(LEFT_PWM_PIN, leftSpd);
  analogWrite(RIGHT_PWM_PIN, rightSpd);


should be

Code: [Select]
//write PWM pin
  analogWrite(LEFT_PWM_PIN, abs(leftSpd));
  analogWrite(RIGHT_PWM_PIN, abs(rightSpd));

Paulware

If you would like to use the PSXAnalog joystick I just posted a project at http://atmega128.org/#PSXAnalogTx

cyberteque

Thanks for posting this!

Just got a Rover 5 and a Sparkfun monster motor shield.

Already have a D15 on a bracket on my prototyping rig, I'll use an analogue PC joystick.
The Rover 5 will be Xbee controlled, I'll have my shiny new Freetronics Eleven reading my joystick and a few switches, then transmit the control data.

With the amount of XBee traffic around here now, I'm gunna have to write some kind of protocol so my ROV and Barnduino can co-exist.
On the bright side, if I make the XBee on the ROV an end-point, I'll get coverage over most of the valley.

Go Up