Go Down

Topic: Kangaroo x2 Motion Controller Issues (Read 1 time) previous topic - next topic

Pack34

I'm working with some Kangaroos to command a mecanum wheeled robot. Using just the Sabertooth motor controllers works fine, but I'm trying to add some speed control into the mix and I'm having difficulty.  Contacting their support is like pulling teeth, so I figure I would ask here.

Now, the code below "works" but the issue that I'm having is that the speed commands seem to take up to 30 seconds to complete. All I'm trying to do is set a speed that I want that motor to turn at and then update it with a new speed when the next command is set. Once I have the motion controller worked out, the full firmware will send down a packet every 100ms or so.

Motor Controller: Sabertooth 2x25
http://www.dimensionengineering.com/products/sabertooth2x25

Motion Controller: Kangaroo x2
http://www.dimensionengineering.com/products/kangaroo


Code: [Select]

#include <SoftwareSerial.h>
#include <Kangaroo.h>

#define TX_PIN 10
#define RX_PIN 11

unsigned long currentTime = 0;
unsigned long previousTime = 0;

SoftwareSerial  SerialPort(RX_PIN, TX_PIN);

KangarooSerial  K_FRONT(SerialPort);
KangarooChannel K1_F(K_FRONT, '1', 128);
KangarooChannel K2_F(K_FRONT, '2', 128);

KangarooSerial  K_REAR(SerialPort);
KangarooChannel K1_R(K_REAR, '1', 129);
KangarooChannel K2_R(K_REAR, '2', 129);

KangarooStatus K_STATUS;

void setup()
{
  Serial.begin(115200);
 
  SerialPort.begin(9600);
  SerialPort.listen();
 
  delay(2000);
 
  K1_F.start();
  K1_F.home();//.wait();
 
  K2_F.start(); 
  K2_F.home();//.wait();

  K1_R.start();
  K1_R.home();//.wait();
 
  K2_R.start(); 
  K2_R.home();//.wait();

}

void loop()
{
  int i = 0;
  int j = 0;
  unsigned char kERROR;
 
  previousTime = millis();

  for(i = 0; i < 1250; i = i + 100)
  {
    j = -1 * i;
    Serial.print("[1]");
    K1_F.s(i);
        printTime();
    K2_F.s(i);
        printTime();
    K1_R.s(j);
        printTime();
    K2_R.s(j);
        printTime();
    delay(100);
    Serial.println();
  }
 
  for(i = 1250; i > -1250; i = i - 100)
  {
    j = -1 * i;
   Serial.print("[2]");   
    K1_F.s(i);
        printTime();
    K2_F.s(i);
        printTime();
    K1_R.s(j);
        printTime();
    K2_R.s(j);
        printTime();
    delay(100);
    Serial.println();
  }
 
  for(i = -1250; i <= 0; i = i + 100)
  {
    j = -1 * i;
    Serial.print("[3]");
    K1_F.s(i);
        printTime();
    K2_F.s(i);
        printTime();
    K1_R.s(j);
        printTime();
    K2_R.s(j);   
    delay(100);
    Serial.println();
  }
}

void printTime()
{
    currentTime = millis();
    Serial.print("[");Serial.print(currentTime-previousTime);Serial.print("]");
    previousTime=millis();
}


chrissunny94

I have successfully used sabertooth 2x32 vai USB cable . The code is listed below ,sabertooth dip switch configuration is (On off on on on on 1-6).

USB cable connected directly (No ttl cable or Arduino in between).

Using the below script one can control a differential drive bot which uses sabertooth 2x32.

I have been hunting for information regarding the use of kangaroo x2 along with sabertooth , i promise to make a good tutorial for the same as i am finding not much material is available related to this .

Kangaroo can handle PID based position/speed control . Kangaroo can also supposedly give back the encoder data (yet to figure out all of that ).

Please see if you could give me some insights on how to use ROS with the above configuration.
 
 
Code: [Select]
   
    #!/usr/bin/env python
    #import arduinoserial
    from pysabertooth import Sabertooth
    from std_msgs.msg import String
    from geometry_msgs.msg import Twist, Pose
    import rospy , time
    import serial.tools.list_ports as port
    #import pylcdlib
    print "\nInit sabertooth....\n"

    print "\nDetecting sabertooth....\n"
    portlist = list(port.comports())
    print portlist
    address = ''
    for p in portlist:
      print p
      if 'Sabertooth' in str(p):
          address = str(p).split(" ")
    print "\nAddress found @"
    print address[0]
    speed1 = 0
    speed2 = 0

    saber = Sabertooth(address[0], baudrate=9600, address=128, timeout=0.1)


    def translate(value, leftMin, leftMax, rightMin, rightMax):
       # Figure out how 'wide' each range is
       leftSpan = leftMax - leftMin
       rightSpan = rightMax - rightMin

       # Convert the left range into a 0-1 range (float)
       valueScaled = float(value - leftMin) / float(leftSpan)

       # Convert the 0-1 range into a value in the right range.
       return rightMin + (valueScaled * rightSpan)

    def callback(data):
        #print data
        saber.text('m1:startup')
        saber.text('1,p100')
        saber.text('md:1000\r\n')
   
        saber.text('m2:startup')
        saber.text('r1:1940\r\n')
        saber.text('r2:1940\r\n')
        speed = translate(data.linear.x,-1,1,-100,100)
        #speed = translate(data.linear.x,-1,1,-2047,2047)
        SPEED = 'md: {}\r\n'.format(speed)
        angle = translate(-data.angular.z,-1,1,100,-100)
        #angle = str(translate(-data.angular.z,-1,1,2047,-2047))
        ANGLE = 'mt: {}\r\n'.format(angle)
        #saber.text('m1:startup')
        #saber.drive(1,speed)
        #saber.drive(2,speed)
        if angle+speed > 100:
            speed1 = 100
        else :
            speed1 = angle+ speed
        if speed-angle < -100:
            speed2 = -100
        else :
            speed2 = speed-angle
        saber.drive(1,speed)
        saber.drive(2,speed)
       

        if(angle < 0):
            print "negative"
            saber.drive(1,speed2)
            saber.drive(2,speed1)
        elif (angle > 0):
            print "positive"
            saber.drive(1,speed2)
            saber.drive(2,speed1)
   

        #saber.text('m2:startup')
        #MD: 0\r\n
        print SPEED
        print ANGLE
   
        #saber.text(SPEED)

        pass
        #print message

    def sabertoothStatusCallback(data):
        print data
        temperature = ('T [C]: {}'.format(saber.textGet('m2:gett')))
   
        saber.textGet('T,start')
        set_position = ('P : {}'.format(saber.textGet('T,p45')))
   
        battery = ('battery [mV]: {}'.format(saber.textGet('m2:getb')))
        print battery , temperature
        #lcd.lcd_write(0x0C) #Cursor uitschakelen.
        #lcd.lcd_write(0x01) #Scherm leegmaken.
        #lcd.lcd_puts("Hallo", 1) #Tekst voor LCD display lijn 1.
        #lcd.lcd_puts("  Wereld!", 2) #Tekst voor LCD display lijn 2.
        #lcd.lcd_backlight(1) #Achterverlichting aanzetten.
   


   
    def listener():
   
   
        # In ROS, nodes are uniquely named. If two nodes with the same
        # node are launched, the previous one is kicked off. The
        # anonymous=True flag means that rospy will choose a unique
        # name for our 'listener' node so that multiple listeners can
        # run simultaneously.
        rospy.init_node('listener', anonymous=True)

        #rospy.Subscriber("/joy_teleop/cmd_vel", Twist, callback)
        rospy.Subscriber("/cmd_vel", Twist, callback)
        #sabertoothStatusCallback("as")
        #rospy.Subscriber("/mastercmd_vel", Twist, callback)
        # spin() simply keeps python from exiting until this node is stopped
        rospy.spin()

    if __name__ == '__main__':
        listener()





https://github.com/smd-ros-devel/kangaroo_x2_driver
This is a good start in the ROSification of kangaroo x2 .


https://github.com/hbrobotics/ros_arduino_bridge
This is a full stack implementation of odometry packages on Arduino .
(I am seeking for something like this for the Kangaroo+sabertooth/Syren configuration )

https://www.youtube.com/watch?v=KIuhDSi-Nr8&t=27s
(Link to the video of the robot i am working on )


https://github.com/LillyCorp/sabertooth_2x32_kangaroo_x2_ros
(Link to the repository for ROS + Sabertooth + kangaroo x2)



Go Up