Let's Make Robots!

Help with Programming (arduino)

Hi LMR,

I've been working on my new robot, which has a lynxmotion A4WD1 base with an arduino as its microcontroller. I must confess that I am not the best programmer and I am still learning. I have found the follow code online and I was wondering if anyone could help me adjust it to be able to turn left and right. As of right now the program only tells the motors to go foward or reverse at full speed or half speed. The code:

 

 #include <SoftwareSerial.h>

 

/*****************************************************

 * DIP Switches as per the wized:

 * - NiMh Battery

 * - TTL RS232

 * - Simplified Serial Mode

 * - Just one Sabertooth conected

 * - 9600 baudrate

 *

 * Pin 1 - ON

 * Pin 2 - OFF

 * Pin 3 - ON

 * Pin 4 - OFF

 * Pin 5 - ON

 * Pin 6 - ON

 ****************************************************/

 

// Labels for use with the Sabertooth 2x5 motor controller

 

// Digital pin 13 is the serial transmit pin to the 

// Sabertooth 2x5

#define SABER_TX_PIN               1

 

// NOT USED (but still init'd)

// Digital pin 12 is the serial receive pin from the 

// Sabertooth 2x5

#define SABER_RX_PIN               2

 

// Set to 9600 through Sabertooth dip switches

#define SABER_BAUDRATE             9600

 

// Simplified serial Limits for each motor

#define SABER_MOTOR1_FULL_FORWARD  127

#define SABER_MOTOR1_FULL_REVERSE  1

 

#define SABER_MOTOR2_FULL_FORWARD  255

#define SABER_MOTOR2_FULL_REVERSE  128

 

// Motor level to send when issuing the full stop command

#define SABER_ALL_STOP             0

 

 

SoftwareSerial SaberSerial = SoftwareSerial( SABER_RX_PIN,

                                             SABER_TX_PIN );

 

void initSabertooth( void )

{

  // Init software UART to communicate 

  // with the Sabertooth 2x5

  pinMode( SABER_TX_PIN, OUTPUT );

 

  SaberSerial.begin( SABER_BAUDRATE );     

 

  // 2 second time delay for the Sabertooth to init

  delay( 2000 );

 

  // Send full stop command

  setEngineSpeed( SABER_ALL_STOP );

}

 

/*****************************************************

 * setEngineSpeed

 *

 * Inputs - cSpeed_Motor1 - Input a percentage of full 

 *                          speed, from -100 to +100

 *

 *****************************************************/

void setEngineSpeed( signed char cNewMotorSpeed )

{

  unsigned char cSpeedVal_Motor1 = 0;

 

  unsigned char cSpeedVal_Motor2 = 0;

 

  // Check for full stop command

  if( cNewMotorSpeed == 0 )

  {

    // Send full stop command for both motors

    SaberSerial.print( 0, BYTE );

 

    return;

  }  

 

  // Calculate the speed value for motor 1

  if( cNewMotorSpeed >= 100 )

  {

    cSpeedVal_Motor1 = SABER_MOTOR1_FULL_FORWARD;

 

    cSpeedVal_Motor2 = SABER_MOTOR2_FULL_FORWARD;

  }

  else if( cNewMotorSpeed <= -100 )

  {

    cSpeedVal_Motor1 = SABER_MOTOR1_FULL_REVERSE;

 

    cSpeedVal_Motor2 = SABER_MOTOR2_FULL_REVERSE;

  }

  else

  {

    // Calc motor 1 speed (Final value ranges from 1 to 127)

    cSpeedVal_Motor1 = map( cNewMotorSpeed, 

                           -100, 

                            100, 

                            SABER_MOTOR1_FULL_REVERSE,

                            SABER_MOTOR1_FULL_FORWARD );

 

    // Calc motor 2 speed (Final value ranges from 128 to 255)

    cSpeedVal_Motor2 = map( cNewMotorSpeed, 

                           -100, 

                            100, 

                            SABER_MOTOR2_FULL_REVERSE, 

                            SABER_MOTOR2_FULL_FORWARD );

  }

 

  // Fire the values off to the Sabertooth motor controller

  SaberSerial.print( cSpeedVal_Motor1, BYTE );

 

  SaberSerial.print( cSpeedVal_Motor2, BYTE );

}

 

void setup( )

{

  initSabertooth( );

}

 

void loop ( )

{

 

  // Full stop

  //setEngineSpeed( 0 ); 

 

  // Half reverse

//   setEngineSpeed( -50 );

 

 

 // Full reverse

 //setEngineSpeed( -100 );

 

  // Half forward

//  setEngineSpeed( 50 );

 // Full forward

  setEngineSpeed( 100 );

}

 

Comment viewing options

Select your preferred way to display the comments and click "Save settings" to activate your changes.

on softserial, I believe the code will require extra code. The setEngineSpeed function only accepts a single input and then acts on both motors with that single input. You will need to rewrite the code so it calls a setLeft() and a setRight() (True function names are up to you. These are just examples.) Your new functions will be almost copies of the setEngineSpeed(), except, they will each only affect a single motor. So, instead of SaberSerial.print( cSpeedVal_Motor1, BYTE ); AND SaberSerial.print( cSpeedVal_Motor2, BYTE ); you will only call out one motor in each function; SaberSerial.print( cSpeedVal_Motor1, BYTE ); in one, and, SaberSerial.print( cSpeedVal_Motor2, BYTE ); in the other. The BYTE keyword is necessary as the motor driver is expecting BYTEs and not some other encoded character.

PS - Not sure where the 'killing' in the title came from. Maybe I was too tired?

Make a copy of the setEngineSpeed function and rename it to setLeftSpeed(). After you have done that go through the function and comment out all of the lines that call out cSpeedVal_Motor2. I believe that if you were to rename a second copy of the setEngineSpeed to setRightSpeed() and then comment out all of the cSpeedVal_Motor1 instead, you should be able to control both motors independently, as long as you call them independently from your loop() (ie setLeftSpeed(50); setRightSpeed(100);)

Just in case my words don't cut it I will include the code as I see it.

/*****************************************************

* setLeftSpeed

*

* Inputs - cSpeed_Motor1 - Input a percentage of full

* speed, from -100 to +100

*

*****************************************************/

void setLeftSpeed( signed char cNewMotorSpeed ) {

unsigned char cSpeedVal_Motor1 = 0;

//unsigned char cSpeedVal_Motor2 = 0;

// Check for full stop command

if( cNewMotorSpeed == 0 ) { // Send full stop command for both motors

SaberSerial.print( 0, BYTE );

return;

}

// Calculate the speed value for motor 1

if( cNewMotorSpeed >= 100 ) {

cSpeedVal_Motor1 = SABER_MOTOR1_FULL_FORWARD;

//cSpeedVal_Motor2 = SABER_MOTOR2_FULL_FORWARD;

}

else if( cNewMotorSpeed <= -100 ) {

cSpeedVal_Motor1 = SABER_MOTOR1_FULL_REVERSE;

//cSpeedVal_Motor2 = SABER_MOTOR2_FULL_REVERSE;

}

else { // Calc motor 1 speed (Final value ranges from 1 to 127)

cSpeedVal_Motor1 = map( cNewMotorSpeed, -100, 100, SABER_MOTOR1_FULL_REVERSE,

SABER_MOTOR1_FULL_FORWARD );

// Calc motor 2 speed (Final value ranges from 128 to 255)

//cSpeedVal_Motor2 = map( cNewMotorSpeed, -100, 100,

SABER_MOTOR2_FULL_REVERSE,

SABER_MOTOR2_FULL_FORWARD );

}

// Fire the values off to the Sabertooth motor controller

SaberSerial.print( cSpeedVal_Motor1, BYTE );

//SaberSerial.print( cSpeedVal_Motor2, BYTE );

}

Thank you very much for the code section it was hard to follow but this is great. So, I add the code above to my original code, what do I need to add now in the void loop section to make the robot turn? again thank you very much for the help. 

your robot will only rotate around the right wheel. To turn left you would add setLeftSpeed(-n) where n is a number between 1 and 100. It will cause the robot to reverse the left wheel and as long as the right wheel is not moving the robot will rotate to the left. I would suggest you actually add a second function called setRightSpeed and copy the setLeftSpeed code, but, instead of commenting out the cSpeedVal_Motor2 lines comment out the cSpeedVal_Motor1 lines. Once you have the pair of functions you should be able to get the motors to rotate in any percentage of speed from full forward to full reverse independently.

Birdmun thank you so so so much! everything is working now! I cant thank you enough, I wont have been able to figure this out on myself, expect a robot post from me coming soon!

in this chunk of code is were i think the problem maybe , you send 1 value to be mapped and end up with 1 direction ,you may need to wright a second 1 like this for left and right speed  

cSpeedVal_Motor1 = map( cNewMotorSpeed, 

                           -100, 

                            100, 

                            SABER_MOTOR1_FULL_REVERSE,

                            SABER_MOTOR1_FULL_FORWARD );

 

    // Calc motor 2 speed (Final value ranges from 128 to 255)

    cSpeedVal_Motor2 = map( cNewMotorSpeed, 

                           -100, 

                            100, 

                            SABER_MOTOR2_FULL_REVERSE, 

                            SABER_MOTOR2_FULL_FORWARD );

  }

Thanks everyone for the help, 

 

I'm trying to do make an aditional map like you said but it still not working ):

This seems to be incorrect test code.

There are a total of 4 bytes that must be sent.

Address
Speed
Direction
CheckSum

I can't believe there is not a library to do all this for you.

it all so has a Simplified serial mode i think that's what he is using not the Packetized serial

Because Sabertooth controls two motors with one 8 byte character, when operating in Simplified
Serial mode, each motor has 7 bits of resolution. Sending a character between 1 and 127 will
control motor 1. 1 is full reverse, 64 is stop and 127 is full forward. Sending a character between
128 and 255 will control motor 2. 128 is full reverse, 192 is stop and 255 is full forward.
Character 0 (hex 0x00) is a special case. Sending this character will shut down both motors.
:)

Typically, to get a robot to turn, one motor is slowed, stopped or reversed.

Am I missing something here?