Merge PR #11 (Add ROS Position Control via Topic Subscription)

* release new minor version: `v0.2.0`
* resolves merge conflicts

Author: acbuynak

arduino_control/control_as_client/a_declarations.ino

@@ -7,21 +7,22 @@
 void setup_network();
 void setup_ros();
 
-// Feedback Position
-void report_serial();
+// Conversions
 float posn_incr2inch();
 float posn_incr2meter();
+float temperature_count2degree();
 
-// Feedback Sensors
+// Feedback
+void report_serial();
 bool get_fork_state();
 bool get_press_power_switch_state();
-float temperature_count2degree();
 bool get_temperature1_alarm1_state();
 
 // External Indicators
 void set_remote_indicator();
 
 // ROS & Network Interfaces
+void cb_joint_command(const std_msgs::Float64);
 void update_ros_robot_status();
 void update_ros_joint_states();
 void update_ros_fork_state();
@@ -34,6 +35,8 @@ void clearSerialBuffer();
 void moveDown();
 void moveUp();
 void halt();
+void set_flow_control_valve(bool state);
+bool tempFlowValveState = false;
 
 // Controller
 void controller();

arduino_control/control_as_client/b_configure.ino

@@ -40,6 +40,13 @@ const float minStrokeSoft = 0.0;
 const float maxStrokeHard = 0.20294907837554;   // not currently used by control
 const float minStrokeHard = 0.0;                // not currently used by control
 
+//Remote Interface
+bool ros_enabled = false;
+float last_ros_joint_command = 0;        // Initialize goal to zero for safety
+
+//Position control
+const float motionBuffer = 0.012;
+
 //Target Position
 bool remoteMotionEnabled = false;
 bool directionOfMotion;                  // false = down, true = up

arduino_control/control_as_client/c_setup.ino

@@ -15,6 +15,7 @@ std_msgs::Bool fork_state_msg;
 std_msgs::Bool press_power_switch_state_msg;
 std_msgs::Bool temperature1_alarm1_state_msg;
 sensor_msgs::Temperature temperature1_msg;
+std_msgs::Float64 joint_command;
 //industrial_msgs::RobotStatus robot_status_msg;
 
 ros::Publisher pub_joint_states("joint_states", &joint_state_msg);
@@ -24,6 +25,7 @@ ros::Publisher pub_temperature1_alarm1_state("temperature1_alarm1_state", &tempe
 ros::Publisher pub_temperature1("temperature1", &temperature1_msg);
 //ros::Publisher pub_robot_status("robot_status", &robot_status_msg);
 
+ros::Subscriber<std_msgs::Float64> sub_joint_command("joint_command", cb_joint_command);
 
 
 // -------------------
@@ -45,6 +47,10 @@ void setup() {
     Serial.println("Setup Configuration: Network enabled. Will now attempt to connect to network");
     Ethernet.begin(mac);
     setup_network();
+    ros_enabled = true;
+  }
+  else {
+    ros_enabled = false;
   }
 
   // Setup ROS handles

arduino_control/control_as_client/control_as_client.ino

@@ -13,7 +13,7 @@
  * Author: Adam Buynak
  */
 
- /* Software Version: v0.1.1 */
+ /* Software Version: v0.2.0 */
 
 
 #include <Arduino.h>
@@ -33,6 +33,7 @@
 #include <std_msgs/Header.h>
 #include <sensor_msgs/JointState.h>
 #include <std_msgs/Bool.h>
+#include <std_msgs/Float64.h>
 #include <sensor_msgs/Temperature.h>
 //#include <industrial_msgs/RobotStatus.h>
 

arduino_control/control_as_client/e_motion.ino

@@ -15,6 +15,15 @@ void moveUp() {
 }
 
 
+void set_flow_control_valve(bool state)
+{
+  //Input 'state' sets the speed of the press
+  //by opening/closing the flow control valve.
+  //High for fast, LOW for slow
+  P1.writeDiscrete(state,1,7); //Set the flow control valve to state
+}
+
+
 void halt() {
   P1.writeDiscrete(LOW,1,2);   //Turn slot 1 channel 1 off
   P1.writeDiscrete(LOW,1,3);   //Turn slot 1 channel 2 off

arduino_control/control_as_client/g_ros.ino

@@ -16,6 +16,32 @@ void setup_ros() {
   nh.advertise(pub_temperature1);
 //  nh.advertise(pub_robot_status);/
 
+  nh.subscribe(sub_joint_command);
+}
+
+void cb_joint_command(const std_msgs::Float64& msg) {
+  
+  // Extract data from message
+  last_ros_joint_command = msg.data;
+
+  // Lookup Current Position
+  float current_posn = posn_incr2meter(P1.readAnalog(2, 1));
+
+  // Soft Stop Limits
+  if      (last_ros_joint_command > maxStrokeSoft) last_ros_joint_command = maxStrokeSoft;
+  else if (last_ros_joint_command < minStrokeSoft) last_ros_joint_command = minStrokeSoft;
+
+  // Set the direction of motion
+  if (current_posn < last_ros_joint_command) directionOfMotion = false;  // down
+  else                                       directionOfMotion = true;   // up
+
+  // Serial Feedback
+  Serial.print("Set New Target Position: ");
+  Serial.println(last_ros_joint_command, 3);
+
+  // Enable Motion
+  remoteMotionEnabled = true;
+  remoteTargetAchieved = false;
 }
 
 

arduino_control/control_as_client/h_controller.ino

@@ -15,7 +15,9 @@ void controller(float current_posn) {
   bool override_lockout = (last_override + override_timeout) > millis();
 
   if (mswitch_up || mswitch_dwn || override_lockout) {
+    set_flow_control_valve(LOW);
     set_remote_indicator(false);
+    remoteMotionEnabled = false;
     if ( mswitch_up == true && mswitch_dwn == false ) {
       last_override = millis();
       moveUp();
@@ -25,9 +27,8 @@ void controller(float current_posn) {
       moveDown();
     }
     else {
-      remoteMotionEnabled = false;
-      target_posn = current_posn; // Ensure fail's safe
       halt();
+      target_posn = current_posn; // Ensure fail's safe
     } 
   }
 
@@ -37,7 +38,12 @@ void controller(float current_posn) {
   // Remote Input ~ Serial or ROS
   // -----------------------------
 
-  if (Serial.available() && override_lockout == false) {
+  if (Serial.available() && override_lockout == true) {
+    Serial.println("--- !! Remote Control Lockout. Please wait 5-seconds !! ---");
+    clearSerialBuffer();
+  }
+  
+  else if (Serial.available() && ros_enabled == false && override_lockout == false) {
 
     // Serial Client Input Position of unit: [mm]
     target_posn = Serial.parseFloat() * 0.001;
@@ -59,14 +65,16 @@ void controller(float current_posn) {
     remoteMotionEnabled = true;
     remoteTargetAchieved = false;
   }
-  else if (Serial.available() && override_lockout == true) {
-    Serial.println("--- !! Remote Control Lockout. Please wait 5-seconds !! ---");
-    clearSerialBuffer();
+
+  else if (ros_enabled == true && override_lockout == false) {
+    // Serial Client Input Position of unit: [meter]
+    target_posn = last_ros_joint_command;
   }
 
-  //else if (_ADD_ROS_CHECK_) {
-  //  //ADD ROS CODE HERE
-  //}
+  if (Serial.available() && ros_enabled == true) {
+    Serial.println("--- !! Serial Control Lockout. Only accepting ROS Control. !! ---");
+    clearSerialBuffer();
+  }
 
 
 
@@ -80,13 +88,29 @@ void controller(float current_posn) {
     if      (directionOfMotion == false && current_posn >= target_posn) remoteTargetAchieved = true;  // overshoot moving down              
     else if (directionOfMotion == true  && current_posn <= target_posn) remoteTargetAchieved = true;  // overshoot moving up
 
-    // Actuate if target not met
+    // Actuate Control given Target Achieved
     if (remoteTargetAchieved == true) {
       halt();
+      set_flow_control_valve(LOW);  // default back to slow speed
       target_posn = current_posn;   // fail safe
     }
     else if (directionOfMotion == false)   moveDown();
     else if (directionOfMotion == true)    moveUp();
 
+    // Set speed of press motion
+    if(directionOfMotion == false && current_posn > target_posn - motionBuffer) // moving down
+    {
+      set_flow_control_valve(LOW);
+    }
+    else 
+    {
+      set_flow_control_valve(HIGH);
+    }
+
+    if(directionOfMotion == true && remoteTargetAchieved == false) // moving up
+    {
+      set_flow_control_valve(HIGH);
+    }
+    else if (directionOfMotion == true) set_flow_control_valve(LOW);
   }
 }

arduino_control/control_as_client/i_run.ino

@@ -19,7 +19,7 @@ void loop()
   report_serial(current_posn_incr, current_temp_incr);
 
   // ROS Interface
-  if (nh.connected()) {
+  if (nh.connected() && ros_enabled) {
 
     // Set ROS Indicator Light = ON
     set_remote_indicator(true);

launch/connect_arduino.launch

@@ -1,4 +1,4 @@
 <launch>
 	<!-- Default Port: 11411 -->
-	<node pkg="rosserial_server" type="socket_node" name="rosserial_server" output="screen" />
+	<node pkg="rosserial_server" type="socket_node" name="control_interface" ns="coaliron" output="screen" />
 </launch>