diff --git a/.DS_Store b/.DS_Store deleted file mode 100644 index ab0f7737..00000000 Binary files a/.DS_Store and /dev/null differ 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 f2a3b7a5..00000000 Binary files a/arduino/.DS_Store and /dev/null differ 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)