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 multiprocessingValveFront = 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 = 12lspeed = 100
rotateSpeed = 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 <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      falsePS2X 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 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