Example: use raspberry pi 4 control multiple motors(tb660)

abstract

This is a project for controlling multiple motors by using joystick, the buttons' information can be obtained from topic "/joystick". We got the sensor's signals from arduino, and sent the command from topic "joystick".

raspberry pi code(python):

python 复制代码

#!/usr/bin/env python3
from time import sleep
import time
import numpy as np
import rospy
import RPi.GPIO as GPIO
from sensor_msgs.msg import JointState
from multiprocessing import Process, Array, Value
import multiprocessing


ValveFront = 17
ValveRear = 27
enPinR = 13
directionpinR = 19
steppinR = 26
enPinL  = 16
directionpinL = 20
steppinL = 21
FORWARD = GPIO.HIGH
BACKWARD = GPIO.LOW
CLOCKWISE = GPIO.HIGH
COUNTERCLOCK = GPIO.LOW
stepsPerRevolution = 6400
OUTPUT = GPIO.OUT
# pin = 12

lspeed = 100
rotateSpeed = 50

rotationCountReader = Value("f",0)
linearCountReader = Value("f",0)
# msg_ = np.zeros((7,),np.float16)
msg_ = Array("f",[0,0,0,0,0,0,0,0,0,0,0,0,0,0])
lock = multiprocessing.Lock()

def delayMicroseconds(sec):
    start = time.time()
    while (time.time()-start)*1000000 < sec:
        pass

def setupBoard():
    GPIO.setmode(GPIO.BCM)
    # GPIO.setup(pin,GPIO.OUT)
    GPIO.setwarnings(False)
    GPIO.setup (ValveFront,OUTPUT)
    GPIO.setup(ValveRear,OUTPUT)
    GPIO.setup (directionpinR,OUTPUT)
    GPIO.setup (steppinR,OUTPUT)
    GPIO.setup(enPinR,OUTPUT)
    GPIO.setup (directionpinL,OUTPUT)
    GPIO.setup (steppinL,OUTPUT)
    GPIO.setup(enPinL,OUTPUT)
    GPIO.output(ValveFront,GPIO.LOW)
    GPIO.output(ValveRear,GPIO.LOW)
    GPIO.output(enPinR,GPIO.LOW)
    GPIO.output(enPinL,GPIO.LOW)

def Lmotor(directionL:bool,dis:float):
    speed_m = 400.0/lspeed
    GPIO.output(directionpinL,directionL)
    for i in range(int(stepsPerRevolution*dis/6)): # stepsPerRevolution*dis/6
        GPIO.output (steppinL,GPIO.HIGH)
        delayMicroseconds(50)
        # sleep(0.005)
        GPIO.output (steppinL,GPIO.LOW)
        delayMicroseconds(50)
        if(directionL == FORWARD):
            linearCountReader.value = linearCountReader.value+1
        else:
            linearCountReader.value = linearCountReader.value-1
        # sleep(0.005)

def LmotorD(directionL:bool):
    GPIO.output(directionpinL,directionL)
    for i in range(2): # stepsPerRevolution*dis/6
        GPIO.output (steppinL,GPIO.HIGH)
        delayMicroseconds(50)
        # sleep(0.005)
        GPIO.output (steppinL,GPIO.LOW)
        delayMicroseconds(50)
        if(directionL == FORWARD):
            linearCountReader.value = linearCountReader.value+1
        else:
            linearCountReader.value = linearCountReader.value-1

def Rmotor(directionR = CLOCKWISE):
    GPIO.output(directionpinR,directionR)
    for i in range(int(stepsPerRevolution/4)):
        GPIO.output (steppinR,GPIO.HIGH)
        delayMicroseconds(rotateSpeed)
        # sleep(0.005)
        GPIO.output (steppinR,GPIO.LOW)
        delayMicroseconds(rotateSpeed)
        if(directionR == CLOCKWISE):
            rotationCountReader.value = rotationCountReader.value+1
        else:
            rotationCountReader.value = rotationCountReader.value-1
        # sleep(0.005)

def RmotorD(directionR = CLOCKWISE):
    GPIO.output(directionpinR,directionR)
    for i in range(2):
        GPIO.output (steppinR,GPIO.HIGH)
        delayMicroseconds(rotateSpeed)
        # sleep(0.005)
        GPIO.output (steppinR,GPIO.LOW)
        delayMicroseconds(rotateSpeed)
        if(directionR == CLOCKWISE):
            rotationCountReader.value = rotationCountReader.value+1
        else:
            rotationCountReader.value = rotationCountReader.value-1

def RotationThread():
    # global msg_
    while True:
        # print(stepsPerRevolution)
        if msg_[0] >= 1:
            print("rotating")
            Rmotor(CLOCKWISE)
            sleep(0.001)
        if msg_[0] == -1:
            print("counter rotating")
            Rmotor(COUNTERCLOCK)
            sleep(0.001)
        if msg_[5] > 50:
            RmotorD(CLOCKWISE)
        if msg_[5] < -50:
            RmotorD(COUNTERCLOCK)

def LinearThread():
    while True:
        if msg_[4] >= 50:
            GPIO.output(ValveFront,GPIO.LOW)
            GPIO.output(ValveRear,GPIO.HIGH)
            sleep(0.02)
            Lmotor(BACKWARD,10)
            GPIO.output(ValveFront,GPIO.HIGH)
            GPIO.output(ValveRear,GPIO.LOW)
            sleep(0.02)
            Lmotor(FORWARD,10)
            print("moving")
            sleep(0.001)
        if msg_[4] <= -50:
            GPIO.output(ValveFront,GPIO.LOW)
            GPIO.output(ValveRear,GPIO.HIGH)
            sleep(0.02)
            Lmotor(FORWARD,10)
            GPIO.output(ValveFront,GPIO.HIGH)
            GPIO.output(ValveRear,GPIO.LOW)
            sleep(0.02)
            Lmotor(BACKWARD,10)
            print("back moving")
            sleep(0.001)

        if msg_[6] > 50:
            LmotorD(BACKWARD)
        if msg_[6] < -50:
            LmotorD(FORWARD)
        

def doMsg(msg:JointState):
    for i in range(7):
        msg_[i] = msg.position[i]


def ft_sub():
    rospy.init_node('mediator',anonymous=True)
    sub = rospy.Subscriber('joystick',JointState,doMsg,queue_size=10)
    rate = rospy.Rate(1000)
    
    while not rospy.is_shutdown():
        # print(msg_[0])
        print(linearCountReader.value)
        print(rotationCountReader.value)
        rate.sleep()

if __name__=='__main__':
    try:
        setupBoard()
        a1 = Process(target=RotationThread)
        a2 = Process(target=LinearThread)
        a3 = Process(target=ft_sub)
        a1.start()
        a2.start()
        a3.start()
        a1.join()
        a2.join()
        a3.join()
        print("----------------")
    except KeyboardInterrupt:
        GPIO.cleanup()

arduino code:

c++ 复制代码

#include <PS2X_lib.h>  //for v1.6
#include <ros.h>
#include <ros/time.h>
#include <std_msgs/Int8.h>
#include <sensor_msgs/JointState.h>

#define PS2_DAT        A9  //14    
#define PS2_CMD        A10  //15
#define PS2_SEL        A11  //16
#define PS2_CLK        A12  //17


#define pressures   false
#define rumble      false

PS2X ps2x; // create PS2 Controller Class
ros::NodeHandle node_handle;

int error = 0;
byte type = 0;
byte vibrate = 0;

// parameters for reading the joystick:
int range = 127;               // output range of X or Y movement
int responseDelay = 5;        // response delay of the mouse, in ms
int threshold = 2;      // resting threshold
int center = 127;         // resting position value

sensor_msgs::JointState msg;
std_msgs::Int8 msg_;
ros::Publisher pub("joystick", &msg);

char* id = "/joint";
char *a[] = {"FL", "FR", "BR", "BL","BA","BC","BV","START","SELECT","CIRCLE","TRIAGNLE","SQUARE","CROSS","Z2"};
float pos[14];

void setup(){
 
  node_handle.getHardware()->setBaud(57600);
  
  delay(300);  //added delay to give wireless ps2 module some time to startup, before configuring it
  
  //setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
  error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);

  node_handle.initNode();
  node_handle.advertise(pub);

  
  msg.header.frame_id = id;
  msg.name_length = 14;
  msg.position_length = 14;
  msg.name= a;
  msg.position = pos;
}

int x_ = 0;
int y_ = 0;
int z_ = 0;
int lx_ = 0;
int ly_ = 0;
int rx_ = 0;
int ry_ = 0;
bool start_ = 0;
int select_ = 0;
int circle_ = 0;
int triangle_ =0;
int square_ = 0;
int cross_ = 0;
int z2_ = 0;

void loop() {

  if(error == 1) //skip loop if no controller found
    return; 
  
   ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed
    
    if(ps2x.Button(PSB_PAD_LEFT)){
      // Serial.print("LEFT held this hard: ");
      // Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC);                  //-x
      x_ = -1;
    }
    else if(ps2x.Button(PSB_PAD_RIGHT)){
      // Serial.print("Right held this hard: ");
      // Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC);                 //+x
      x_ = 1;
    }
    else{
      x_ = 0;
    }
    
    if(ps2x.Button(PSB_PAD_UP)) {      //will be TRUE as long as button is pressed
      // Serial.print("Up held this hard: ");
      // Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);                    //-y
      y_ = -1;
    }
    else if(ps2x.Button(PSB_PAD_DOWN)){
      // Serial.print("DOWN held this hard: ");
      // Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);                  //+y
      y_ = +1;
    }  
    else{
      y_ = 0;
    }
     
    if(ps2x.Button(PSB_L2)){
      // Serial.println("L2 pressed");                                   //-z
      z_ = -1;
    }
    else if(ps2x.Button(PSB_L1)){
        // Serial.println("L1 pressed");                                   //+z
        z_ = +1;
    }
    else{
        z_ = 0;
    }

    if(ps2x.Button(PSB_R2)){
      // Serial.println("L2 pressed");                                   //-z
      z2_ = -1;
    }
    else if(ps2x.Button(PSB_R1)){
        // Serial.println("L1 pressed");                                   //+z
        z2_ = +1;
    }
    else{
        z2_ = 0;
    }

    vibrate = ps2x.Analog(PSAB_CROSS);  //this will set the large motor vibrate speed based on how hard you press the blue (X) button
    if (ps2x.NewButtonState()) {        //will be TRUE if any button changes state (on to off, or off to on)
      if(ps2x.Button(PSB_START)){         //will be TRUE as long as button is pressed
        Serial.println("Start is being held");
        start_ = !start_;
      }
      if(ps2x.Button(PSB_SELECT)){
        Serial.println("Select is being held"); 
        select_ += 1;
      }
      if(ps2x.Button(PSB_CIRCLE)){               //will be TRUE if button was JUST pressed
        Serial.println("Circle just pressed");
        circle_ = 1;
      }
      else if(ps2x.ButtonReleased(PSB_CIRCLE))
        circle_ = 0;
        
      if(ps2x.Button(PSB_CROSS)){               //will be TRUE if button was JUST pressed OR released
        Serial.println("X just changed");
        cross_ = 1;
      }
      else if(ps2x.ButtonReleased(PSB_CROSS))
        cross_ = 0;
        
      if(ps2x.Button(PSB_SQUARE)){              //will be TRUE if button was JUST released
        Serial.println("Square just released");
        square_ = 1;
      }
      else if(ps2x.ButtonReleased(PSB_SQUARE))
        square_ = 0;     
        
      if(ps2x.Button(PSB_TRIANGLE)){
        Serial.println("Triangle pressed");
        triangle_ = 1; 
      }  
      else if(ps2x.ButtonReleased(PSB_TRIANGLE))
        triangle_ = 0;   
    }

    lx_ = readAxis(PSS_LX);
    ly_ = readAxis(PSS_LY); 
    rx_ = readAxis(PSS_RX);
    ry_ = readAxis(PSS_RY); 

    // if(abs(rx_)>0)
    //     Serial.println(rx_, DEC);
    // if(abs(ry_)>0)
    //     Serial.println(ry_, DEC);

//     Serial.println(x_, DEC); 

  msg.position[0] = x_; 
  msg.position[1] = y_; 
  msg.position[2] = z_; 
  msg.position[3] = lx_; 
  msg.position[4] = ly_;
  msg.position[5] = rx_; 
  msg.position[6] = ry_;
  msg.position[7] = start_;
  msg.position[8] = select_;
  msg.position[9] = circle_;
  msg.position[10] = triangle_;
  msg.position[11] = square_;
  msg.position[12] = cross_;
  msg.position[13] = z2_;


  // msg.header.stamp = ros::Time::now();
  
  pub.publish( &msg );

  node_handle.spinOnce();
  delay(25);  
}

int readAxis(int thisAxis) { 
  // read the analog input:
  int reading = ps2x.Analog(thisAxis);

  // map the reading from the analog input range to the output range:
  // reading = map(reading, 0, 255, 0, range);

  // if the output reading is outside from the
  // rest position threshold,  use it:
  int distance = reading - center;

  if (abs(distance) < threshold) {
    distance = 0;
  } 

  // return the distance for this axis:
  return distance;
}

Reference

https://github.com/zouyuelin/colomagJoycontrol_ros/blob/master