Got both marionette and arm working. all that is left is some physics…

… tweaking
yanatan16 · Oct 16, 2011 · 7f96695 · 7f96695
1 parent 3a3b5ec
commit 7f96695
Show file tree

Hide file tree

Showing 92 changed files with 3,558 additions and 163 deletions.
diff --git a/RoboticNaturalUserInterface/MessagingUtilities/bin/Debug/Utilities.dll b/RoboticNaturalUserInterface/MessagingUtilities/bin/Debug/Utilities.dll
diff --git a/RoboticNaturalUserInterface/MessagingUtilities/bin/Debug/Utilities.pdb b/RoboticNaturalUserInterface/MessagingUtilities/bin/Debug/Utilities.pdb
diff --git a/RoboticNaturalUserInterface/MessagingUtilities/obj/Debug/Utilities.dll b/RoboticNaturalUserInterface/MessagingUtilities/obj/Debug/Utilities.dll
diff --git a/RoboticNaturalUserInterface/MessagingUtilities/obj/Debug/Utilities.pdb b/RoboticNaturalUserInterface/MessagingUtilities/obj/Debug/Utilities.pdb
diff --git a/RoboticNaturalUserInterface/RoboNuiTest/RoboticArmServoControllerTest.cs b/RoboticNaturalUserInterface/RoboNuiTest/RoboticArmServoControllerTest.cs
@@ -51,10 +51,11 @@ public TestContext TestContext
         //}
         //
         //Use TestInitialize to run code before running each test
-        //[TestInitialize()]
-        //public void MyTestInitialize()
-        //{
-        //}
+        [TestInitialize()]
+        public void MyTestInitialize()
+        {
+            Assert.IsNotNull(target);
+        }
         //
         //Use TestCleanup to run code after each test has run
         //[TestCleanup()]
@@ -64,49 +65,34 @@ public TestContext TestContext
         //
         #endregion
 
+        static string defaultPortName = "COM5";
+        static Dictionary<RoboticAngle, uint> defaultChannelMap;
+        static ulong defaultSpeed;
+
+        static RoboticArmServoController target;
+
         //Use ClassInitialize to run code before running the first test in the class
         [ClassInitialize()]
         public static void MyClassInitialize(TestContext tc)
         {
             BasicConfigurator.Configure();
-        }
-
 
-        //Use ClassInitialize to run code before running the first test in the class
-        [TestInitialize()]
-        public void MyTestInitialize()
-        {
             defaultChannelMap = new Dictionary<RoboticAngle, uint>();
             RoboticAngle[] angles = { RoboticAngle.ArmBaseRotate, RoboticAngle.ArmElbowBend, RoboticAngle.ArmHandGrasp, 
                                        RoboticAngle.ArmShoulderLift, RoboticAngle.ArmWristRotate, RoboticAngle.ArmWristTilt };
             uint[] channels = { 0, 1, 2, 3, 4, 5 };
             for (uint i = 0; i < angles.Length; i++)
                 defaultChannelMap.Add(angles[i], channels[i]);
-            defaultSpeed = 1;
-        }
-
-        string defaultPortName = "COM3";
-        Dictionary<RoboticAngle, uint> defaultChannelMap;
-        ulong defaultSpeed;
-
-        /// <summary>
-        ///A test for RoboticArmServoController Constructor
-        ///</summary>
-        [TestMethod()]
-        public void RoboticArmServoControllerConstructorTest()
-        {
-            RoboticArmServoController target = new RoboticArmServoController(defaultPortName, defaultChannelMap, defaultSpeed);
-            Assert.IsNotNull(target);
+            defaultSpeed = 1000;
+            target = new RoboticArmServoController(defaultPortName, defaultChannelMap, defaultSpeed);
         }
 
         /// <summary>
         ///A test for GetAngles
         ///</summary>
-        [TestMethod()]
+//        [TestMethod()]
         public void StartGetAnglesTest()
         {
-            RoboticArmServoController target = new RoboticArmServoController(defaultPortName, defaultChannelMap, defaultSpeed);
-
             List<RoboticAngle> roboticAngleList = new List<RoboticAngle>(defaultChannelMap.Keys);
             AngleSet expected = new AngleSet();
             for (int i = 0; i < roboticAngleList.Count; i++)
@@ -125,8 +111,6 @@ public void StartGetAnglesTest()
         [DeploymentItem("RoboticNaturalUserInterface.exe")]
         public void UpdateTest()
         {
-            RoboticArmServoController target = new RoboticArmServoController(defaultPortName, defaultChannelMap, defaultSpeed);
-
             List<RoboticAngle> roboticAngleList = new List<RoboticAngle>(defaultChannelMap.Keys);
             AngleSet requested = new AngleSet();
             for (int i = 0; i < roboticAngleList.Count; i++)
@@ -140,11 +124,9 @@ public void UpdateTest()
         /// <summary>
         ///A test for IsMovementFinished
         ///</summary>
-        [TestMethod()]
+//        [TestMethod()]
         public void IsMovementFinishedTest()
         {
-            RoboticArmServoController target = new RoboticArmServoController(defaultPortName, defaultChannelMap, 10);
-
             List<RoboticAngle> roboticAngleList = new List<RoboticAngle>(defaultChannelMap.Keys);
             AngleSet requested = new AngleSet(); 
             for (int i = 0; i < roboticAngleList.Count; i++)

diff --git a/RoboticNaturalUserInterface/RoboNuiTest/bin/Debug/RoboNuiTest.dll b/RoboticNaturalUserInterface/RoboNuiTest/bin/Debug/RoboNuiTest.dll
diff --git a/RoboticNaturalUserInterface/RoboNuiTest/bin/Debug/RoboNuiTest.pdb b/RoboticNaturalUserInterface/RoboNuiTest/bin/Debug/RoboNuiTest.pdb
diff --git a/RoboticNaturalUserInterface/RoboNuiTest/bin/Debug/RoboticNaturalUserInterface.exe b/RoboticNaturalUserInterface/RoboNuiTest/bin/Debug/RoboticNaturalUserInterface.exe
diff --git a/RoboticNaturalUserInterface/RoboNuiTest/bin/Debug/RoboticNaturalUserInterface.pdb b/RoboticNaturalUserInterface/RoboNuiTest/bin/Debug/RoboticNaturalUserInterface.pdb
diff --git a/RoboticNaturalUserInterface/RoboNuiTest/bin/Debug/Utilities.dll b/RoboticNaturalUserInterface/RoboNuiTest/bin/Debug/Utilities.dll
diff --git a/RoboticNaturalUserInterface/RoboNuiTest/bin/Debug/Utilities.pdb b/RoboticNaturalUserInterface/RoboNuiTest/bin/Debug/Utilities.pdb
diff --git a/RoboticNaturalUserInterface/RoboNuiTest/bin/Release/RoboNuiTest.dll b/RoboticNaturalUserInterface/RoboNuiTest/bin/Release/RoboNuiTest.dll
diff --git a/RoboticNaturalUserInterface/RoboNuiTest/bin/Release/RoboNuiTest.pdb b/RoboticNaturalUserInterface/RoboNuiTest/bin/Release/RoboNuiTest.pdb
diff --git a/RoboticNaturalUserInterface/RoboNuiTest/bin/Release/RoboticNaturalUserInterface.exe b/RoboticNaturalUserInterface/RoboNuiTest/bin/Release/RoboticNaturalUserInterface.exe
diff --git a/RoboticNaturalUserInterface/RoboNuiTest/bin/Release/RoboticNaturalUserInterface.pdb b/RoboticNaturalUserInterface/RoboNuiTest/bin/Release/RoboticNaturalUserInterface.pdb
diff --git a/RoboticNaturalUserInterface/RoboNuiTest/obj/Debug/ResolveAssemblyReference.cache b/RoboticNaturalUserInterface/RoboNuiTest/obj/Debug/ResolveAssemblyReference.cache
diff --git a/RoboticNaturalUserInterface/RoboNuiTest/obj/Debug/RoboNuiTest.dll b/RoboticNaturalUserInterface/RoboNuiTest/obj/Debug/RoboNuiTest.dll
diff --git a/RoboticNaturalUserInterface/RoboNuiTest/obj/Debug/RoboNuiTest.pdb b/RoboticNaturalUserInterface/RoboNuiTest/obj/Debug/RoboNuiTest.pdb
diff --git a/RoboticNaturalUserInterface/RoboNuiTest/obj/Release/ResolveAssemblyReference.cache b/RoboticNaturalUserInterface/RoboNuiTest/obj/Release/ResolveAssemblyReference.cache
diff --git a/RoboticNaturalUserInterface/RoboNuiTest/obj/Release/RoboNuiTest.dll b/RoboticNaturalUserInterface/RoboNuiTest/obj/Release/RoboNuiTest.dll
diff --git a/RoboticNaturalUserInterface/RoboNuiTest/obj/Release/RoboNuiTest.pdb b/RoboticNaturalUserInterface/RoboNuiTest/obj/Release/RoboNuiTest.pdb
diff --git a/RoboticNaturalUserInterface/RoboticNaturalUserInterface.suo b/RoboticNaturalUserInterface/RoboticNaturalUserInterface.suo
diff --git a/RoboticNaturalUserInterface/RoboticNaturalUserInterface/Core/Position3d.cs b/RoboticNaturalUserInterface/RoboticNaturalUserInterface/Core/Position3d.cs
@@ -5,6 +5,9 @@
 
 namespace RoboNui.Core
 {
+
+    public enum Dimension { X, Y, Z }
+
     /**
      * <summary>
      * 3 Dimensional position
@@ -173,13 +176,14 @@ public Position3d(double r, double phi, double theta, bool unused)
          * Default Constructor
          * </summary>
          */
-        public Position3d()
+        public Position3d() : this(0,0,0)
         {
-            x = 0;
-            y = 0;
-            z = 0;
         }
 
+        public Position3d(Position3d v)
+            : this(v.x, v.y, v.z)
+        { }
+
         /**
          * <summary>
          * Constructor with rectangular coordinates

diff --git a/RoboticNaturalUserInterface/RoboticNaturalUserInterface/Management/StateConfiguration.cs b/RoboticNaturalUserInterface/RoboticNaturalUserInterface/Management/StateConfiguration.cs
@@ -32,6 +32,8 @@ public class StateConfiguration
         public struct _RobotAdapterConfig
         {
 
+            public int UseArm;
+
             /**
              * <summary>
              * Configuration sub-struct for the Robotic Arm component(s).
@@ -56,6 +58,11 @@ public struct _Arm
              */
             public struct _Marionette
             {
+                /// <summary>Serial Port name of the Robotic Arm's Servo Controller</summary>
+                public string Port;
+
+                /// <summary>Robotic Angle to Channel Number list</summary>
+                public Dictionary<RoboticAngle, uint> Channels;
             }
 
             /**
@@ -117,11 +124,19 @@ public StateConfiguration()
             RobotAdapter.Arm.Speed = 0;
 
             RobotAdapter.Arm.Channels[RoboticAngle.ArmBaseRotate] = 0;
-            RobotAdapter.Arm.Channels[RoboticAngle.ArmElbowBend] = 1;
-            RobotAdapter.Arm.Channels[RoboticAngle.ArmHandGrasp] = 2;
-            RobotAdapter.Arm.Channels[RoboticAngle.ArmShoulderLift] = 3;
+            RobotAdapter.Arm.Channels[RoboticAngle.ArmShoulderLift] = 1;
+            RobotAdapter.Arm.Channels[RoboticAngle.ArmElbowBend] = 2;
+            RobotAdapter.Arm.Channels[RoboticAngle.ArmWristTilt] = 3;
             RobotAdapter.Arm.Channels[RoboticAngle.ArmWristRotate] = 4;
-            RobotAdapter.Arm.Channels[RoboticAngle.ArmWristTilt] = 5;
+            RobotAdapter.Arm.Channels[RoboticAngle.ArmHandGrasp] = 5;
+
+            RobotAdapter.Marionette.Channels = new Dictionary<RoboticAngle, uint>();
+            RobotAdapter.Marionette.Port = "";
+            RobotAdapter.Marionette.Channels[RoboticAngle.CurtainOpen] = 6;
+            RobotAdapter.Marionette.Channels[RoboticAngle.RightArmLift] = 1;
+            RobotAdapter.Marionette.Channels[RoboticAngle.LeftArmLift] = 7;
+            RobotAdapter.Marionette.Channels[RoboticAngle.RearLift] = 3;
+            RobotAdapter.Marionette.Channels[RoboticAngle.HeadLift] = 5;
         }
 
         /**
@@ -139,6 +154,7 @@ public static StateConfiguration ReadConfigFile(string fn)
 
             StateConfiguration realSc = new StateConfiguration();
             sc.RobotAdapter.Arm.Channels = realSc.RobotAdapter.Arm.Channels;
+            sc.RobotAdapter.Marionette.Channels = realSc.RobotAdapter.Marionette.Channels;
             return sc;
         }
     }

diff --git a/RoboticNaturalUserInterface/RoboticNaturalUserInterface/Management/StateManager.cs b/RoboticNaturalUserInterface/RoboticNaturalUserInterface/Management/StateManager.cs
@@ -273,8 +273,8 @@ public void Initialize()
             sjm = new SkeletalJointMonitor(runtimeNui);
 
             // Robot Adapter
-
             rsc_arm = new RoboticArmServoController(config.RobotAdapter.Arm.Port, config.RobotAdapter.Arm.Channels, config.RobotAdapter.Arm.Speed);
+            rsc_mar = new RoboticMarionetteServoController(config.RobotAdapter.Marionette.Port, config.RobotAdapter.Marionette.Channels);
 
             Startup();
             log.Info("Default System configuration Initialized.");
@@ -289,18 +289,24 @@ public void Initialize()
          */
         public void Startup()
         {
-            // Set up SJM-JAT-RSC/Arm path
-            RoboticArmModel ram = new RoboticArmModel();
+            IRoboticModel irm;
+            if (config.RobotAdapter.UseArm == 1)
+            {
+                // Set up SJM-JAT-RSC/Arm path
+                irm = new RoboticArmModel();
+                CurrentAngleConsumer = rsc_arm;
+            } else {
+                irm = new RoboticMarionetteModel();
+                CurrentAngleConsumer = rsc_mar;
+            }
 
-            sjm.InterestedJoints = ram.NeededJoints;
+            sjm.InterestedJoints = irm.NeededJoints;
             sjm.Period = config.KinectAdapter.Period;
             CurrentJointProvider = sjm;
 
-            jat.Model = new RoboticArmModel();
+            jat.Model = irm;
             CurrentJointConsumer = jat;
             CurrentAngleProvider = jat;
-
-            CurrentAngleConsumer = rsc_arm;
 
             // And...GO!
             Active = true;

diff --git a/RoboticNaturalUserInterface/RoboticNaturalUserInterface/Management/robonui.json b/RoboticNaturalUserInterface/RoboticNaturalUserInterface/Management/robonui.json
@@ -4,15 +4,16 @@
 {
 	"RobotAdapter" : 
 	{
+		"UseArm" : 0,
 		"Arm" :
 		{
-			"Port" : "COM5",
+			"Port" : "COM3",
 			"Speed" : 1000
 		},
 
 		"Marionette" :
 		{
-
+			"Port" : "COM5"
 		},
 	},
 

diff --git a/...turalUserInterface/RoboticNaturalUserInterface/RobotAdapter/MiniSSC-II/ServoController.cs b/...turalUserInterface/RoboticNaturalUserInterface/RobotAdapter/MiniSSC-II/ServoController.cs
@@ -0,0 +1,126 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.IO.Ports;
+using System.IO;
+
+using log4net;
+
+// TODO Implement
+
+namespace RoboNui.RobotAdapter.MiniSSC2
+{
+    /**
+     * <summary>
+     * Base class for communicating commands to an SSC-32 servo controller
+     * </summary>
+     * <remarks>Author: Jon Eisen (yanatan16@gmail.com)</remarks>
+     */
+    class MiniSSCIIServoController
+    {
+        /**
+         * <summary>Log for logging events in this class</summary>
+         */
+        private ILog log;
+
+        /**
+         * <summary>
+         * Serial port this controller communicates on
+         * </summary>
+         */
+        private SerialPort port;
+
+        bool inactive;
+
+        ICollection<uint> activeChannels;
+
+        /**
+         * <summary>
+         * Constructor with port name and open the port.
+         * Also initializes servo channels to center
+         * </summary>
+         * 
+         * <param name="portName">Name of the serial port</param>
+         * <param name="channels">Channels to center on construction</param>
+         */
+        protected MiniSSCIIServoController(string portName, ICollection<uint> channels)
+        {
+            log = LogManager.GetLogger(this.GetType());
+            log.Debug(this.ToString() + " constructed.");
+
+            activeChannels = channels;
+            try
+            {
+                port = new SerialPort(portName);
+                port.Open();
+                port.BaudRate = 9600;
+                port.NewLine = string.Empty + Convert.ToChar(13);
+                port.Handshake = System.IO.Ports.Handshake.None;
+                port.BaseStream.Flush();
+
+                port.ReadTimeout = 1000;
+                inactive = false;
+
+                foreach (uint ch in channels)
+                {
+                    ServoMovementCommand smc = new ServoMovementCommand(ch, 128);
+                    sendCommand(smc);
+                }
+                log.Info("Initiating all servos to center.");
+            }
+            catch (IOException ex)
+            {
+                log.Error("Could not open Servo Controller Port on " + portName, ex);
+                inactive = true;
+            }
+
+        }
+
+        /**
+         * <summary>
+         * Destructor of the class. 
+         * Closes the serial port.
+         * </summary>
+         */
+        ~MiniSSCIIServoController()
+        {
+            if (activeChannels != null)
+            {
+                foreach (uint ch in activeChannels)
+                {
+                    ServoMovementCommand smc = new ServoMovementCommand(ch, 128);
+                    sendCommand(smc);
+                }
+            }
+            if (port != null)
+            {
+                port.Close();
+            }
+        }
+
+        /**
+         * <summary>
+         * Send a command to the servo controller
+         * </summary>
+         * 
+         * <param name="com">Command to send</param>
+         */
+        protected void sendCommand(ServoMovementCommand com)
+        {
+            string str = "Send Command 0x";
+            foreach (byte b in com.CommandString())
+            {
+                str += string.Format("{0:00x}", b);
+            }
+            log.Debug(str);
+
+            if (!inactive)
+            {
+                IEnumerable<byte> command = com.CommandString();
+                port.Write(command.ToArray(), 0, command.Count());
+            }
+        }
+
+    }
+}