Full push of code that works for blade control via controller

scripts/HindBrain.py

@@ -4,6 +4,7 @@
 import rospy as rp
 import sys
 from ackermann_msgs.msg import AckermannDrive
+from geometry_msgs.msg import Pose
 from std_msgs.msg import Bool
 from helper_functions import *
 
@@ -21,6 +22,7 @@ def __init__(self):
         self.pose_sub = rp.Subscriber('/cmd_hitch', Pose, self.pose_cb)
         self.rate = rp.Rate(5)
         self.prev_command = ''
+        self.name = 'hb'
 
         self.vel_range = (-1,0,1)
         self.steer_range = (-45,0,45)
@@ -77,8 +79,8 @@ def pose_cb(self, msg):
         :param[out] self.prev_command: Updates previous command attribute
         """
         z_cmd = cmd2msg(msg.position.z, self.z_range, self.msg_range)
-        pitch_cmd = cmd2msg(msg.orientation.x, self.pitch_range, self.msg_range)
-        command = "!b:" + str(int(z_cmd)) + ":" + str(int(pitch_cmd)) + ":"
+        pitch_cmd = cmd2msg(msg.orientation.y, self.pitch_range, self.msg_range)
+        command = "!b:" + str(int(z_cmd)) + ":" + str(int(pitch_cmd)) + ":\n"
         if (command != self.prev_command):
             self.send(command)
             self.prev_command = command
@@ -93,19 +95,20 @@ def send(self, msg):
         """
         try:
             self.uno_port.write(msg.encode('utf-8'))
-            return true
+            rp.loginfo("%s - Sent message: %s", self.name, msg)
+            return True
         except Exception as e:
-            rp.logerr("Lost connectivity with rover")
+            rp.logerr("%s - lost connectivity with rover", self.name)
             rp.logerr(e)
-            return false
+            return False
 
     def run(self):
         """runs node mainloop
 
         sends watchdog msg at rate attr. frequency if ros is ok
         """
         while not rp.is_shutdown():
-            self.send('!w') # Satisfy watchdog
+            #self.send('!w') # Satisfy watchdog
             self.rate.sleep()
 
 if __name__ == '__main__':

src/hind_brain/hind_brain.h

@@ -20,11 +20,12 @@ const byte STEER_SERVO_PIN = 5;
 const byte VEL_SERVO_PIN   = 6;
 const byte Z_SERVO_PIN     = 9;
 const byte PITCH_SERVO_PIN = 10;
+const byte PITCH_SERVO2_PIN = 11;
 const byte ESTOP_LED_PIN   = 13;
 
 // General Constants
 #define DEBUG True
-#define WATCHDOG_TIMEOUT 250        // milliseconds
+#define WATCHDOG_TIMEOUT 500        // milliseconds
 #define LED_BLINK_DELAY 750         // milliseconds
 #define BAUD_RATE 9600              // Hz
 #define DEBOUNCE_DELAY 500          // milliseconds
@@ -54,12 +55,12 @@ const int Z_MSG_CENTER = 50;        // . . .    middle . . .
 const int Z_MSG_BOTTOM = 0;         // . . .    lowest . . .
 
 // Blade Pitch Motor Ranges
-const int PITCH_CMD_UP = 0;         // Servo cmd for maximum upward   blade pitch
-const int PITCH_CMD_CENTER = 90;    // . . .         central          . . .
-const int PITCH_CMD_DOWN = 180;     // . . .         maximum downward . . .
-const int PITCH_MSG_UP = 0;         // Pose msg maximum upward   blade pitch
+const int PITCH_CMD_UP = 60;         // Servo cmd for maximum upward   blade pitch
+const int PITCH_CMD_CENTER = 30;    // . . .         central          . . .
+const int PITCH_CMD_DOWN = 0;     // . . .         maximum downward . . .
+const int PITCH_MSG_UP = 100;         // Pose msg maximum upward   blade pitch
 const int PITCH_MSG_CENTER = 50;    // . . .    central          . . .
-const int PITCH_MSG_DOWN = 100;     // . . .    maximum downward . . .
+const int PITCH_MSG_DOWN = 0;     // . . .    maximum downward . . .
 
 // Blade Z Motor PWM Tuning
 const int Z_PWM_MIN = 1000;

src/hind_brain/hind_brain.ino

@@ -13,6 +13,7 @@
 Servo steerServo;
 Servo velServo;
 Servo pitchServo;
+Servo pitchServo2;
 Servo zServo;
 
 // Global Variables
@@ -40,11 +41,15 @@ void setup() {
   steerServo.attach(STEER_SERVO_PIN);
   velServo.attach(VEL_SERVO_PIN);
   pitchServo.attach(PITCH_SERVO_PIN);
+  pitchServo2.attach(PITCH_SERVO2_PIN);
   zServo.attach(Z_SERVO_PIN, Z_PWM_MIN, Z_PWM_MAX);
   pinMode(ESTOP_LED_PIN, OUTPUT);
   pinMode(ESTOP_BUTTON_PIN, OUTPUT);
   attachInterrupt(digitalPinToInterrupt(ESTOP_BUTTON_PIN), onEStopPress, RISING);
 
+  // Set blade to init pose
+  updateBladeState(zCmd, pitchCmd);
+
   // Initialize timers
   watchdogTimer = millis();
   ledTimer = millis();
@@ -143,7 +148,7 @@ int parseSerialMsg() {
 	          pitchCmd = msgToCmd(atof(p_char),
                                 PITCH_MSG_DOWN,PITCH_MSG_CENTER,PITCH_MSG_UP,
                                 PITCH_CMD_DOWN,PITCH_CMD_CENTER,PITCH_CMD_UP);
-            break;
+            Serial.println(pitchCmd);            break;
           }
           case 'e': case 'E': {// Estop msg
             const char* e_char = strtok(NULL, "\n");  // Read only val
@@ -184,6 +189,7 @@ void updateBladeState(int z_cmd, int pitch_cmd) {
 
   zServo.write(z_cmd);
   pitchServo.write(pitch_cmd);
+  pitchServo2.write(PITCH_CMD_UP - pitch_cmd);
 
 }
 
@@ -245,7 +251,7 @@ float msgToCmd(float m_in, float m_low, float m_mid, float m_hi, float c_low, fl
     else if (c_out < c_low) c_out = c_low;
   } else if (c_low > c_hi) {
     if      (c_out > c_low) c_out = c_low;
-    else if (c_out < c_hi) c_out = c_hi;
+    else if (c_out < c_hi)  c_out = c_hi;
   }
 
   return c_out;
@@ -266,6 +272,14 @@ float cmdToMsg(float c_in, float c_low, float c_mid, float c_hi, float m_low, fl
     else m_out = m_mid;
   }
 
+  // Threshold outgoing msg
+  if (m_low < m_hi) {
+    if      (m_out < m_low) m_out = m_low;
+    else if (m_out > m_hi)  m_out = m_hi;
+  } else if (m_low > m_hi) {
+    if      (m_out > m_low) m_out = m_low;
+    else if (m_out < m_hi)  m_out = m_hi;
+  }
   return m_out;
 }