Throttle speed changes according to variable ping threshold value.

Projects Programming
1

I decided to start a new topic as this is more specific to what I'm currently attempting.
So far results are good and movement is smooth.

*Just a few problems:
-Right when I upload my sketch, the motors temporarily go in reverse (for a few seconds) and then code works as I want it to
-At the moment acceleration/deceleration is a bit slow. I would like to modify this by increasing the ++ and -- in the void handleAcceleration() function...just wondering the best way.

Here is the sketch:

//Sabertooth 2X25 motor controlled robot in RC microcontroller (mixed) mode
// with differential & exponential settings.
//S1 controls throttle.
//S2 controls steering.
//Date:  7Jan14

#include <Servo.h>
#define N_PINGS 4
const byte pingPins [N_PINGS] = {
  22, 23, 24, 25};   // PING pins
const int minRanges [N_PINGS] = {
  14, 14, 36, 14};
Servo servoThrottle; // create servo object for throttle
Servo servoSteering; // create servo object steering

int currentSpeed = 0; // variable to store the servo current speed
int targetSpeed = 0; // variable to store the servo target speed
unsigned long lastSpeedUpdateTime = 0;
const unsigned long SPEED_UPDATE_INTERVAL = 10; // ms



void setup()
{
  Serial.begin(9600);
  servoThrottle.attach(53, 1000, 2000); // attach the throttle servo  
  servoSteering.attach(52, 1000, 2000); // attach the steering servo 
}



void loop()
{
  handleAcceleration();
  servoThrottle.write(currentSpeed);
  //...
}  


void setSpeed(int requiredSpeed)
{
  int range [N_PINGS];
  range [2] = ping(pingPins [2]);
  delay(5); 
  int centerThresh = range [2];

  centerThresh = map(centerThresh, 14, 54, 1500, 2000);
  requiredSpeed = constrain(centerThresh, 1500, 2000);  

  targetSpeed = requiredSpeed;
}


void handleAcceleration()
{
  setSpeed(targetSpeed);
  unsigned long now = millis();
  if(now - lastSpeedUpdateTime > SPEED_UPDATE_INTERVAL)
  {
    // time to check the motor speed again
    if(currentSpeed < targetSpeed)
    {
      // accelerating
      currentSpeed++;
      servoThrottle.write(currentSpeed);
    }
    else if(currentSpeed > targetSpeed)
    {
      // deccelerating
      currentSpeed--;
      servoThrottle.write(currentSpeed);
    }
    else
    {
      // motor is already at the target speed - no action required
    }
    lastSpeedUpdateTime = now;
  }
}


void pingCalc()
{
  int range [N_PINGS];
  for (int i = 0; i < N_PINGS; ++i) {
    range [i] = ping(pingPins [i]);
    //delay(5);
  }  

}


long ping(int pin)
{

  pinMode(pin, OUTPUT);
  digitalWrite(pin, LOW);
  delayMicroseconds(2);
  digitalWrite(pin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pin, LOW);
  pinMode(pin, INPUT);

  return (microsecondsToInches(pulseIn(pin, HIGH)));
}

long microsecondsToInches(long microseconds)
{
  return microseconds / 74 / 2;
}


// The following are for object tracking/follow with Kinect sensor
void motForward()
{

}


void motBackward()
{

}


void motStop()
{

}


void motRight()
{

}


void motLeft()
{

}


void serialEvent()
{
  
}
2

I would like to modify this by increasing the ++ and -- in the void handleAcceleration() function...just wondering the best way.

Instead of sneaking up on the final speed in increments of 1, increment by 10 % of the difference, or 1, which ever is greater.

The NewPing library is non-blocking. The pulseIn() function IS blocking. Just so you know...

3

PaulS:

I would like to modify this by increasing the ++ and -- in the void handleAcceleration() function...just wondering the best way.

Instead of sneaking up on the final speed in increments of 1, increment by 10 % of the difference, or 1, which ever is greater.

The NewPing library is non-blocking. The pulseIn() function IS blocking. Just so you know...

Thanks for the reply Paul.

So possibly using a For loop?:

 for(int i = 1; i <= 10; i++)  //accel

Also thought of it this way:

for(int i=1; i <= 10; i++)  //accel     
 {
currentSpeed*i;
      servoThrottle.write(currentSpeed);
 }
4

No, not like that at all.

If you are accelerating, it is because the current speed is less than the target speed. So:

int incr = (targetSpeed - currentSpeed) / 10;
if(incr == 0)
   incr = 1;
currentSpeed += incr;

If you are decelerating, it is because the current speed is greater than the target speed, reverse targetSpeed and currentSpeed and use -= instead.

5

Thanks Paul,
The accel/decel with threshold distance part of the code works great.

I'm trying now to incorporate the steering according to data received from the Kinect camera.
Incoming data is the X axis values of the object being tracked.
The width of the camera screen is 640, so values range from 0 to 640 and 320 being center.
I setup a threshold of something like 160>=centerthresh<=480. If the object is on the left of the screen or<160 then steer left until object is in center threshold. If object is on the right of the screen or >480 then steer right until object is in center threshold.

Some things I was unsure of doing:
-Should I use acceleration/deceleration for the steering? Something similar to the handleAcceleration() function?
-Should the steering be independent to the throttle accel/decel functions and actions?
-should I/could I combine the serialEvent() and handleCommand() functions?

Here is my code as of now:

//Sabertooth 2X25 motor controlled robot in RC microcontroller (mixed) mode
// with differential & exponential settings.
//S1 controls throttle.
//S2 controls steering.
//Date:  7Jan14

#include <Servo.h>
#define N_PINGS 4
const byte pingPins [N_PINGS] = {
  22, 23, 24, 25};   // PING pins
const int minRanges [N_PINGS] = {
  14, 14, 36, 14};

String command; // a string to hold the incoming command
#define SOP '<'
#define EOP '>'
boolean commandReceived = false;
int valAxisX =0;  //Variable to hold incoming Kinect camera X axis values

Servo servoThrottle; // create servo object for throttle
Servo servoSteering; // create servo object steering

int currentSpeed = 0; // variable to store the servo current speed
int targetSpeed = 0; // variable to store the servo target speed


unsigned long lastSpeedUpdateTime = 0;

const unsigned long SPEED_UPDATE_INTERVAL = 10; // ms
const int neutralSpeed = 1500;
const int neutralSteer = 1500;
int currSteerSpeed =0;

void setup()
{
  Serial.begin(9600);
  servoThrottle.attach(53, 1000, 2000); // attach the throttle servo  
  servoSteering.attach(52, 1000, 2000); // attach the steering servo 
}



void loop()
{
  pingCalc();
  serialEvent();
  handleCommand();
  handleAcceleration();
  servoThrottle.write(currentSpeed);



}


void setSpeed(int requiredSpeed)
{
  int range [N_PINGS];
  range [2] = ping(pingPins [2]);
  delay(5); 
  int centerThresh = range [2];

  centerThresh = map(centerThresh, 14, 54, 1500, 2000);
  requiredSpeed = constrain(centerThresh, 1500, 2000);  

  targetSpeed = requiredSpeed;
}


void handleAcceleration()
{
  setSpeed(targetSpeed);
  unsigned long now = millis();
  if(now - lastSpeedUpdateTime > SPEED_UPDATE_INTERVAL)
  {
    // time to check the motor speed again
    if(currentSpeed < targetSpeed)
    {
      // accelerating
      int incr = (targetSpeed - currentSpeed) / 10;
      if(incr == 0)
        incr = 1;
      currentSpeed += incr;
      servoThrottle.write(currentSpeed);
    }
    else if(currentSpeed > targetSpeed)
    {
      // deccelerating
      int incr = (currentSpeed - targetSpeed) / 10;
      if(incr == 0)
        incr = 1;
      currentSpeed -= incr;
      servoThrottle.write(currentSpeed);
    }
    else
    {
      // motor is already at the target speed - no action required
    }
    lastSpeedUpdateTime = now;
  }
}


void leftSteer()
{
}


void rightSteer()
{
}


int pingCalc()
{
  int range [N_PINGS];
  for (int i = 0; i < N_PINGS; ++i) {
    range [i] = ping(pingPins [i]);
    //delay(5);
  }  
  return range [0];
}


long ping(int pin)
{

  pinMode(pin, OUTPUT);
  digitalWrite(pin, LOW);
  delayMicroseconds(2);
  digitalWrite(pin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pin, LOW);
  pinMode(pin, INPUT);

  return (microsecondsToInches(pulseIn(pin, HIGH)));
}

long microsecondsToInches(long microseconds)
{
  return microseconds / 74 / 2;
}


// The following are for object tracking/follow with Kinect sensor



void serialEvent()
{
  while (Serial.available()) 
  {
    // all we do is construct the incoming command to be handled in the main loop

    // get the incoming byte from the serial stream
    char incomingByte = (char)Serial.read();

    if (incomingByte == EOP)
    {
      // marks the end of a command
      commandReceived = true;
      return;
    }
    else if (incomingByte == SOP)
    {
      // marks the start of a new command
      command = "";
      commandReceived = false;
      return;
    }
    else
    {
      command += incomingByte;
      return;
    }
  }
}


void handleCommand() 
{

  if (!commandReceived) return; // no command to handle

  // variables to hold the command id and the command data

  int data[17]; 

  char cmd[command.length()+1];
  command.toCharArray(cmd, command.length()+1);
  char *token = strtok(cmd, ",");


  // data  
  for (int i = 0; i < 17; i++) 
  {

    if (token) 
    {
      data[i] = atoi(token);
      token = strtok(NULL, ",");
    }

  }

  //test
  //Serial.println(command);
  //Serial.print("data[0] = ");
  //Serial.println(data[0]);


  valAxisX = data[0];

  // flag that we've handled the command 
  commandReceived = false;
}
6

-Should I use acceleration/deceleration for the steering? Something similar to the handleAcceleration() function?

I think you are using acceleration where you mean something else. You want to make the amount to turn proportional to the angle that the object is off from directly in center. A small amount off requires a small correction. A large amount off requires a large correction. I don't see acceleration playing a role.

-Should the steering be independent to the throttle accel/decel functions and actions?

Yes. That is how people move. Looking left and right, and focusing on a destination is independent on how fast you approach the destination.

-should I/could I combine the serialEvent() and handleCommand() functions?

My opinion is that serialEvent() should never have been added. Handling serial data is not a separate activity. It should be handled in loop(). So, my recommendation here is no.

Even in loop(), you read and store some serial data, until you get an end-of-packet marker. Then, you call handleCommand() to determine what the serial data is telling you to do.

7

PaulS:

-Should I use acceleration/deceleration for the steering? Something similar to the handleAcceleration() function?

I think you are using acceleration where you mean something else. You want to make the amount to turn proportional to the angle that the object is off from directly in center. A small amount off requires a small correction. A large amount off requires a large correction. I don't see acceleration playing a role.

Ahh, I understand. It would be more of a timing thing. The further away from center threshold the more time steering back to center? etc...

8

It would be more of a timing thing. The further away from center threshold the more time steering back to center?

I don't see this as a timing issue, at all. You are heading some direction. It is either spot on, or it isn't. If it isn't, you make a correction. The amount of correction is dependent on how far off course you are.

Then, you check again, and correct again.

The only role that timing plays is in how often you check and how much you correct your off-course-ness. If you check often, you make small corrections. If you check once a month, you make larger corrections.

9

PaulS:

It would be more of a timing thing. The further away from center threshold the more time steering back to center?

I don't see this as a timing issue, at all. You are heading some direction. It is either spot on, or it isn't. If it isn't, you make a correction. The amount of correction is dependent on how far off course you are.

Then, you check again, and correct again.

The only role that timing plays is in how often you check and how much you correct your off-course-ness. If you check often, you make small corrections. If you check once a month, you make larger corrections.

I updated my sketch, let me know if I'm getting close

//Sabertooth 2X25 motor controlled robot in RC microcontroller (mixed) mode
// with differential & exponential settings.
//S1 controls throttle.
//S2 controls steering.
//Date:  7Jan14

#include <Servo.h>
#define N_PINGS 4
const byte pingPins [N_PINGS] = {
  22, 23, 24, 25};   // PING pins
const int minRanges [N_PINGS] = {
  14, 14, 36, 14};

String command; // a string to hold the incoming command
#define SOP '<'
#define EOP '>'
boolean commandReceived = false;
int valAxisX =0;  //Variable to hold incoming Kinect camera X axis values

Servo servoThrottle; // create servo object for throttle
Servo servoSteering; // create servo object steering

int currentSpeed = 0; // variable to store the servo current speed
int targetSpeed = 0; // variable to store the servo target speed


unsigned long lastSpeedUpdateTime = 0;

const unsigned long SPEED_UPDATE_INTERVAL = 10; // ms
const int neutralSpeed = 1500;
const int neutralSteer = 1500;
int angle =0;

void setup()
{
  Serial.begin(9600);
  servoThrottle.attach(53, 1000, 2000); // attach the throttle servo  
  servoSteering.attach(52, 1000, 2000); // attach the steering servo 
}



void loop()
{
  while (Serial.available()) 
  {
    // all we do is construct the incoming command to be handled in the main loop

    // get the incoming byte from the serial stream
    char incomingByte = (char)Serial.read();

    if (incomingByte == EOP)
    {
      // marks the end of a command
      commandReceived = true;
      return;
    }
    else if (incomingByte == SOP)
    {
      // marks the start of a new command
      command = "";
      commandReceived = false;
      return;
    }
    else
    {
      command += incomingByte;
      return;
    }

  }

pingCalc();
handleAcceleration();
servoThrottle.write(currentSpeed);
handleCommand();
if((valAxisX >= 0) && (valAxisX <= 160))
{
  leftSteer();
}

else if((valAxisX >= 480) && (valAxisX <= 640))
{
  rightSteer();
}


}



void setSpeed(int requiredSpeed)
{
  int range [N_PINGS];
  range [2] = ping(pingPins [2]);
  delay(5); 
  int centerThresh = range [2];

  centerThresh = map(centerThresh, 14, 54, 1500, 2000);
  requiredSpeed = constrain(centerThresh, 1500, 2000);  

  targetSpeed = requiredSpeed;
}


void handleAcceleration()
{
  setSpeed(targetSpeed);
  unsigned long now = millis();
  if(now - lastSpeedUpdateTime > SPEED_UPDATE_INTERVAL)
  {
    // time to check the motor speed again
    if(currentSpeed < targetSpeed)
    {
      // accelerating
      int incr = (targetSpeed - currentSpeed) / 10;
      if(incr == 0)
        incr = 1;
      currentSpeed += incr;
      //currentSpeed++;
      servoThrottle.write(currentSpeed);
    }
    else if(currentSpeed > targetSpeed)
    {
      // deccelerating
      int incr = (currentSpeed - targetSpeed) / 10;
      if(incr == 0)
        incr = 1;
      currentSpeed -= incr;
      //currentSpeed--;
      servoThrottle.write(currentSpeed);
    }
    else
    {
      // motor is already at the target speed - no action required
    }
    lastSpeedUpdateTime = now;
  }
}


void leftSteer()
{
for(angle = valAxisX; angle < 220; angle += 10)
{
  servoSteering.write(angle);
}
  
}


void rightSteer()
{
for(angle = valAxisX; angle > 420; angle -= 10)
{
  servoSteering.write(angle);
}

}


int pingCalc()
{
  int range [N_PINGS];
  for (int i = 0; i < N_PINGS; ++i) {
    range [i] = ping(pingPins [i]);
    //delay(5);
  }  
  return range [0];
}


long ping(int pin)
{

  pinMode(pin, OUTPUT);
  digitalWrite(pin, LOW);
  delayMicroseconds(2);
  digitalWrite(pin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pin, LOW);
  pinMode(pin, INPUT);

  return (microsecondsToInches(pulseIn(pin, HIGH)));
}

long microsecondsToInches(long microseconds)
{
  return microseconds / 74 / 2;
}


// The following are for object tracking/follow with Kinect sensor




void handleCommand() 
{

  if (!commandReceived) return; // no command to handle

  // variables to hold the command id and the command data

  int data[17]; 

  char cmd[command.length()+1];
  command.toCharArray(cmd, command.length()+1);
  char *token = strtok(cmd, ",");


  // data  
  for (int i = 0; i < 17; i++) 
  {

    if (token) 
    {
      data[i] = atoi(token);
      token = strtok(NULL, ",");
    }

  }

  //test
  //Serial.println(command);
  //Serial.print("data[0] = ");
  //Serial.println(data[0]);


  valAxisX = data[0];

  // flag that we've handled the command 
  commandReceived = false;
}
10 
    if (incomingByte == EOP)
    {
      // marks the end of a command
      commandReceived = true;
      return;
    }
    else if (incomingByte == SOP)
    {
      // marks the start of a new command
      command = "";
      commandReceived = false;
      return;
    }
    else
    {
      command += incomingByte;
      return;
    }

Every time you get a new character, start loop() over again. Why? (That is what return is doing.)

handleCommand();

Regardless of whether or not you got a complete packet, handle the command. No, I don't think so.

void leftSteer()
{
for(angle = valAxisX; angle < 220; angle += 10)
{
  servoSteering.write(angle);
}
  
}

Regardless of how far off course you are, turn the same amount. No, I don't think so. (You will start with the steering angle close to the amount you are off course, but you will end up steering to 220.) (Or 420, in the other case.)

leftSteer() and rightSteer() should accept arguments - the point to steer towards. The call to the function should include the correct angle, based on the angle between the current heading and the desired heading. The functions should not be using for loops.

11

PaulS:

    if (incomingByte == EOP)

{
     // marks the end of a command
     commandReceived = true;
     return;
   }
   else if (incomingByte == SOP)
   {
     // marks the start of a new command
     command = "";
     commandReceived = false;
     return;
   }
   else
   {
     command += incomingByte;
     return;
   }



Every time you get a new character, start loop() over again. Why? (That is what return is doing.)

So, would I only need a 'return;' if I get a EOP?

if (incomingByte == EOP)
    {
      // marks the end of a command
      commandReceived = true;
     }
   return;
else if (incomingByte == SOP)
    {
      // marks the start of a new command
      command = "";
      commandReceived = false;
  
    }
    else
    {
      command += incomingByte;
 }

How about using something along these lines:

void handleAngle()
{
  int targetAngle =0;
  unsigned long now = millis();
  if(now - lastAngleUpdateTime > ANGLE_UPDATE_INTERVAL)
  {
    
    if(currentAngle < targetAngle)
    {
      //left steer
      int incr = (targetAngle - currentAngle) / 10;
      if(incr == 0)
        incr = 1;
      currentAngle += incr;
      
      servoSteering.write(currentAngle);
    }
    else if(currentAngle > targetAngle)
    {
      // right steer
      int incr = (currentAngle - targetAngle) / 10;
      if(incr == 0)
        incr = 1;
      currentAngle -= incr;
      
      servoSteering.write(currentAngle);
    }
    else
    {
      // Already at target angle..
    }
    lastAngleUpdateTime = now;
  }
}
12

So, would I only need a 'return;' if I get a EOP?

Why return at all? That is a cue to call handleCommand(), not to restart loop().

13

How about using something along these lines:

Well, I don't see where currentAngle is defined, and I see no purpose in defining targetAngle to 0. currentAngle, as that code is currently written is either positive or negative. so, targetAngle (= 0) doesn't seem to contribute much.

14

PaulS:

How about using something along these lines:

Well, I don't see where currentAngle is defined, and I see no purpose in defining targetAngle to 0. currentAngle, as that code is currently written is either positive or negative. so, targetAngle (= 0) doesn't seem to contribute much.

//Sabertooth 2X25 motor controlled robot in RC microcontroller (mixed) mode
// with differential & exponential settings.
//S1 controls throttle.
//S2 controls steering.
//Date:  7Jan14

#include <Servo.h>
#define N_PINGS 4
const byte pingPins [N_PINGS] = {
  22, 23, 24, 25};   // PING pins
const int minRanges [N_PINGS] = {
  14, 14, 36, 14};

String command; // a string to hold the incoming command
#define SOP '<'
#define EOP '>'
boolean commandReceived = false;
int valAxisX =0;  //Variable to hold incoming Kinect camera X axis values

Servo servoThrottle; // create servo object for throttle
Servo servoSteering; // create servo object steering

int currentSpeed = 0; // variable to store the servo current speed
int targetSpeed = 0; // variable to store the servo target speed


unsigned long lastSpeedUpdateTime = 0;
unsigned long lastPosUpdateTime = 0;
const unsigned long SPEED_UPDATE_INTERVAL = 10; // ms
const unsigned long POS_UPDATE_INTERVAL = 10; //ms 
const int neutralSpeed = 1500;
const int neutralSteer = 1500;
int currentPos = valAxisX;
int targetPos = constrain(valAxisX, 220, 420);

void setup()
{
  Serial.begin(9600);
  servoThrottle.attach(53, 1000, 2000); // attach the throttle servo  
  servoSteering.attach(52, 1000, 2000); // attach the steering servo 
}



void loop()
{
  while (Serial.available()) 
  {
    // all we do is construct the incoming command to be handled in the main loop

    // get the incoming byte from the serial stream
    char incomingByte = (char)Serial.read();

    if (incomingByte == EOP)
    {
      // marks the end of a command
      commandReceived = true;

    }
    else if (incomingByte == SOP)
    {
      // marks the start of a new command
      command = "";
      commandReceived = false;

    }
    else
    {
      command += incomingByte;

    }

  }

  pingCalc();
  handleAcceleration();
  servoThrottle.write(currentSpeed);
  handleCommand();
  if((valAxisX >= 0) && (valAxisX <= 160))
  {
    leftSteer();
  }

  else if((valAxisX >= 480) && (valAxisX <= 640))
  {
    rightSteer();
  }


}



void setSpeed(int requiredSpeed)
{
  int range [N_PINGS];
  range [2] = ping(pingPins [2]);
  delay(5); 
  int centerThresh = range [2];

  centerThresh = map(centerThresh, 14, 54, 1500, 2000);
  requiredSpeed = constrain(centerThresh, 1500, 2000);  

  targetSpeed = requiredSpeed;
}


void handleAcceleration()
{
  setSpeed(targetSpeed);
  unsigned long now = millis();
  if(now - lastSpeedUpdateTime > SPEED_UPDATE_INTERVAL)
  {
    // time to check the motor speed again
    if(currentSpeed < targetSpeed)
    {
      // accelerating
      int incr = (targetSpeed - currentSpeed) / 10;
      if(incr == 0)
        incr = 1;
      currentSpeed += incr;
      servoThrottle.write(currentSpeed);
    }
    else if(currentSpeed > targetSpeed)
    {
      // deccelerating
      int incr = (currentSpeed - targetSpeed) / 10;
      if(incr == 0)
        incr = 1;
      currentSpeed -= incr;
      servoThrottle.write(currentSpeed);
    }
    else
    {
      // motor is already at the target speed - no action required
    }
    lastSpeedUpdateTime = now;
  }
}




void leftSteer()
{
  valAxisX = map(valAxisX, 0, 640, 1000, 2000);
  unsigned long now = millis();
  if(now - lastPosUpdateTime > POS_UPDATE_INTERVAL)
  {

    if(currentPos < targetPos)
    {
      //left steer
      int incr = (targetPos - currentPos) / 10;
      if(incr == 0)
        incr = 1;
      currentPos += incr;

      servoThrottle.write(currentPos);
    }
    else
    {
      // Already at target angle..
    }
    lastPosUpdateTime = now;
  }
}



void rightSteer()
{
  valAxisX = map(valAxisX, 0, 640, 1000, 2000);
  unsigned long now = millis();
  if(now - lastPosUpdateTime > POS_UPDATE_INTERVAL)
  { 
    if(currentPos > targetPos)
    {
      // right steer
      int incr = (currentPos - targetPos) / 10;
      if(incr == 0)
        incr = 1;
      currentPos -= incr;

      servoThrottle.write(currentPos);
    }
    else
    {
      // Already at target angle..
    }
    lastPosUpdateTime = now;
  }
}

int pingCalc()
{
  int range [N_PINGS];
  for (int i = 0; i < N_PINGS; ++i) {
    range [i] = ping(pingPins [i]);
    //delay(5);
  }  
  return range [0];
}


long ping(int pin)
{

  pinMode(pin, OUTPUT);
  digitalWrite(pin, LOW);
  delayMicroseconds(2);
  digitalWrite(pin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pin, LOW);
  pinMode(pin, INPUT);

  return (microsecondsToInches(pulseIn(pin, HIGH)));
}

long microsecondsToInches(long microseconds)
{
  return microseconds / 74 / 2;
}


void handleCommand() 
{

  if (!commandReceived) return; // no command to handle

  // variables to hold the command id and the command data

  int data[17]; 

  char cmd[command.length()+1];
  command.toCharArray(cmd, command.length()+1);
  char *token = strtok(cmd, ",");


  // data  
  for (int i = 0; i < 17; i++) 
  {

    if (token) 
    {
      data[i] = atoi(token);
      token = strtok(NULL, ",");
    }

  }

  //test
  //Serial.println(command);
  //Serial.print("data[0] = ");
  //Serial.println(data[0]);


  valAxisX = data[0];

  // flag that we've handled the command 
  commandReceived = false;
}
15

Here's my left & right Steer functions.

void leftSteer()
{

  unsigned long now = millis();
  if(now - lastPosUpdateTime > POS_UPDATE_INTERVAL)
  {

    if(valAxisX < targetPos)
    {
      //left steer
      int incr = (targetPos - valAxisX) / 10;
      if(incr == 0)
        incr = 1;
      valAxisX += incr;

      servoThrottle.write(currentPos);
    }
    else
    {
      // Already at target angle..
    }
    lastPosUpdateTime = now;
  }
}



void rightSteer()
{

  unsigned long now = millis();
  if(now - lastPosUpdateTime > POS_UPDATE_INTERVAL)
  { 
    if(valAxisX > targetPos)
    {
      // right steer
      int incr = (valAxisX - targetPos) / 10;
      if(incr == 0)
        incr = 1;
      valAxisX -= incr;

      servoThrottle.write(currentPos);
    }
    else
    {
      // Already at target angle..
    }
    lastPosUpdateTime = now;
  }
}

Not sure on how to convert the values of valAxisX to servo values...
Possibly?:

currentPos = map(valAxisX, 0, 640, 1000, 2000);

And full sketch:

//Sabertooth 2X25 motor controlled robot in RC microcontroller (mixed) mode
// with differential & exponential settings.
//S1 controls throttle.
//S2 controls steering.
//Date:  7Jan14

#include <Servo.h>
#define N_PINGS 4
const byte pingPins [N_PINGS] = {
  22, 23, 24, 25};   // PING pins
const int minRanges [N_PINGS] = {
  14, 14, 36, 14};

String command; // a string to hold the incoming command
#define SOP '<'
#define EOP '>'
boolean commandReceived = false;
int valAxisX =0;
const int targetPos = 320;

Servo servoThrottle; // create servo object for throttle
Servo servoSteering; // create servo object steering

int currentSpeed = 0; // variable to store the servo current speed
int targetSpeed = 0; // variable to store the servo target speed


unsigned long lastSpeedUpdateTime = 0;
unsigned long lastPosUpdateTime = 0;
const unsigned long SPEED_UPDATE_INTERVAL = 10; // ms
const unsigned long POS_UPDATE_INTERVAL = 10; // ms
const int neutralSpeed = 1500;
const int neutralSteer = 1500;
int currentPos = 0;

void setup()
{
  Serial.begin(9600);
  servoThrottle.attach(53, 1000, 2000); // attach the throttle servo  
  servoSteering.attach(52, 1000, 2000); // attach the steering servo 
}



void loop()
{
  while (Serial.available()) 
  {
    // all we do is construct the incoming command to be handled in the main loop

    // get the incoming byte from the serial stream
    char incomingByte = (char)Serial.read();

    if (incomingByte == EOP)
    {
      // marks the end of a command
      commandReceived = true;

    }
    else if (incomingByte == SOP)
    {
      // marks the start of a new command
      command = "";
      commandReceived = false;

    }
    else
    {
      command += incomingByte;

    }

  } 




  pingCalc();
  handleAcceleration();
  servoThrottle.write(currentSpeed);
  handleCommand();
  if((valAxisX >= 0) && (valAxisX <= 160))
  {
    leftSteer();
  }

  else if((valAxisX >= 480) && (valAxisX <= 640))
  {
    rightSteer();
  }  


}



void setSpeed(int requiredSpeed)
{
  int range [N_PINGS];
  range [2] = ping(pingPins [2]);
  delay(5); 
  int centerThresh = range [2];

  centerThresh = map(centerThresh, 14, 54, 1500, 2000);
  requiredSpeed = constrain(centerThresh, 1500, 2000);  

  targetSpeed = requiredSpeed;
}


void handleAcceleration()
{
  setSpeed(targetSpeed);
  unsigned long now = millis();
  if(now - lastSpeedUpdateTime > SPEED_UPDATE_INTERVAL)
  {
    // time to check the motor speed again
    if(currentSpeed < targetSpeed)
    {
      // accelerating
      int incr = (targetSpeed - currentSpeed) / 10;
      if(incr == 0)
        incr = 1;
      currentSpeed += incr;
      servoThrottle.write(currentSpeed);
    }
    else if(currentSpeed > targetSpeed)
    {
      // deccelerating
      int incr = (currentSpeed - targetSpeed) / 10;
      if(incr == 0)
        incr = 1;
      currentSpeed -= incr;
      servoThrottle.write(currentSpeed);
    }
    else
    {
      // motor is already at the target speed - no action required
    }
    lastSpeedUpdateTime = now;
  }
}



void leftSteer()
{

  unsigned long now = millis();
  if(now - lastPosUpdateTime > POS_UPDATE_INTERVAL)
  {

    if(valAxisX < targetPos)
    {
      //left steer
      int incr = (targetPos - valAxisX) / 10;
      if(incr == 0)
        incr = 1;
      valAxisX += incr;

      servoThrottle.write(currentPos);
    }
    else
    {
      // Already at target angle..
    }
    lastPosUpdateTime = now;
  }
}



void rightSteer()
{

  unsigned long now = millis();
  if(now - lastPosUpdateTime > POS_UPDATE_INTERVAL)
  { 
    if(valAxisX > targetPos)
    {
      // right steer
      int incr = (valAxisX - targetPos) / 10;
      if(incr == 0)
        incr = 1;
      valAxisX -= incr;

      servoThrottle.write(currentPos);
    }
    else
    {
      // Already at target angle..
    }
    lastPosUpdateTime = now;
  }
}







int pingCalc()
{
  int range [N_PINGS];
  for (int i = 0; i < N_PINGS; ++i) {
    range [i] = ping(pingPins [i]);
    //delay(5);
  }  
  return range [0];
}


long ping(int pin)
{

  pinMode(pin, OUTPUT);
  digitalWrite(pin, LOW);
  delayMicroseconds(2);
  digitalWrite(pin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pin, LOW);
  pinMode(pin, INPUT);

  return (microsecondsToInches(pulseIn(pin, HIGH)));
}

long microsecondsToInches(long microseconds)
{
  return microseconds / 74 / 2;
}


// The following are for object tracking/follow with Kinect sensor




void handleCommand() 
{

  if (!commandReceived) return; // no command to handle

  // variables to hold the command id and the command data

  int data[17]; // up to three integers of data

  char cmd[command.length()+1];
  command.toCharArray(cmd, command.length()+1);
  char *token = strtok(cmd, ",");


  // data  
  for (int i = 0; i < 17; i++) 
  {

    if (token) 
    {
      data[i] = atoi(token);
      token = strtok(NULL, ",");
    }

  }

  //test
  //Serial.println(command);
  //Serial.print("data[0] = ");
  //Serial.println(data[0]);


  valAxisX = data[0];

  // flag that we've handled the command 
  commandReceived = false;
}

Any help appreciated..

16

Since you're using servo.write, you'll need a value between 0 and 180 for currentpos (if your servo can manage those angles) so:

currentPos = map(valAxisX, 0, 640, 0, 180);

There is some strange stuff going on with arrays:

int pingCalc()
{
  int range [N_PINGS];
  for (int i = 0; i < N_PINGS; ++i) {
    range [i] = ping(pingPins [i]);
    //delay(5);
  }  
  return range [0];
}

You're building an array of ping data which is then thrown away when the function exits, returning only the first element; why use an array at all?

The same pattern appears in HandleCommand.

17

Thanks for the reply and info, made a few changes and seem to have it working like I want to...not sure how the code looks though...

//Sabertooth 2X25 motor controlled robot in RC microcontroller (mixed) mode
// with differential & exponential settings.
//S1 controls throttle.
//S2 controls steering.
//Date:  7Jan14

#include <Servo.h>
#define N_PINGS 4
const byte pingPins [N_PINGS] = {
  22, 23, 24, 25};   // PING pins
const int minRanges [N_PINGS] = {
  14, 14, 36, 14};
int pingRange = 0;

String command; // a string to hold the incoming command
#define SOP '<'
#define EOP '>'
boolean commandReceived = false;
int valAxisX =0;


Servo servoThrottle; // create servo object for throttle
Servo servoSteering; // create servo object steering

int currentSpeed = 0; // variable to store the servo current speed
int targetSpeed = 0; // variable to store the servo target speed


unsigned long lastSpeedUpdateTime = 0;
unsigned long lastPosUpdateTime = 0;
const unsigned long SPEED_UPDATE_INTERVAL = 10; // ms
const unsigned long POS_UPDATE_INTERVAL = 10; // ms
const int neutralSpeed = 1500;
const int neutralSteer = 1500;
int currentPos = 0;
const int targetPos = constrain(valAxisX, 260, 370);
int Step = 10;


void setup()
{
  Serial.begin(9600);
  servoThrottle.attach(53, 1000, 2000); // attach the throttle servo  
  servoSteering.attach(52, 1000, 2000); // attach the steering servo 
}



void loop()
{
  while (Serial.available()) 
  {
    // all we do is construct the incoming command to be handled in the main loop

    // get the incoming byte from the serial stream
    char incomingByte = (char)Serial.read();

    if (incomingByte == EOP)
    {
      // marks the end of a command
      commandReceived = true;
      break;
    }
    else if (incomingByte == SOP)
    {
      // marks the start of a new command
      command = "";
      commandReceived = false;

    }
    else
    {
      command += incomingByte;

    }

  } 
  pingCalc();
  handleCommand();


  if((valAxisX <= 200) && (valAxisX >= 0))
  {
    servoThrottle.write(neutralSpeed);
    leftSteer();
  }
  else if((valAxisX >= 440) && (valAxisX <= 640))
  {
    servoThrottle.write(neutralSpeed);
    rightSteer();
  }
  else if((valAxisX > 200) && (valAxisX < 440))
  {
    handleAcceleration();
    servoSteering.write(neutralSteer);  
  }
}



void setSpeed(int requiredSpeed)
{
  int range [N_PINGS];
  range [2] = ping(pingPins [2]);
  delay(5); 
  int centerThresh = range [2];

  centerThresh = map(centerThresh, 14, 54, 1500, 2000);
  requiredSpeed = constrain(centerThresh, 1500, 2000);  

  targetSpeed = requiredSpeed;
}


void handleAcceleration()
{
  setSpeed(targetSpeed);
  unsigned long now = millis();
  if(now - lastSpeedUpdateTime > SPEED_UPDATE_INTERVAL)
  {
    // time to check the motor speed again
    if(currentSpeed < targetSpeed)
    {
      // accelerating
      int incr = (targetSpeed - currentSpeed) / 10;
      if(incr == 0)
        incr = 1;
      currentSpeed += incr;

      servoThrottle.write(currentSpeed);
    }
    else if(currentSpeed > targetSpeed)
    {
      // deccelerating
      int incr = (currentSpeed - targetSpeed) / 10;
      if(incr == 0)
        incr = 1;
      currentSpeed -= incr;

      servoThrottle.write(currentSpeed);
    }
    else
    {
      // motor is already at the target speed - no action required
    }
    lastSpeedUpdateTime = now;
  }
}



void leftSteer()
{
  currentPos = valAxisX;
  int newPos = currentPos + Step;
  if((currentPos < targetPos) && (newPos > targetPos))
  {
    newPos = targetPos;
  } 
  int currServPos = map(newPos, 0, 640, 1000, 2000);   
  servoSteering.write(currServPos);

}



void rightSteer()
{
  currentPos = valAxisX;
  int newPos = currentPos - Step;
  if((currentPos > targetPos) && (newPos < targetPos))
  {
    newPos = targetPos;
  } 
  int currServPos = map(newPos, 0, 640, 1000, 2000);   
  servoSteering.write(currServPos);

}



void pingCalc()
{
  int range [N_PINGS];
  for (int i = 0; i < N_PINGS; ++i) {
    range [i] = ping(pingPins [i]);
    //delay(5);
    pingRange = range[i];
  }  

}







long ping(int pin)
{

  pinMode(pin, OUTPUT);
  digitalWrite(pin, LOW);
  delayMicroseconds(2);
  digitalWrite(pin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pin, LOW);
  pinMode(pin, INPUT);

  return (microsecondsToInches(pulseIn(pin, HIGH)));
}

long microsecondsToInches(long microseconds)
{
  return microseconds / 74 / 2;
}



void handleCommand() 
{

  if (!commandReceived) return; // no command to handle

  // variables to hold the command id and the command data

  int data[17]; // up to three integers of data

  char cmd[command.length()+1];
  command.toCharArray(cmd, command.length()+1);
  char *token = strtok(cmd, ",");


  // data  
  for (int i = 0; i < 17; i++) 
  {

    if (token) 
    {
      data[i] = atoi(token);
      token = strtok(NULL, ",");
      valAxisX = data[i];
    }

  }
  // flag that we've handled the command 
  commandReceived = false;
}
18

Argh....
Having a small bug :confused:
When I move my red object to the left of the kinect the motors turn right and moving the object to the right of the kinect motors turn left.
Basically the steering is reversed. Been trying everything I can think of (all night...)

Here's the main loop:

void loop()
{
  while (Serial.available()) 
  {
    // all we do is construct the incoming command to be handled in the main loop

    // get the incoming byte from the serial stream
    char incomingByte = (char)Serial.read();

    if (incomingByte == EOP)
    {
      // marks the end of a command
      commandReceived = true;
      break;
    }
    else if (incomingByte == SOP)
    {
      // marks the start of a new command
      command = "";
      commandReceived = false;

    }
    else
    {
      command += incomingByte;

    }

  } 
  pingCalc();

  //stepperMove();
  //stepper.run();
  handleCommand();
  if((valAxisX >= 0) && (valAxisX <= 200))
  {
    servoThrottle.write(neutralSpeed);
    leftSteer();
  }
  else if((valAxisX >= 440) && (valAxisX <= 640))
  {
    servoThrottle.write(neutralSpeed);
    rightSteer();
  }
  else if((valAxisX > 200) && (valAxisX < 440))
  {
    handleAcceleration();
    servoSteering.write(neutralSteer);  
  }
}

Here's the left & right steering functions:

void leftSteer()
{
  const int targetPos = 1500;
  const int Step = 10;
  int currentLPos = valAxisX;
  currentLPos = map(currentLPos, 0, 640, 1000, 2000);
  int newPos = currentLPos + Step;
  if((currentLPos < targetPos) && (newPos > targetPos))
  {
    newPos = targetPos;
  } 
     
  servoSteering.write(newPos);

}


void rightSteer()
{
  const int targetPos = 1500;
  const int Step = 10;
  int currentRPos = valAxisX;
  currentRPos = map(currentRPos, 0, 640, 1000, 2000);
  int newPos = currentRPos - Step;
  if((currentRPos > targetPos) && (newPos < targetPos))
  {
    newPos = targetPos;
  } 
     
  servoSteering.write(newPos);

}

And the full sketch:

//Sabertooth 2X25 motor controlled robot in RC microcontroller (mixed) mode
// with differential & exponential settings.
//S1 controls throttle.
//S2 controls steering.
//Date:  7Jan14
#include <AccelStepper.h>
#include <Servo.h>
#define N_PINGS 4
const byte pingPins [N_PINGS] = {
  22, 23, 24, 25};   // PING pins
const int minRanges [N_PINGS] = {
  14, 14, 36, 14};
int pingRange = 0;

AccelStepper stepper(2,12,13);
const int pwmA = 3;
const int pwmB = 11;
const int brakeA = 8;
const int brakeB = 9;

String command; // a string to hold the incoming command
#define SOP '<'
#define EOP '>'
boolean commandReceived = false;
int valAxisX =0;


Servo servoThrottle; // create servo object for throttle
Servo servoSteering; // create servo object steering

int currentSpeed = 0; // variable to store the servo current speed
int targetSpeed = 0; // variable to store the servo target speed


unsigned long lastSpeedUpdateTime = 0;
unsigned long lastPosUpdateTime = 0;
const unsigned long SPEED_UPDATE_INTERVAL = 10; // ms
const unsigned long POS_UPDATE_INTERVAL = 10; // ms
const int neutralSpeed = 1500;
const int neutralSteer = 1500;




void setup()
{
  Serial.begin(9600);
  servoThrottle.attach(53, 1000, 2000); // attach the throttle servo  
  servoSteering.attach(52, 1000, 2000); // attach the steering servo 

  pinMode(pwmA, OUTPUT);
  pinMode(pwmB, OUTPUT);
  pinMode(brakeA, OUTPUT);
  pinMode(brakeB, OUTPUT);

  digitalWrite(pwmA, HIGH);
  digitalWrite(pwmB, HIGH);
  digitalWrite(brakeA, LOW);
  digitalWrite(brakeB, LOW);

  stepper.setMaxSpeed(100);
  stepper.setSpeed(50);
  stepper.setAcceleration(200);
  //stepper.moveTo(40);

}


void loop()
{
  while (Serial.available()) 
  {
    // all we do is construct the incoming command to be handled in the main loop

    // get the incoming byte from the serial stream
    char incomingByte = (char)Serial.read();

    if (incomingByte == EOP)
    {
      // marks the end of a command
      commandReceived = true;
      break;
    }
    else if (incomingByte == SOP)
    {
      // marks the start of a new command
      command = "";
      commandReceived = false;

    }
    else
    {
      command += incomingByte;

    }

  } 
  pingCalc();

  //stepperMove();
  //stepper.run();
  handleCommand();
  if((valAxisX >= 0) && (valAxisX <= 200))
  {
    servoThrottle.write(neutralSpeed);
    leftSteer();
  }
  else if((valAxisX >= 440) && (valAxisX <= 640))
  {
    servoThrottle.write(neutralSpeed);
    rightSteer();
  }
  else if((valAxisX > 200) && (valAxisX < 440))
  {
    handleAcceleration();
    servoSteering.write(neutralSteer);  
  }
}


void setSpeed(int requiredSpeed)
{
  int range [N_PINGS];
  range [2] = ping(pingPins [2]);
  delay(5); 
  int centerThresh = range [2];

  centerThresh = map(centerThresh, 14, 54, 1500, 2000);
  requiredSpeed = constrain(centerThresh, 1500, 2000);  

  targetSpeed = requiredSpeed;
}


void handleAcceleration()
{
  setSpeed(targetSpeed);
  unsigned long now = millis();
  if(now - lastSpeedUpdateTime > SPEED_UPDATE_INTERVAL)
  {
    // time to check the motor speed again
    if(currentSpeed < targetSpeed)
    {
      // accelerating
      int incr = (targetSpeed - currentSpeed) / 10;
      if(incr == 0)
        incr = 1;
      currentSpeed += incr;

      servoThrottle.write(currentSpeed);
    }
    else if(currentSpeed > targetSpeed)
    {
      // deccelerating
      int incr = (currentSpeed - targetSpeed) / 10;
      if(incr == 0)
        incr = 1;
      currentSpeed -= incr;

      servoThrottle.write(currentSpeed);
    }
    else
    {
      // motor is already at the target speed - no action required
    }
    lastSpeedUpdateTime = now;
  }
}


void leftSteer()
{
  const int targetPos = 1500;
  const int Step = 10;
  int currentLPos = valAxisX;
  currentLPos = map(currentLPos, 0, 640, 1000, 2000);
  int newPos = currentLPos + Step;
  if((currentLPos < targetPos) && (newPos > targetPos))
  {
    newPos = targetPos;
  } 
     
  servoSteering.write(newPos);

}


void rightSteer()
{
  const int targetPos = 1500;
  const int Step = 10;
  int currentRPos = valAxisX;
  currentRPos = map(currentRPos, 0, 640, 1000, 2000);
  int newPos = currentRPos - Step;
  if((currentRPos > targetPos) && (newPos < targetPos))
  {
    newPos = targetPos;
  } 
     
  servoSteering.write(newPos);

}


void pingCalc()
{
  int range [N_PINGS];
  for (int i = 0; i < N_PINGS; ++i) {
    range [i] = ping(pingPins [i]);
    //delay(5);
    pingRange = range[i];
  }  
}


long ping(int pin)
{

  pinMode(pin, OUTPUT);
  digitalWrite(pin, LOW);
  delayMicroseconds(2);
  digitalWrite(pin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pin, LOW);
  pinMode(pin, INPUT);

  return (microsecondsToInches(pulseIn(pin, HIGH)));
}

long microsecondsToInches(long microseconds)
{
  return microseconds / 74 / 2;
}


void handleCommand() 
{

  if (!commandReceived) return; // no command to handle

  // variables to hold the command id and the command data

  int data[17]; // up to three integers of data

  char cmd[command.length()+1];
  command.toCharArray(cmd, command.length()+1);
  char *token = strtok(cmd, ",");


  // data  
  for (int i = 0; i < 17; i++) 
  {

    if (token) 
    {
      data[i] = atoi(token);
      token = strtok(NULL, ",");
      valAxisX = data[i];
    }

  }
  // flag that we've handled the command 
  commandReceived = false;
}


void stepperWaiting()
{
  while (stepper.distanceToGo() != 0)
  {
    stepper.run();
  }
  Serial.println(stepper.currentPosition());
  delay(2000);
}


void stepperMove()
{
  stepper.moveTo(40);
  stepperWaiting();  

  stepper.moveTo(0);
  stepperWaiting();

  stepper.move(-40);
  stepperWaiting();

  stepper.moveTo(0);
  stepperWaiting();
}
19

Captain Obvious says, rename leftsteer as rightsteer and vice versa :stuck_out_tongue:

20

wildbill:
Captain Obvious says, rename leftsteer as rightsteer and vice versa :stuck_out_tongue:

Yeah, I've tried that....no luck, no change (includes the functions and the function calls in main loop :confused:
thanks WildBill.
..must be something small..