From bfa79b8862a41895ef450a2d2b9f9a1a8f08b951 Mon Sep 17 00:00:00 2001 From: Neil Nie Date: Sat, 19 Jan 2019 20:30:05 -0500 Subject: [PATCH] joystick accelerator control finished, tested --- .DS_Store | Bin 8196 -> 0 bytes .idea/workspace.xml | 101 ++++++++++-------- arduino/.DS_Store | Bin 6148 -> 0 bytes .../brake_accelerator_controller.ino | 30 ++---- ros/src/gui/launch/rqt.launch | 7 +- .../joystick/scripts/selective_output.py | 16 ++- 6 files changed, 84 insertions(+), 70 deletions(-) delete mode 100644 .DS_Store delete mode 100644 arduino/.DS_Store diff --git a/.DS_Store b/.DS_Store deleted file mode 100644 index ab0f773769231d8c7707856eacd46c2c89ef0bdf..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 8196 zcmeI1-Hy^g6vxjjtXuG7i7~R5y)p5E7|62hCdLH$FcNh&vg(Bzw%s;vW49%=NE9*L z_zbR(;FIVZ_#i%j{^vsi10*^n!tZa zfZrcXRIXK}W2xfSfkvbN&@_fcK_7X5@G+HEm5!wfSIjB12c}$^N->yp$9a>RL#s;1 zQl&dF=}t_2GE*4}6Q2%0Q#$Vx7DfU>_bd!U}p=eI)i1VP_WkTV`hu zqOKzI#qRoE6c>wM*-9$CdS=ZqvPSlN_Idj-9JFIA?ngDN_mm$!u;N2o_4mTipY$#J zvF8ptxl8phh%GO0d&1dsT$nt2;ss7PsD=H&iNri@dBQL4k6ynSz8$?g#$HgEx``6M`qSkj4kjF4wR?dd1}`wi?bpQh zt>cRA;wm?1X^OZwo{EcA#DEK;9$5)X9tlek$5#PexGO+XX)_Xx0bqur=s-9LX8Cbz(IZ4-4Ll^1R|mMUD(h;$q( g(sAhI4@2}#m~x-0(y>&Gpy(F?0)uWefj>&%2eZdc`v3p{ diff --git a/.idea/workspace.xml b/.idea/workspace.xml index 465e39d3..3670cdb1 100644 --- a/.idea/workspace.xml +++ b/.idea/workspace.xml @@ -2,12 +2,9 @@ - - - - - - + + + - + - @@ -533,16 +544,6 @@ - - - - - - - - - - @@ -580,13 +581,6 @@ - - - - - - - @@ -706,10 +700,27 @@ - + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/arduino/.DS_Store b/arduino/.DS_Store deleted file mode 100644 index f2a3b7a55b57b15e0a3f978f17873a27c1448075..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 6148 zcmeHK%}(1u5S|SwY*JO^fGFbROQjw-1o3x3NLC65Zb(LOpjEJm1s2AO9ES+fRDKJ1 z1)hXQ;Bne-b~gw_kt&3cC_B;YH@iDC&VFm}4-tu0uQN~7AR-B6tkh8aMYx~!kyLC; z6)1F$nA&tgiVCQcx4;0;-84m%Q%J{jfBtgJ0S z>u)Wvh-TrHASOmW&JZF;K|LV96aSX;`XzEk{qCv#pEuEInske1^Hx;GsvpKDq^!&O zr}~fJNcDpvEPCBmnD23ID=dy8Yi}!+-q~Talf?aYW9pmAiZIFITn8te7%ty;ldPlq zE!E39U7aT|9a7e0t=*U&3>KSn-qO-=&Ks<(z+7Bd9u8~r^OyOx&Ha<}i^0#~uS-oJ z34BUgCM}NO3dXbOJqz+IRoNlpN4ZBi9XbXa1MidpyBNri?{xfb*p30mz|#!y`rtts zeT$_*y>y_^R{$U%BelThVmvc`u_hg$kiMJj)DJ*0a0BK)>{~o-CG|G wj`vy*x({VxztZ4c2n@Xy!8K@Stdu#xGi={!dK-@t<)8Gonz^gLw2R_+}5C8xG diff --git a/arduino/brake_accelerator_controller/brake_accelerator_controller.ino b/arduino/brake_accelerator_controller/brake_accelerator_controller.ino index 4a522ef6..b14ae712 100644 --- a/arduino/brake_accelerator_controller/brake_accelerator_controller.ino +++ b/arduino/brake_accelerator_controller/brake_accelerator_controller.ino @@ -14,12 +14,12 @@ #define LPWM 6 #define M_PI 3.14159265359 #define POT_MAX 100 -#define POT_MIN 0 +#define POT_MIN 30 #define LA_MIN 0 #define LA_MAX 0 #define LA_PIN 0 -ros::NodeHandle nh; +ros::NodeHandle nh; boolean joystick_enabled = false; float target_speed = 0.0; @@ -34,11 +34,10 @@ void cruise_callback( const std_msgs::Float32& cmd_msg) { void joystick_callback( const std_msgs::Float32& cmd_msg) { - if (joystick_enabled) { - - if (cmd_val > 0) { + if (joystick_enabled == 1) { + if (cmd_msg.data >= 0) { cmd_val = mapf(cmd_msg.data, 0, 1, POT_MIN, POT_MAX); - } else if (cmd_val < 0) { + } else { // engage brakes } } @@ -77,7 +76,6 @@ void setup() { // set the slaveSelectPin as an output: pinMode (slave_Select_Pin, OUTPUT); digitalWrite(slave_Select_Pin, LOW); - // initialize SPI: SPI.begin(); } @@ -86,18 +84,12 @@ void loop() { potWrite(slave_Select_Pin, B00010001, cmd_val); potWrite(slave_Select_Pin, B00010010, cmd_val); - - /* - if (cruise_speed == -1) { - potWrite(slave_Select_Pin, B00010001, 0); - potWrite(slave_Select_Pin, B00010010, 0); - - press_break(1); - delay(1000); - release_break(1); - - Serial.println("brakes engaged"); - }*/ + + pos_msg.data = cmd_val; + pos_pub.publish(&pos_msg); + + nh.spinOnce(); + delay(5); } diff --git a/ros/src/gui/launch/rqt.launch b/ros/src/gui/launch/rqt.launch index 094a3989..e86f4590 100644 --- a/ros/src/gui/launch/rqt.launch +++ b/ros/src/gui/launch/rqt.launch @@ -4,10 +4,15 @@ - + + + + + + diff --git a/ros/src/sensors/joystick/scripts/selective_output.py b/ros/src/sensors/joystick/scripts/selective_output.py index 88d28d1d..da69b9b0 100755 --- a/ros/src/sensors/joystick/scripts/selective_output.py +++ b/ros/src/sensors/joystick/scripts/selective_output.py @@ -13,19 +13,24 @@ class SelectiveOutput(object): def __init__(self): self.output = None + self.left_stick_y = None rospy.init_node('selective_output') rospy.Subscriber('/sensor/joystick/joy', Joy, callback=self.joystick_input_callback, queue_size=5) - self.publisher = rospy.Publisher('/sensor/joystick/left_stick_x', data_class=Float32, queue_size=5) - - rate = rospy.Rate(24) + self.publisher1 = rospy.Publisher('/sensor/joystick/left_stick_x', data_class=Float32, queue_size=5) + self.publisher2 = rospy.Publisher('/sensor/joystick/right_stick_y', data_class=Float32, queue_size=5) + rate = rospy.Rate(30) while not rospy.is_shutdown(): - if self.output is not None: + if self.output is not None and self.left_stick_y is not None: data = Float32() data.data = self.output - self.publisher.publish(data) + self.publisher1.publish(data) + + data2 = Float32() + data2.data = self.left_stick_y + self.publisher2.publish(data2) rate.sleep() @@ -33,6 +38,7 @@ def joystick_input_callback(self, data): inputs = data.axes self.output = inputs[0] + self.left_stick_y = inputs[4] # rospy.loginfo(self.output)