Need help with code, using rosserial in motor control

Projects Programming
1

Actually I am using this code from this tutorial which uses ROS Click here
DETAILS:
ROS (JETSON NANO)
ARDUINO MEGA 11TH PIN CONNECTED TO S1 OF SABERTOOTH 2 X 32 MOTOR DRIVER, ORAGNE ENCODER1 CONNECTED TO 2,3 , ORANGE ENCODER 2 CONNECTED TO 18,19, WORKS FINE, But MY MOTOR IS NOT RUNNING WHILE I USE

ROSCORE

rosrun rosserial_arduino serial_node.py _port:=/dev/ttyUSB0 _baud:=115200

rosrun rqt_robot_steering rqt_robot_steering

and for using my controller , i have edited the arduino code like this,



#include <ros.h>
#include <std_msgs/Int16.h>
#include <geometry_msgs/Twist.h>
#include <SoftwareSerial.h>
#include <SabertoothSimplified.h>
#include <Sabertooth.h>

SoftwareSerial SWSerial(NOT_A_PIN, 11); // RX on no pin (unused), TX on pin 11 (to S1).
Sabertooth ST(128, SWSerial); // Address 128, and use SWSerial as the serial port.


// Handles startup and shutdown of ROS
ros::NodeHandle nh;

////////////////// Tick Data Publishing Variables and Constants ///////////////

int encoderPin1 = 2;
int encoderPin2 = 3;
int encoderPin3 = 18;
int encoderPin4 = 19;



// True = Forward; False = Reverse
boolean Direction_left = true;
boolean Direction_right = true;

// Minumum and maximum values for 16-bit integers
// Range of 65,535
const int encoder_minimum = -32768;
const int encoder_maximum = 32767;

volatile int lastEncoded = 0;
//volatile long encoderValue = 0;
volatile int lastEncoded1 = 0;

long lastencoderValue = 0;
long lastencoderValue1 = 0;

std_msgs::Int16 encoderValue;
ros::Publisher rightPub("right_ticks", &encoderValue);

std_msgs::Int16 encoderValue1;
ros::Publisher leftPub("left_ticks", &encoderValue1);

// Time interval for measurements in milliseconds
const int interval = 30;
long previousMillis = 0;
long currentMillis = 0;

////////////////// Motor Controller Variables and Constants ///////////////////

// Motor A connections
const int enA = 9;
const int in1 = 5;
const int in2 = 6;

// Motor B connections
const int enB = 10;
const int in3 = 7;
const int in4 = 8;

// How much the PWM value can change each cycle
const int PWM_INCREMENT = 1;

// Number of ticks per wheel revolution. We won't use this in this code.
const int TICKS_PER_REVOLUTION = 610;

// Wheel radius in meters
const double WHEEL_RADIUS = 0.127;

// Distance from center of the left tire to the center of the right tire in m
const double WHEEL_BASE = 0.715;

// Number of ticks a wheel makes moving a linear distance of 1 meter
// This value was measured manually.
const double TICKS_PER_METER = 762; // Originally 2880

// Proportional constant, which was measured by measuring the
// PWM-Linear Velocity relationship for the robot.
const int K_P = 278;

// Y-intercept for the PWM-Linear Velocity relationship for the robot
const int b = 52;

// Correction multiplier for drift. Chosen through experimentation.
const int DRIFT_MULTIPLIER = 120;

// Turning PWM output (0 = min, 255 = max for PWM values)
const int PWM_TURN = 80;

// Set maximum and minimum limits for the PWM values
const int PWM_MIN = 80; // about 0.1 m/s
const int PWM_MAX = 100; // about 0.172 m/s

// Set linear velocity and PWM variable values for each wheel
double velLeftWheel = 0;
double velRightWheel = 0;
double pwmLeftReq = 0;
double pwmRightReq = 0;

// Record the time that the last velocity command was received
double lastCmdVelReceived = 0;

void updateEncoder() {
  int MSB = digitalRead(encoderPin1); //MSB = most significant bit
  int LSB = digitalRead(encoderPin2); //LSB = least significant bit

  int encoded = (MSB << 1) | LSB; //converting the 2 pin value to single number
  int sum  = (lastEncoded << 2) | encoded; //adding it to the previous encoded value

  if (sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011) encoderValue.data ++;
  if (sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) encoderValue.data --;

  lastEncoded = encoded; //store this value for next time
}

void updateEncoder1() {
  int MSB = digitalRead(encoderPin3); //MSB = most significant bit
  int LSB = digitalRead(encoderPin4); //LSB = least significant bit

  int encoded1 = (MSB << 1) | LSB; //converting the 2 pin value to single number
  int sum1  = (lastEncoded1 << 2) | encoded1; //adding it to the previous encoded value

  if (sum1 == 0b1101 || sum1 == 0b0100 || sum1 == 0b0010 || sum1 == 0b1011) encoderValue1.data ++;
  if (sum1 == 0b1110 || sum1 == 0b0111 || sum1 == 0b0001 || sum1 == 0b1000) encoderValue1.data --;

  lastEncoded1 = encoded1; //store this value for next time
}

/////////////////////// Motor Controller Functions ////////////////////////////

// Calculate the left wheel linear velocity in m/s every time a
// tick count message is rpublished on the /left_ticks topic.
void calc_vel_left_wheel() {

  // Previous timestamp
  static double prevTime = 0;

  // Variable gets created and initialized the first time a function is called.
  static int prevLeftCount = 0;

  // Manage rollover and rollunder when we get outside the 16-bit integer range
  int numOfTicks = (65535 + encoderValue1.data - prevLeftCount) % 65535;

  // If we have had a big jump, it means the tick count has rolled over.
  if (numOfTicks > 10000) {
    numOfTicks = 0 - (65535 - numOfTicks);
  }

  // Calculate wheel velocity in meters per second
  velLeftWheel = numOfTicks / TICKS_PER_METER / ((millis() / 1000) - prevTime);

  // Keep track of the previous tick count
  prevLeftCount = encoderValue1.data;

  // Update the timestamp
  prevTime = (millis() / 1000);

}

// Calculate the right wheel linear velocity in m/s every time a
// tick count message is published on the /right_ticks topic.
void calc_vel_right_wheel() {

  // Previous timestamp
  static double prevTime = 0;

  // Variable gets created and initialized the first time a function is called.
  static int prevRightCount = 0;

  // Manage rollover and rollunder when we get outside the 16-bit integer range
  int numOfTicks = (65535 + encoderValue.data - prevRightCount) % 65535;

  if (numOfTicks > 10000) {
    numOfTicks = 0 - (65535 - numOfTicks);
  }

  // Calculate wheel velocity in meters per second
  velRightWheel = numOfTicks / TICKS_PER_METER / ((millis() / 1000) - prevTime);

  prevRightCount = encoderValue.data;

  prevTime = (millis() / 1000);

}

// Take the velocity command as input and calculate the PWM values.
void calc_pwm_values(const geometry_msgs::Twist& cmdVel) {

  // Record timestamp of last velocity command received
  lastCmdVelReceived = (millis() / 1000);

  // Calculate the PWM value given the desired velocity
  pwmLeftReq = K_P * cmdVel.linear.x + b;
  pwmRightReq = K_P * cmdVel.linear.x + b;

  // Check if we need to turn
  if (cmdVel.angular.z != 0.0) {

    // Turn left
    if (cmdVel.angular.z > 0.0) {
      pwmLeftReq = -PWM_TURN;
      pwmRightReq = PWM_TURN;
    }
    // Turn right
    else {
      pwmLeftReq = PWM_TURN;
      pwmRightReq = -PWM_TURN;
    }
  }
  // Go straight
  else {

    // Remove any differences in wheel velocities
    // to make sure the robot goes straight
    static double prevDiff = 0;
    static double prevPrevDiff = 0;
    double currDifference = velLeftWheel - velRightWheel;
    double avgDifference = (prevDiff + prevPrevDiff + currDifference) / 3;
    prevPrevDiff = prevDiff;
    prevDiff = currDifference;

    // Correct PWM values of both wheels to make the vehicle go straight
    pwmLeftReq -= (int)(avgDifference * DRIFT_MULTIPLIER);
    pwmRightReq += (int)(avgDifference * DRIFT_MULTIPLIER);
  }

  // Handle low PWM values
  if (abs(pwmLeftReq) < PWM_MIN) {
    pwmLeftReq = 0;
  }
  if (abs(pwmRightReq) < PWM_MIN) {
    pwmRightReq = 0;
  }
}

void set_pwm_values() {

  // These variables will hold our desired PWM values
  static int pwmLeftOut = 0;
  static int pwmRightOut = 0;

  // If the required PWM is of opposite sign as the output PWM, we want to
  // stop the car before switching direction
  static bool stopped = false;
  if ((pwmLeftReq * velLeftWheel < 0 && pwmLeftOut != 0) ||
      (pwmRightReq * velRightWheel < 0 && pwmRightOut != 0)) {
    pwmLeftReq = 0;
    pwmRightReq = 0;
  }

  // Increase the required PWM if the robot is not moving
  if (pwmLeftReq != 0 && velLeftWheel == 0) {
    pwmLeftReq *= 50;
  }
  if (pwmRightReq != 0 && velRightWheel == 0) {
    pwmRightReq *= 50;
  }

  // Calculate the output PWM value by making slow changes to the current value
  if (abs(pwmLeftReq) > pwmLeftOut) {
    pwmLeftOut += PWM_INCREMENT;
  }
  else if (abs(pwmLeftReq) < pwmLeftOut) {
    pwmLeftOut -= PWM_INCREMENT;
  }
  else {}

  if (abs(pwmRightReq) > pwmRightOut) {
    pwmRightOut += PWM_INCREMENT;
  }
  else if (abs(pwmRightReq) < pwmRightOut) {
    pwmRightOut -= PWM_INCREMENT;
  }
  else {}

  // Conditional operator to limit PWM output at the maximum
  pwmLeftOut = (pwmLeftOut > PWM_MAX) ? PWM_MAX : pwmLeftOut;
  pwmRightOut = (pwmRightOut > PWM_MAX) ? PWM_MAX : pwmRightOut;

  // PWM output cannot be less than 0
  pwmLeftOut = (pwmLeftOut < 0) ? 0 : pwmLeftOut;
  pwmRightOut = (pwmRightOut < 0) ? 0 : pwmRightOut;

  /* Set the PWM value on the pins
  analogWrite(enA, pwmLeftOut);
  analogWrite(enB, pwmRightOut);*/
  // Set the direction of the motors
  if (pwmLeftReq > 0) { // Left wheel forward
    ST.motor(1, pwmLeftOut);//left_motor_forward
    
    
  }
  else if (pwmLeftReq < 0) { // Left wheel reverse
    ST.motor(1, -pwmLeftOut);//left_motor_forward
  }
  else if (pwmLeftReq == 0 && pwmLeftOut == 0 ) { // Left wheel stop
    ST.motor(1, 0);
  }
  else { // Left wheel stop
    ST.motor(1, 0);
  }

  if (pwmRightReq > 0) { // Right wheel forward
    ST.motor(2, -pwmRightOut);//right_motor_forward
  }
  else if (pwmRightReq < 0) { // Right wheel reverse
    ST.motor(2, pwmRightOut);//right_motor_reverse
  }
  else if (pwmRightReq == 0 && pwmRightOut == 0) { // Right wheel stop
    ST.motor(2, 0);
  }
  else { // Right wheel stop
    ST.motor(2, 0);
  }
}

// Set up ROS subscriber to the velocity command
ros::Subscriber<geometry_msgs::Twist> subCmdVel("cmd_vel", &calc_pwm_values );

void setup() {

  // Set pin states of the encoder
  pinMode(encoderPin1, INPUT);
  pinMode(encoderPin2, INPUT);
  pinMode(encoderPin3, INPUT);
  pinMode(encoderPin4, INPUT);

  digitalWrite(encoderPin1, HIGH); //turn pullup resistor on
  digitalWrite(encoderPin2, HIGH); //turn pullup resistor on
  digitalWrite(encoderPin3, HIGH); //turn pullup resistor on
  digitalWrite(encoderPin4, HIGH); //turn pullup resistor on

  //call updateEncoder() when any high/low changed seen
  //on interrupt 0 (pin 2), or interrupt 1 (pin 3)
  attachInterrupt(digitalPinToInterrupt(encoderPin1), updateEncoder, CHANGE); //attachInterrupt(digitalPinToInterrupt(pin), ISR, mode)
  attachInterrupt(digitalPinToInterrupt(encoderPin2), updateEncoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(encoderPin3), updateEncoder1, CHANGE); //attachInterrupt(digitalPinToInterrupt(pin), ISR, mode)
  attachInterrupt(digitalPinToInterrupt(encoderPin4), updateEncoder1, CHANGE);

  // Turn off motors - Initial state
  digitalWrite(in1, LOW);
  digitalWrite(in2, LOW);
  digitalWrite(in3, LOW);
  digitalWrite(in4, LOW);

  // Set the motor speed
  analogWrite(enA, 0);
  analogWrite(enB, 0);

  // ROS Setup
  nh.getHardware()->setBaud(115200);
  nh.initNode();
  nh.advertise(rightPub);
  nh.advertise(leftPub);
  nh.subscribe(subCmdVel);
}

void loop() {

  nh.spinOnce();

  // Record the time
  currentMillis = millis();

  // If the time interval has passed, publish the number of ticks,
  // and calculate the velocities.
  if (currentMillis - previousMillis > interval) {

    previousMillis = currentMillis;

    // Publish tick counts to topics
    rightPub.publish(&encoderValue);
    leftPub.publish(&encoderValue1);

    // Calculate the velocity of the right and left wheels
    calc_vel_right_wheel();
    calc_vel_left_wheel();
    //set_pwm_values();

  }

  // Stop the car if there are no cmd_vel messages
  if ((millis() / 1000) - lastCmdVelReceived > 1) {
    pwmLeftReq = 0;
    pwmRightReq = 0;
  }

  set_pwm_values();
}

TOPICS LIKE TICKS, CMD_VEL EVERYTHING IS PUBLISHED, BUT MY MOTORS CONTROLLED USING SABERTOOTH 2 X32 MOTOR DRIVER IS NOT RUNNING

2

why not work with simpler code to just get a motor to work

i don't see where the motor enA or enB pins are set to anything other than zero.

why are these commented out?

3

tried with this code, and the motor works fine, its just this particular code, i am not able to make it run, i am using sabertooth motor driver, so can't go ahead with this piece of code, as sabertooth motor driver itself a computer, they communicate using serial communication, the working code i will post below



#include <math.h>

#include <SoftwareSerial.h>
#include <SabertoothSimplified.h>
#include <Sabertooth.h>



SoftwareSerial SWSerial(NOT_A_PIN, 11); // RX on no pin (unused), TX on pin 11 (to S1).
Sabertooth ST(128, SWSerial); // Address 128, and use SWSerial as the serial port.
SoftwareSerial portROS(0, 1);



const long CONTROL_TIMEOUT = 1000; //ms to wait  before killing motors

void cmdVelCallback(const geometry_msgs::Twist&);

//ros::NodeHandle nh;

int encoderPin1 = 2;
int encoderPin2 = 3;
int encoderPin3 = 18;
int encoderPin4 = 19;

// Minumum and maximum values for 16-bit integers
const int encoder_minimum = -32768;
const int encoder_maximum = 32767;

boolean Direction_left = true;
boolean Direction_right = true;

volatile int lastEncoded = 0;
//volatile long encoderValue = 0;
volatile int lastEncoded1 = 0;
//volatile long encoderValue1 = 0;

long lastencoderValue = 0;
long lastencoderValue1 = 0;

int lastMSB = 0;
int lastLSB = 0;
int lastMSB1 = 0;
int lastLSB1 = 0;

std_msgs::Int16 encoderValue;
ros::Publisher rightPub("right_ticks", &encoderValue);

std_msgs::Int16 encoderValue1;
ros::Publisher leftPub("left_ticks", &encoderValue1);

// 100ms interval for measurements
const int interval = 100;
long previousMillis = 0;
long currentMillis = 0;

ros::NodeHandle handle;
ros::Subscriber<geometry_msgs::Twist> subscriber("cmd_vel", &cmdVelCallback);
geometry_msgs::Twist msg;
ros::Publisher chatter_pub("cmd_vel", &msg);

unsigned long lastData = 0;
int cnt = 0;

void cmdVelCallback(const geometry_msgs::Twist &twist)//motor control serial
{
  if (cnt % 2 == 0)
    digitalWrite(2, HIGH);
  else
    digitalWrite(2, LOW);

  cnt = cnt + 1;


  // this function is called as soon as there is any cmd_vel command in the ros network

  lastData = millis();

  // this variable is given forth/back velocity in range of [-0.5 +0.5]
  const float linear = twist.linear.x;
  // this variable is given left/right velocity in range of [-1 +1]
  const float spin = twist.angular.z;



  // give movement to forth/back direction
  front(int(66 * linear));


  // if given command is right means spin is negative
  if (spin < 0)
  {
    turn1(int(100 * spin));
  }
  else if (spin > 0)
  {
    turn1(int(100 * spin));
  }

}

void setup() {
  setupfn();

}

void loop()
{
  currentMillis = millis();
  //delay(1000);
  handle.spinOnce();
  //nh.spinOnce();

  if (millis() - lastData >= CONTROL_TIMEOUT)// for motors
  {
    lastData = millis();
    msg.linear.x = 0;
    msg.angular.z = 0;
    chatter_pub.publish(&msg);

  }

  if (currentMillis - previousMillis > interval) {//for encoders

    previousMillis = currentMillis;

    rightPub.publish(&encoderValue);
    leftPub.publish(&encoderValue1);

  }

}

void setupfn()
{
  SWSerial.begin(9600);
  ST.autobaud();
  portROS.begin(57600);
  //Serial.begin(9600);
  pinMode(encoderPin1, INPUT);
  pinMode(encoderPin2, INPUT);
  pinMode(encoderPin3, INPUT);
  pinMode(encoderPin4, INPUT);

  digitalWrite(encoderPin1, HIGH); //turn pullup resistor on
  digitalWrite(encoderPin2, HIGH); //turn pullup resistor on
  digitalWrite(encoderPin3, HIGH); //turn pullup resistor on
  digitalWrite(encoderPin4, HIGH); //turn pullup resistor on

  //call updateEncoder() when any high/low changed seen
  //on interrupt 0 (pin 2), or interrupt 1 (pin 3)
  attachInterrupt(digitalPinToInterrupt(encoderPin1), updateEncoder, CHANGE); //attachInterrupt(digitalPinToInterrupt(pin), ISR, mode)
  attachInterrupt(digitalPinToInterrupt(encoderPin2), updateEncoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(encoderPin3), updateEncoder1, CHANGE); //attachInterrupt(digitalPinToInterrupt(pin), ISR, mode)
  attachInterrupt(digitalPinToInterrupt(encoderPin4), updateEncoder1, CHANGE);


  handle.initNode();
  handle.subscribe(subscriber);
  handle.advertise(chatter_pub);
  handle.advertise(rightPub);
  handle.advertise(leftPub);
}


void updateEncoder() {
  int MSB = digitalRead(encoderPin1); //MSB = most significant bit
  int LSB = digitalRead(encoderPin2); //LSB = least significant bit

  int encoded = (MSB << 1) | LSB; //converting the 2 pin value to single number
  int sum  = (lastEncoded << 2) | encoded; //adding it to the previous encoded value

  if (sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011) encoderValue.data ++;
  if (sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) encoderValue.data --;

  lastEncoded = encoded; //store this value for next time
}

void updateEncoder1() {
  int MSB = digitalRead(encoderPin3); //MSB = most significant bit
  int LSB = digitalRead(encoderPin4); //LSB = least significant bit

  int encoded1 = (MSB << 1) | LSB; //converting the 2 pin value to single number
  int sum1  = (lastEncoded1 << 2) | encoded1; //adding it to the previous encoded value

  if (sum1 == 0b1101 || sum1 == 0b0100 || sum1 == 0b0010 || sum1 == 0b1011) encoderValue1.data ++;
  if (sum1 == 0b1110 || sum1 == 0b0111 || sum1 == 0b0001 || sum1 == 0b1000) encoderValue1.data --;

  lastEncoded1 = encoded1; //store this value for next time
}



int front(int propel) {
  ST.motor(1, -propel);
  ST.motor(2, propel);
}

int turn1(signed int rueck) {
  ST.motor(1, -rueck);
  ST.motor(2, -rueck);
}

void standgas() {
  ST.motor(1, 0);
  ST.motor(2, 0);

}
4

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.