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):

#!/usr/bin/env python3from time import sleepimport timeimport numpy as npimport rospyimport RPi.GPIO as GPIOfrom sensor_msgs.msg import JointStatefrom multiprocessing import Process, Array, Valueimport multiprocessingValveFront = 17ValveRear = 27enPinR = 13directionpinR = 19steppinR = 26enPinL= 16directionpinL = 20steppinL = 21FORWARD = GPIO.HIGHBACKWARD = GPIO.LOWCLOCKWISE = GPIO.HIGHCOUNTERCLOCK = GPIO.LOWstepsPerRevolution = 6400OUTPUT = GPIO.OUT# pin = 12lspeed = 100rotateSpeed = 50rotationCountReader = 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:passdef 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/lspeedGPIO.output(directionpinL,directionL)for i in range(int(stepsPerRevolution*dis/6)): # stepsPerRevolution*dis/6GPIO.output (steppinL,GPIO.HIGH)delayMicroseconds(50)# sleep(0.005)GPIO.output (steppinL,GPIO.LOW)delayMicroseconds(50)if(directionL == FORWARD):linearCountReader.value = linearCountReader.value+1else:linearCountReader.value = linearCountReader.value-1# sleep(0.005)def LmotorD(directionL:bool):GPIO.output(directionpinL,directionL)for i in range(2): # stepsPerRevolution*dis/6GPIO.output (steppinL,GPIO.HIGH)delayMicroseconds(50)# sleep(0.005)GPIO.output (steppinL,GPIO.LOW)delayMicroseconds(50)if(directionL == FORWARD):linearCountReader.value = linearCountReader.value+1else:linearCountReader.value = linearCountReader.value-1def 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+1else: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+1else:rotationCountReader.value = rotationCountReader.value-1def 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:

#include //for v1.6#include #include #include #include #define PS2_DATA9//14#define PS2_CMDA10//15#define PS2_SELA11//16#define PS2_CLKA12//17#define pressures false#define rumblefalsePS2X ps2x; // create PS2 Controller Classros::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 movementint responseDelay = 5;// response delay of the mouse, in msint threshold = 2;// resting thresholdint center = 127; // resting position valuesensor_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 errorerror = 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 foundreturn;ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speedif(ps2x.Button(PSB_PAD_LEFT)){// Serial.print("LEFT held this hard: ");// Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC);//-xx_ = -1;}else if(ps2x.Button(PSB_PAD_RIGHT)){// Serial.print("Right held this hard: ");// Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC); //+xx_ = 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);//-yy_ = -1;}else if(ps2x.Button(PSB_PAD_DOWN)){// Serial.print("DOWN held this hard: ");// Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);//+yy_ = +1;}else{y_ = 0;} if(ps2x.Button(PSB_L2)){// Serial.println("L2 pressed"); //-zz_ = -1;}else if(ps2x.Button(PSB_L1)){// Serial.println("L1 pressed"); //+zz_ = +1;}else{z_ = 0;}if(ps2x.Button(PSB_R2)){// Serial.println("L2 pressed"); //-zz2_ = -1;}else if(ps2x.Button(PSB_R1)){// Serial.println("L1 pressed"); //+zz2_ = +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) buttonif (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 pressedSerial.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 pressedSerial.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 releasedSerial.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 releasedSerial.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