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 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:
#include //for v1.6
#include
#include
#include
#include
#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