some more debug logging and some different sleep values.

ulrichard · Jan 24, 2012 · 4ae8072 · 4ae8072
1 parent dbea4d7
commit 4ae8072
Show file tree

Hide file tree

Showing 5 changed files with 48 additions and 27 deletions.
diff --git a/ros/uc_arm/ArexxArmHardware.h b/ros/uc_arm/ArexxArmHardware.h
@@ -26,9 +26,15 @@ class ArexxArmHardware
     {
         const uint8_t stat = getUARTReceiveStatus();
 
-		char cc;
-		receiveBytesToBuffer(1, &cc);
-		return cc;
+		char cc[4];
+//		receiveBytesToBuffer(1, &cc[0]);
+
+        receiveBytes(1);
+        while(getUARTReceiveStatus() == UART_BUISY)
+            ; // busy waiting
+        copyReceivedBytesToBuffer(&cc[0]);
+
+		return cc[0];
 	}
 
     void write(uint8_t* data, int length)
@@ -37,8 +43,20 @@ class ArexxArmHardware
         	writeChar(data[i]);
     }
 
-    void writeStr(const char* data)
-    {   write(reinterpret_cast<uint8_t*>(const_cast<char*>(data)), strlen(data));  }
+    void writeStr(const char* data, const bool newline)
+    {
+        write(reinterpret_cast<uint8_t*>(const_cast<char*>(data)), strlen(data));
+        if(newline)
+            writeChar('\n');
+    }
+
+    void writeInt(const int16_t val, const bool newline)
+    {
+        writeInteger(val, BIN);
+        if(newline)
+            writeChar('\n');
+    }
+
 
     unsigned long time()
     { return getStopwatch1(); }

diff --git a/ros/uc_arm/RobotArm_ROS.cpp b/ros/uc_arm/RobotArm_ROS.cpp
@@ -30,6 +30,7 @@ namespace ros
 /*
 #else
 
+//  typedef NodeHandle_<ArexxArmHardware, 25, 25, 512, 512> NodeHandle;
   typedef NodeHandle_<ArexxArmHardware> NodeHandle;
 
 #endif
@@ -68,11 +69,7 @@ int main(void)
         int j = i * i * i;
 	initRobotBase(); // Always call this first!
 	Power_Servos();
-	mSleep(200);
-
-#ifdef ROBOT_ARM_UART_DEBUGGING
-    writeNStringP("started RobotArm_ROS");
-#endif
+	mSleep(200);
 
 	ros::NodeHandle nh;
 	nh.initNode();
@@ -98,13 +95,9 @@ int main(void)
     ros::Publisher pubServo1("ArexxArmServoCurrent1", &servo1curr);
     nh.advertise(pubServo1);
 
-
-    PowerLEDgreen();
-
 #ifdef ROBOT_ARM_UART_DEBUGGING
-    writeNStringP("entering main loop");
-#endif
-
+    writeString_P("entering main loop\n");
+#endif
 	while(true) // main loop
 	{
 //		setBeepsound();
@@ -115,6 +108,7 @@ int main(void)
         servo1curr.data = readADC(1);
         pubServo1.publish(&servo1curr);
 
+
         // ros communication
 		nh.spinOnce();
 	}

diff --git a/ros/uc_arm/nodes/MySerialClient.py b/ros/uc_arm/nodes/MySerialClient.py
@@ -217,6 +217,7 @@ def __init__(self, port=None, baud=57600, timeout=5.0):
         self.port.setRTS(True)
         time.sleep(0.3) 
         self.port.setRTS(False)
+        time.sleep(2.0)
 
         self.port.timeout = 0.01  # Edit the port timeout
 

diff --git a/ros/uc_arm/request_topics.py b/ros/uc_arm/request_topics.py
@@ -19,14 +19,17 @@ def output_data(filedev, ignore = '\r'):
 	port = Serial('/dev/ttyUSB0', 38400, timeout=0.5)
 
 	# reset the robot arm
+	print 'reset the robot arm.'
 	port.flushInput()
+	port.flushOutput()
 	port.setRTS(True)
 	time.sleep(0.3) 
 	port.setRTS(False)
 
-	time.sleep(1.0)
+	time.sleep(5.0)
 
 	port.flushInput()
+	print 'sending the request.'
 	port.write("\xff\xff\x00\x00\x00\x00\xff")
 
 	try:

diff --git a/ros/uc_arm/ros_lib/ros/node_handle.h b/ros/uc_arm/ros_lib/ros/node_handle.h
@@ -157,16 +157,21 @@ namespace ros {
         }
 
 #ifdef ROBOT_ARM_UART_DEBUGGING
-        hardware_.writeStr("spinOnce() c_time: ");
-        char tmp[16];
-        sprintf(tmp, "%010d", c_time);
-        hardware_.writeStr(tmp);
+//        hardware_.writeStr("spinOnce() c_time: ", false);
+//        hardware_.writeInt(c_time, true);
 #endif
 
         /* while available buffer, read data */
         while( true )
         {
-          int data = hardware_.read();
+          const int data = hardware_.read();
+#ifdef ROBOT_ARM_UART_DEBUGGING
+//        hardware_.writeStr("received: ", false);
+//        hardware_.writeInt(data, true);
+		setBeepsound();
+		mSleep(10);
+		clearBeepsound();
+#endif
           if( data < 0 )
             break;
           checksum_ += data;
@@ -207,7 +212,7 @@ namespace ros {
             if( (checksum_%256) == 255){
               if(topic_ == TopicInfo::ID_PUBLISHER){
 #ifdef ROBOT_ARM_UART_DEBUGGING
-                hardware_.writeStr("spinOnce() ID_PUBLISHER");
+                hardware_.writeStr("spinOnce() ID_PUBLISHER", true);
 #endif
                 requestSyncTime();
                 negotiateTopics();
@@ -216,25 +221,25 @@ namespace ros {
                 return -1;
               }else if(topic_ == TopicInfo::ID_TIME){
 #ifdef ROBOT_ARM_UART_DEBUGGING
-                hardware_.writeStr("spinOnce() ID_TIME");
+                hardware_.writeStr("spinOnce() ID_TIME", true);
 #endif
                 syncTime(message_in);
               }else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST){
 #ifdef ROBOT_ARM_UART_DEBUGGING
-                hardware_.writeStr("spinOnce() D_PARAMETER_REQUEST");
+                hardware_.writeStr("spinOnce() D_PARAMETER_REQUEST", true);
 #endif
                   req_param_resp.deserialize(message_in);
                   param_recieved= true;
               }else{
 #ifdef ROBOT_ARM_UART_DEBUGGING
-                hardware_.writeStr("spinOnce() else");
+                hardware_.writeStr("spinOnce() else", true);
 #endif
                 if(subscribers[topic_-100])
                   subscribers[topic_-100]->callback( message_in );
               }
             }
 #ifdef ROBOT_ARM_UART_DEBUGGING
-            hardware_.writeStr("spinOnce() checksum error");
+            hardware_.writeStr("spinOnce() checksum error", true);
 #endif
           }
         }