2 continuous servo differential with one joystick

sirbow2 · January 7, 2012, 8:56am

i have a 2 axis joystick, and have had the interfacing code done for a while(reads the joystick writes to a int in values 0 to 1023), but now im trying to use the 2 axis joystick to control 2 servos in differential setup(one on the left, one on right). so if i were to push the stick straight up that would make the robot go forward. if i lean the stick to the right, it would slow down the right wheel a bit and keep the left wheel at full speed to make it turn right.

ive tried two different versions of code, i cant get either to work: int YaxisVar and XaxisVar from both attempts of my code are the joystick inputs(0 to 1023).

if (YaxisVar > YCenterPnt + DeadZone || YaxisVar < YCenterPnt - DeadZone)
    {
      // Map values from potentiometers to a smaller range for the PWM motor controllers (0-255)
      XaxisVar = map(XaxisVar, 0, 1023, 0, 180); 
      YaxisVar = map(YaxisVar, 0, 1023, 0, 180);

      int lYaxisVar = YaxisVar;
      int rYaxisVar = YaxisVar;

      if (XaxisVar < 88)
      { // turning left, so slow-down left track
        if (YaxisVar > 92)
        { // moving forward
          lYaxisVar -= (180 - XaxisVar); // decreasing the value - moving it closer to the center-point - slows it down
        }

        if (YaxisVar < 88)
        { // moving in reverse
          lYaxisVar += (180 - XaxisVar); // increasing the value - moving it closer to the center-point - slows it down
        }
      }

      if (XaxisVar > 92)
      { // turning right, so slow-down right track
        if (YaxisVar > 92)
        { // moving forward
          rYaxisVar -= (180 - XaxisVar); // decreasing the value - moving it closer to the center-point - slows it down
        }

        if (YaxisVar < 88)
        { // moving in reverse
          rYaxisVar += (180 - XaxisVar); // increasing the value - moving it closer to the center-point - slows it down
        }
      }
      LServoMtr.write(lYaxisVar);
      RServoMtr.write(rYaxisVar);

      Serial.print(lYaxisVar);
      Serial.print("   ");
      Serial.println(rYaxisVar);
      Serial.println();
    }
  //}
  else
  {
    LServoMtr.write(90);
    RServoMtr.write(90);
  }

XaxisVar = analogRead(Xaxis);  //Read X-Axis postion
  YaxisVar = analogRead(Yaxis);  //Read Y-Axis postion
  SpeedVar = analogRead(SpeedPot);  //Read Speed Pot postion  
  //XaxisVar=512;
  //YaxisVar=1023;

  // scale x and y axis so that both are zeroed at natural center of potentiometer, implement deadband
  if(YaxisVar > YCenterPnt + DeadZone)//forward
  {
    throttle = map(YaxisVar, YCenterPnt + DeadZone, 1023, 90, 180); //when joy 100% up, no turn, this is 180(fullspeed) :)    
    if(XaxisVar < XCenterPnt + DeadZone)//left motor
    {
      steering = -map(XaxisVar, XCenterPnt + DeadZone, 1023, 90, 15);// reverse pot coordinates to agree with cartesian
          
    }
    else if(XaxisVar > XCenterPnt - DeadZone)// right motor
    {
      steering = map(XaxisVar, XCenterPnt - DeadZone, 0, 90, 135);// reverse pot coordinates to agree with cartesian        
    }
    else // in X deadband
    {
      steering = 90;
    }
  }

  else if(YaxisVar < YCenterPnt - DeadZone)//reverse
  {
    throttle = map(YaxisVar, YCenterPnt - DeadZone, 0, 90, 0);   //when joy 100% down, no turn, this is 0(fullspeed reverse) :)   
    //  
    if(XaxisVar < XCenterPnt + DeadZone)//left
    {
      steering = map(XaxisVar, XCenterPnt + DeadZone, 1023, 90, 45);// reverse pot coordinates to agree with cartesian        
    }
    else if(XaxisVar > XCenterPnt - DeadZone)// right
    {
      steering = map(XaxisVar, XCenterPnt - DeadZone, 0, 90, 135);// reverse pot coordinates to agree with cartesian        
    }
    else// in X deadband
    {
      steering = 90;
    }
  }
  else //in Y deadband
  {
    throttle = 90;
    if(XaxisVar > XCenterPnt + DeadZone)//left
    {
      steering = map(XaxisVar, XCenterPnt + DeadZone, 1023, -90, -0);// reverse pot coordinates to agree with cartesian        
    }
    else if(XaxisVar < XCenterPnt - DeadZone)// right
    {
      steering = map(XaxisVar, XCenterPnt - DeadZone, 0, 90, 180);// reverse pot coordinates to agree with cartesian        
    }
    else// in X deadband
    {
      steering = 90;
    }
  }
 LmtrSpd = abs(throttle + steering);
  RmtrSpd = abs(throttle - steering);

  Serial.print(LmtrSpd);
  Serial.print("   ");
  Serial.println(RmtrSpd);
  Serial.println();

this one i know for sure the issue is in the mapping functions, but ive already tried changing the values it maps to, but no luck...

why cant i get these to work ? ive spent hours dont worry

system · January 7, 2012, 1:42pm

If the joystick value is greater than center + deadzone, it will have a value like 520 or greater, won't it? Mapping from 0 to 1023 to 0 to 180, when the value is really in the range 520 to 1023 doesn't make a lot of sense. Does it?

system · January 7, 2012, 1:56pm

if you want a simple method, I think you can possibly turn the joystick 45 degrees t the right or something, and making the X and Y axis control the joystick directly without any special algorithms. if your joystick is stuck like that, I guess you can use the horizontal value as a "multiplier" for the two servos as well.

sirbow2 · January 7, 2012, 4:38pm

ok so i went through the muxing code again, and this is what i managed:

if(YaxisVar > YCenterPnt + DeadZone)//forward
  {
    throttle = map(YaxisVar, YCenterPnt + DeadZone, 1023, 90, 180); //when joy 100% up, no turn, this is 180(fullspeed) :)    
    if(XaxisVar < XCenterPnt - DeadZone)//left
    {
      steering = map(XaxisVar, XCenterPnt - DeadZone, 0, 90, 0);
          
    }
    else if(XaxisVar > XCenterPnt + DeadZone)// right
    {
      steering = map(XaxisVar, XCenterPnt + DeadZone, 1023, 90, 180);      
    }
    else // in X deadband
    {
      steering = 0;
    }
  }

  else if(YaxisVar < YCenterPnt - DeadZone)//reverse
  {
    throttle = map(YaxisVar, YCenterPnt - DeadZone, 0, 90, 0);   //when joy 100% down, no turn, this is 0(fullspeed reverse) :)   
    //  
    if(XaxisVar < XCenterPnt - DeadZone)//left
    {
      steering = map(XaxisVar, XCenterPnt - DeadZone, 0, 90, 0);// reverse pot coordinates to agree with cartesian        
    }
    else if(XaxisVar > XCenterPnt + DeadZone)// right
    {
      steering = map(XaxisVar, XCenterPnt + DeadZone, 1023, 90, 180);// reverse pot coordinates to agree with cartesian        
    }
    else// in X deadband
    {
      steering = 0;
    }
  }
  else //in Y deadband
  {
    throttle = 90;
    if(XaxisVar > XCenterPnt + DeadZone)//right 
    {
      steering = map(XaxisVar, XCenterPnt + DeadZone, 1023, 90, 180);// reverse pot coordinates to agree with cartesian        
    }
    else if(XaxisVar < XCenterPnt - DeadZone)//left
    {
      steering = map(XaxisVar, XCenterPnt - DeadZone, 0, 90, 0);// reverse pot coordinates to agree with cartesian        
    }
    else// in X deadband
    {
      steering = 0;
    }
  }

  //transfer from x,y to l,r
  LmtrSpd = abs(throttle + steering);
  RmtrSpd = abs(throttle - steering);

the only issues are when im in y dead zone, and go right(X axis would = 1023) the values are almost equal to 90(stop) on both motors. full left (X=0) in y dead zone gives 270, and 90 for left and right servos... even more strangely, going upper left gives me 270 on the left servo and 90 on the right, but, 270 is way out of servo parameter and isnt quite the right proportion between the motors.

so im getting there...anyone see anything that would cause any issues like i describe?

sirbow2 · January 8, 2012, 12:27am

does anyone have example code i could look at or explanation of how to do differential mixing?

thanks

system · January 8, 2012, 12:36am

I think that printing XaxisVar and steering, as you move the joystick from one end of the range to the other might give you a clue.

sirbow2 · January 8, 2012, 12:39am

that's what ive been doing to see what its been outputting...

system · January 8, 2012, 12:40am

that's what ive been doing to see what its been outputting...

Would you care to share that?

ajofscott · January 8, 2012, 12:54am

This code was written for brushed motors on an H Bridge namely the motor3 shield. Deadband is to provide a bounding box around the center rest position of the stick so control doesn't happen on incidental stick movement. Map values before using analogWrite to a motor channel. The x Axis is added to one motor and subtracted from the other depending on which side of X_Center the analogRead value was. Y axis is equally applied for forward or reverse based on position above or below Y_Center. Note joysticks may not give a full range read on a full range of motion as most pots are 180 to 270 degree rotation and the gimbal assembly is mechanically limited to 90 degrees. I wrote the code you have referenced and will be more than willing to see you through.

sirbow2 · January 8, 2012, 1:05am

joy centered, left column is left servo, right column->right servo. each line is one refresh of mixing(besides the blank lines of course)

from center to right

from center to left

from center to back

from center to front

center to front right

center to front left

center to back right

center to back left

latest code:

//-----Axis-------------------------------------------------------//
#define Xaxis 0  //Joystick X axis Analog pin 3
#define Yaxis 1  //Joystick Y axis Analog pin 4
#define SpeedPot 5  //Pot in base for speed, Analog pin 5

int XaxisVar = 0;  //...analog pin 3---X axis
int YaxisVar = 0;  //...analog pin 4---Y axis
int SpeedVar = 0;  //...analog pin 5---Speed pot

int DeadZone = 50;
int XCenterPnt = 522;
int YCenterPnt = 497;
int throttle, steering;
//----------------------------------------------------------------//

//-----Buttons----------------------------------------------------//
#define TrigBtn 2  //trigger button, digital 2
#define MidBtn 6  //Button below trigger on joystick, digital 3
#define LowBtn 4  //Lowest button on the joystick, digital 4
#define BaseBtn 5  //button on base of joystick, digital 5

boolean TrigBtnState = 0;  //Stores the state of TrigBtn
boolean MidBtnState = 0;  //...MidBtnState
boolean LowBtnState = 0;  //...LowBtnState
boolean BaseBtnState = 0;  //...BaseBtnState
//----------------------------------------------------------------//

//-----Robot Stuff------------------------------------------------//
int LmtrSpd=0;
int RmtrSpd=0;
#define LServoMtrPin 5
#define RServoMtrPin 6
#include <Servo.h>
Servo LServoMtr;
Servo RServoMtr;
//----------------------------------------------------------------//

void setup() 
{
  Serial.begin(9600);
  LServoMtr.attach(LServoMtrPin);
  LServoMtr.write(90);
  RServoMtr.attach(RServoMtrPin);
  RServoMtr.write(90);


  for (int pin=2; pin<=6; ++pin)  //sets all digital pin 2-5 as input and enables internal pullups.
  {                           
    pinMode(pin, INPUT);  //sets button pins to input
    digitalWrite(pin, HIGH);  // sets the pull up.
  }
}

void ReadJoyBtns()
{
  TrigBtnState = digitalRead(TrigBtn);  //read status of buttons on joystick
  MidBtnState = digitalRead(MidBtn);  //...
  LowBtnState = digitalRead(LowBtn);  //...
  BaseBtnState = digitalRead(BaseBtn);  //...
}

void loop()
{ 
  XaxisVar = analogRead(Xaxis);  //Read X-Axis postion
  YaxisVar = analogRead(Yaxis);  //Read Y-Axis postion
  SpeedVar = analogRead(SpeedPot);  //Read Speed Pot postion  
  //XaxisVar=512;
  //YaxisVar=1023;

  // scale x and y axis so that both are zeroed at natural center of potentiometer, implement deadband
  if(YaxisVar > YCenterPnt + DeadZone)//forward
  {
    throttle = map(YaxisVar, YCenterPnt + DeadZone, 1023, 90, 180); //when joy 100% up, no turn, this is 180(fullspeed) :)    
    if(XaxisVar < XCenterPnt - DeadZone)//left
    {
      steering = map(XaxisVar, XCenterPnt - DeadZone, 0, 90, 0);       
    }
    else if(XaxisVar > XCenterPnt + DeadZone)// right
    {
      steering = map(XaxisVar, XCenterPnt + DeadZone, 1023, 90, 180);      
    }
    else // in X deadband
    {
      steering = 0;
    }
  }

  else if(YaxisVar < YCenterPnt - DeadZone)//reverse
  {
    throttle = map(YaxisVar, YCenterPnt - DeadZone, 0, 90, 0);   //when joy 100% down, no turn, this is 0(fullspeed reverse) :)   
    //  
    if(XaxisVar < XCenterPnt - DeadZone)//left
    {
      steering = map(XaxisVar, XCenterPnt - DeadZone, 0, 90, 0);// reverse pot coordinates to agree with cartesian        
    }
    else if(XaxisVar > XCenterPnt + DeadZone)// right
    {
      steering = map(XaxisVar, XCenterPnt + DeadZone, 1023, 90, 180);// reverse pot coordinates to agree with cartesian        
    }
    else// in X deadband
    {
      steering = 0;
    }
  }
  else //in Y deadband
  {
    throttle = 0;
    if(XaxisVar > XCenterPnt + DeadZone)//right 
    {
      steering = map(XaxisVar, XCenterPnt + DeadZone, 1023, 90, 180);// reverse pot coordinates to agree with cartesian        
    }
    else if(XaxisVar < XCenterPnt - DeadZone)//left
    {
      steering = map(XaxisVar, XCenterPnt - DeadZone, 0, 90, 0);// reverse pot coordinates to agree with cartesian        
    }
    else// in X deadband
    {
      steering = 90;
    }
  }

  //transfer from x,y to l,r
  LmtrSpd = abs(throttle + steering);
  RmtrSpd = abs(throttle - steering);

  Serial.print(LmtrSpd);
  Serial.print("   ");
  Serial.println(RmtrSpd);
  Serial.println();
  delay(100);

}

sirbow2 · January 8, 2012, 1:13am

ajofscott:
This code was written for brushed motors on an H Bridge namely the motor3 shield. Deadband is to provide a bounding box around the center rest position of the stick so control doesn't happen on incidental stick movement. Map values before using analogWrite to a motor channel. The x Axis is added to one motor and subtracted from the other depending on which side of X_Center the analogRead value was. Y axis is equally applied for forward or reverse based on position above or below Y_Center. Note joysticks may not give a full range read on a full range of motion as most pots are 180 to 270 degree rotation and the gimbal assembly is mechanically limited to 90 degrees. I wrote the code you have referenced and will be more than willing to see you through.

i saw PM thanks.

that makes sense, but why does mine not work...?

ajofscott · January 8, 2012, 1:17am

You need to map from 0 to 180, as 90 is rest.

sirbow2 · January 8, 2012, 1:43am

which ones though?? i thought since i was mapping the right side it would only be maping half of it like 0 to 90 not 0 to 180.

kf2qd · January 8, 2012, 1:57am

you will have to do a little more complex mixing.
stick centered, both servo outputs 90
Stick centered and full forward both servos 150
stick centered and full back both servos 40
stick any other position, full left or right stick adds 30 to one servo and subtracts 30 from the other.

May not be quite what you want, but it might give you a place to start.

ajofscott · January 8, 2012, 2:00am

0-1023 is absolute end point range, center is 511 so you are calculating distance from 511 above or below hence the abs(), the same is true for both channels x and y. This will give you values ranging from -511 to 511, you could do this with the map function as well, however true center may be a value other than 511. So for the moment assume ideal hardware. Y is at 255 roughly 50% forward, X is at 0 so we set both servos to 45 degrees. Now we move the stick right so X is now 255. We add 45 to the left servo and subtract 45 from the right servo making the net for each 90 left and 0 right. The right servo goes in reverse, and the left goes full speed forward, the vehicle yaws to the right as the left side overtakes the right. As you can see you don't necessarily want X to be as dominant as Y, you sill want the slow track still going forward at almost full hard turn. So map X to a more restricted range such as 45- 135.

sirbow2 · January 8, 2012, 2:23am

thanks for your help!

this link right here is a MIRACLE! Algorithm for mixing 2 axis analog input to control a differential motor drive - Electrical Engineering Stack Exchange

ill let you guys know how it works out and come back begging if it doesnt

sirbow2 · January 9, 2012, 1:41pm

well i turns out i can get my continuous rotation servos to vary their speed when i "send" them 135 or 113, they spin as they would at 180. so i just used if statements for basic left right up down

but i have an issue:
im trying to use my IMU+FC on a two wheel balance bot while i wait for my quadcopter parts in the mail. i dont know how to do PIDs or vector correction in arduino for sensors so i just used the provided AHRS software from pololu(i have a 9DOF IMU). it outputs pitch, roll, and yaw(all outputted in degree/integer form). i calibrated the sensors at level position, then brought the bot to 90deg(where the bot would balance upright), and recorded the degree. then i created simple if statements say basically: if less than center balance degree, turnn motors to compensate/same for if robot is higher than the previously recorded bot center degree/ finally if currently balanced, lock motors.

the problem is; since im starting the bot flat against the floor to calibrate the sensors, it thinks it is completely far away from the center point and kicks in the motors once calibration has complete. then, when i lift the robot to center point, it resets the arduino??? but if i move the bot quickly to the opposite side of center, it doesn't reset, but motors don't change directions to compensate for the robot being on the opposite side. if i take out the actual sending of the servo commands and just output to serial, i can verify that it works, just not when i actually hooked up to the motors. why does servo code cause the Arduino to freeze?

here is the code that manages which way the motors turn according to the output of pitch.
Code:

tempVal=ToDeg(pitch);
if(tempVal>=82 && tempVal<=83)
  {
    //Serial.println(" 3");
    LServoMtrSpd=90;
    RServoMtrSpd=90;
  }
  if(tempVal < 82)
  {
    //Serial.println("1");
    RServoMtrSpd=68;
    LServoMtrSpd=113;
  }
  else if(tempVal > 83)
  {
    //Serial.println(" 2");
    RServoMtrSpd=113;
    LServoMtrSpd=68;
  }

  LServoMtr.write(LServoMtrSpd);
  RServoMtr.write(RServoMtrSpd);

also, the loop only takes ~5ms, and servos can refresh every ~20-40ms, so i added in a calculation to add in a delay to the end of the loop that = 25-the time the loop actually took.

please help if you can. IMU and AHRS code here: Pololu - MinIMU-9 Gyro, Accelerometer, and Compass (L3G4200D and LSM303DLM Carrier) hope it makes sense

sirbow2 · January 10, 2012, 5:55am

anyone have ideas for the IMU or servo issue?
the arduino controls like a robot OK with 4 directions and if statements, but just not the same as real variable servos( i mean my servos that currently wont vary like they should)

zoomkat · January 10, 2012, 6:25am

but just not the same as real variable servos

Perhaps you need to provide more detail on your specific issue and your concept of "real variable servos ".

sirbow2 · January 10, 2012, 6:46am

ok
i have these parallax continuous servos: http://www.parallax.com/tabid/768/ProductID/102/Default.aspx

i have the servos from my BOE BOT and they work great with the BS2(can vary the speed and change directions etc.), but with Arduino, i cant vary the speed. With arduino 0 is reverse, 90 stop, 180 forward. i should be able to "write" a value like 135 and get half speed in that direction, but it still turns like i set it to 180. same with 91; it will run like 180.

so basically any value 0 to 89 goes one way full speed,90 is stop, 91 to 180 any value is the other way full speed. does the boebot servo have a different PWM standard then the average servo?

Topic		Replies	Views
Servo Speed on XY Axis Programming	32	5108	May 5, 2021
Differential Steering using a joystick / programming question.... Robotics	8	22147	May 6, 2021
HELP: 1 joystick 2 continuous servo motor General Guidance	5	2712	May 5, 2021
Joystick 2 servos x and y postitions General Guidance	16	1128	January 26, 2023
Continuous rotation servos and joystick Programming	9	3987	May 5, 2021

2 continuous servo differential with one joystick

Related topics