Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP

Comparing changes

Choose two branches to see what's changed or to start a new pull request. If you need to, you can also compare across forks.

Open a pull request

Create a new pull request by comparing changes across two branches. If you need to, you can also compare across forks.
base fork: veggielane/Robot
base: 36187e7d6d
...
head fork: veggielane/Robot
compare: 486717e12a
Checking mergeability… Don't worry, you can still create the pull request.
  • 2 commits
  • 32 files changed
  • 0 commit comments
  • 1 contributor
Showing with 632 additions and 157 deletions.
  1. +1 −1  Robot.Core/Maths/Angle.cs
  2. +12 −3 Robot.Core/Maths/Matrix4.cs
  3. +34 −0 Robot.Micro.Core.Inject_old/Class1.cs
  4. +36 −0 Robot.Micro.Core.Inject_old/Properties/AssemblyInfo.cs
  5. +54 −0 Robot.Micro.Core.Inject_old/Robot.Micro.Core.Inject.csproj
  6. +1 −1  Robot.Micro.Core.Test/Program.cs
  7. +38 −6 Robot.Micro.Core.Test/TestsKinematics.cs
  8. +2 −2 Robot.Micro.Core.Test/TestsMatrix4.cs
  9. +32 −0 Robot.Micro.Core/Devices/Bluetooth.cs
  10. +2 −2 Robot.Micro.Core/Devices/CommunicationChannels/Bluetooth.cs
  11. +5 −2 Robot.Micro.Core/Devices/ExtendedSerialPort.cs
  12. +1 −1  Robot.Micro.Core/Devices/SSC32.cs
  13. +3 −5 Robot.Micro.Core/IRobot.cs
  14. +29 −0 Robot.Micro.Core/Inject/Kernel.cs
  15. +9 −5 Robot.Micro.Core/Kinematics/Body.cs
  16. +2 −0  Robot.Micro.Core/Kinematics/IBody.cs
  17. +106 −17 Robot.Micro.Core/Kinematics/Leg3DOF.cs
  18. +12 −0 Robot.Micro.Core/Kinematics/Leg4DOF.cs
  19. +27 −0 Robot.Micro.Core/Messaging/Gateways/BluetoothGateway.cs
  20. +14 −0 Robot.Micro.Core/Messaging/Gateways/Gateway.cs
  21. +11 −0 Robot.Micro.Core/Messaging/Gateways/IGateway.cs
  22. +1 −0  Robot.Micro.Core/Messaging/IMessage.cs
  23. +15 −3 Robot.Micro.Core/Messaging/MessageBus.cs
  24. +2 −0  Robot.Micro.Core/Messaging/Messages/BaseMessage.cs
  25. +15 −0 Robot.Micro.Core/Messaging/Messages/RemoteMessage.cs
  26. +11 −3 Robot.Micro.Core/Robot.Micro.Core.csproj
  27. +24 −0 Robot.Micro.Core/Serialisation/ASCIISerialiser.cs
  28. +15 −0 Robot.Micro.Core/Serialisation/BinarySerialiser.cs
  29. +15 −0 Robot.Micro.Core/Serialisation/ISerialiser.cs
  30. +10 −0 Robot.Micro.Core/Serialisation/Serialiser.cs
  31. +87 −30 Robot.Micro.Limpy/Limpy.cs
  32. +6 −76 Robot.Micro.Limpy/Program.cs
View
2  Robot.Core/Maths/Angle.cs
@@ -39,7 +39,7 @@ public static Angle FromDegrees(double degrees)
public override string ToString()
{
- return Degrees + " Degrees";
+ return "Angle<" + Degrees + "\x00B0>";
}
public static Angle Zero
View
15 Robot.Core/Maths/Matrix4.cs
@@ -28,14 +28,20 @@ public Matrix4(Double[][] data)
}
}
- public static Matrix4 Identity()
+ public Double X { get { return this[1, 4]; } set { this[1, 4] = value; } }
+ public Double Y { get { return this[2, 4]; } set { this[2, 4] = value; } }
+ public Double Z { get { return this[3, 4]; } set { this[3, 4] = value; } }
+
+
+ public static Matrix4 Identity
{
- return new Matrix4(new[]{
+ get{
+ return new Matrix4(new[]{
new[]{ 1.0, 0.0, 0.0, 0.0 },
new[]{ 0.0, 1.0, 0.0, 0.0 },
new[]{ 0.0, 0.0, 1.0, 0.0 },
new[]{ 0.0, 0.0, 0.0, 1.0 }
- });
+ });}
}
public double Mag()
@@ -132,6 +138,9 @@ public static Matrix4 Translate(Double x, Double y, Double z)
});
}
+
+
+
public static Matrix4 Rotate(Axis axis, Angle theta)
{
switch (axis)
View
34 Robot.Micro.Core.Inject_old/Class1.cs
@@ -0,0 +1,34 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Linq;
+using System.Reflection;
+using System.Text;
+
+namespace Robot.Micro.Core.Inject
+{
+ public class Kernal : IKernel
+ {
+ public object Get(Type T)
+ {
+ var constructor = T.GetConstructor(null);
+
+ if (constructor != null)
+ {
+ var paraminfo = constructor.GetParameters();
+ foreach (ParameterInfo parameterInfo in paraminfo)
+ {
+ Debug.Write(parameterInfo.Name);
+ }
+ }
+
+ return 4;
+ }
+ }
+
+ public interface IKernel
+ {
+ object Get(Type T);
+ }
+}
+
View
36 Robot.Micro.Core.Inject_old/Properties/AssemblyInfo.cs
@@ -0,0 +1,36 @@
+using System.Reflection;
+using System.Runtime.CompilerServices;
+using System.Runtime.InteropServices;
+
+// General Information about an assembly is controlled through the following
+// set of attributes. Change these attribute values to modify the information
+// associated with an assembly.
+[assembly: AssemblyTitle("Robot.Micro.Core.Inject")]
+[assembly: AssemblyDescription("")]
+[assembly: AssemblyConfiguration("")]
+[assembly: AssemblyCompany("")]
+[assembly: AssemblyProduct("Robot.Micro.Core.Inject")]
+[assembly: AssemblyCopyright("Copyright © 2012")]
+[assembly: AssemblyTrademark("")]
+[assembly: AssemblyCulture("")]
+
+// Setting ComVisible to false makes the types in this assembly not visible
+// to COM components. If you need to access a type in this assembly from
+// COM, set the ComVisible attribute to true on that type.
+[assembly: ComVisible(false)]
+
+// The following GUID is for the ID of the typelib if this project is exposed to COM
+[assembly: Guid("1fbad7bc-be0e-4c02-b327-9dfa671ccac5")]
+
+// Version information for an assembly consists of the following four values:
+//
+// Major Version
+// Minor Version
+// Build Number
+// Revision
+//
+// You can specify all the values or you can default the Build and Revision Numbers
+// by using the '*' as shown below:
+// [assembly: AssemblyVersion("1.0.*")]
+[assembly: AssemblyVersion("1.0.0.0")]
+[assembly: AssemblyFileVersion("1.0.0.0")]
View
54 Robot.Micro.Core.Inject_old/Robot.Micro.Core.Inject.csproj
@@ -0,0 +1,54 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project ToolsVersion="4.0" DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+ <PropertyGroup>
+ <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
+ <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
+ <ProductVersion>8.0.30703</ProductVersion>
+ <SchemaVersion>2.0</SchemaVersion>
+ <ProjectGuid>{2FF1B64D-B9EC-4D89-B3A6-7217E520C685}</ProjectGuid>
+ <OutputType>Library</OutputType>
+ <AppDesignerFolder>Properties</AppDesignerFolder>
+ <RootNamespace>Robot.Micro.Core.Inject</RootNamespace>
+ <AssemblyName>Robot.Micro.Core.Inject</AssemblyName>
+ <TargetFrameworkVersion>v4.0</TargetFrameworkVersion>
+ <FileAlignment>512</FileAlignment>
+ </PropertyGroup>
+ <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
+ <DebugSymbols>true</DebugSymbols>
+ <DebugType>full</DebugType>
+ <Optimize>false</Optimize>
+ <OutputPath>bin\Debug\</OutputPath>
+ <DefineConstants>DEBUG;TRACE</DefineConstants>
+ <ErrorReport>prompt</ErrorReport>
+ <WarningLevel>4</WarningLevel>
+ </PropertyGroup>
+ <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
+ <DebugType>pdbonly</DebugType>
+ <Optimize>true</Optimize>
+ <OutputPath>bin\Release\</OutputPath>
+ <DefineConstants>TRACE</DefineConstants>
+ <ErrorReport>prompt</ErrorReport>
+ <WarningLevel>4</WarningLevel>
+ </PropertyGroup>
+ <ItemGroup>
+ <Reference Include="System" />
+ <Reference Include="System.Core" />
+ <Reference Include="System.Xml.Linq" />
+ <Reference Include="System.Data.DataSetExtensions" />
+ <Reference Include="Microsoft.CSharp" />
+ <Reference Include="System.Data" />
+ <Reference Include="System.Xml" />
+ </ItemGroup>
+ <ItemGroup>
+ <Compile Include="Class1.cs" />
+ <Compile Include="Properties\AssemblyInfo.cs" />
+ </ItemGroup>
+ <Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
+ <!-- To modify your build process, add your task inside one of the targets below and uncomment it.
+ Other similar extension points exist, see Microsoft.Common.targets.
+ <Target Name="BeforeBuild">
+ </Target>
+ <Target Name="AfterBuild">
+ </Target>
+ -->
+</Project>
View
2  Robot.Micro.Core.Test/Program.cs
@@ -27,7 +27,7 @@ public static void Main()
foreach (DictionaryEntry entry in ht)
{
Exception exception = null;
- Debug.Print("---------------------------");
+ Debug.Print("--------------------------");
Debug.Print("Running: " + entry.Key);
try
{
View
44 Robot.Micro.Core.Test/TestsKinematics.cs
@@ -7,18 +7,50 @@ namespace Robot.Micro.Core.Test
{
public class TestsKinematics
{
- public void Test3DOF()
+ public void Test3DOF1()
{
- var body = new Body();
+ var body = new Body{Position = Matrix4.Identity};
var leg = new Leg3DOF(body)
{
- BasePosition = Matrix4.Identity(),
- CoxaLength = 0.0,
+ BasePosition = Matrix4.Translate(0.0, 0.0, 0.0),
+ CoxaLength = 2.0,
FemurLength = 10.0,
TibiaLength = 5.0,
- FootPosition = new Vect3(10.0,0.0,-5.0).ToMatrix4(),
+ TibiaOffset = Angle.FromDegrees(-90.0),
+ FootPosition = Matrix4.Translate(12.0, 0.0, -5.0),
};
- leg.Inverse();
+
+ Assert.NearlyEquals(leg.CoxaServo.Angle.Degrees,0.0);
+ Assert.NearlyEquals(leg.FemurServo.Angle.Degrees, 0.0);
+ Assert.NearlyEquals(leg.TibiaServo.Angle.Degrees, 0.0);
+
+
+ body.Position *= Matrix4.Translate(0.0, 0.0, 1.0);
+ body.Position *= Matrix4.Translate(0.0, 0.0, 1.0);
+ body.Position *= Matrix4.Translate(0.0, 0.0, 1.0);
+ body.Position *= Matrix4.Translate(0.0, 0.0, 1.0);
+
+ }
+ public void Test3DOF2()
+ {
+ var body = new Body { Position = Matrix4.Identity };
+ var leg = new Leg3DOF(body)
+ {
+ BasePosition = Matrix4.Translate(0.0, 0.0, 0.0),
+ CoxaLength = 2.0,
+ FemurLength = 10.0,
+ TibiaLength = 5.0,
+ TibiaOffset = Angle.FromDegrees(-90.0),
+ FootPosition = Matrix4.Translate(17.0, 0.0, 0.0),
+ };
+
+ Assert.NearlyEquals(leg.CoxaServo.Angle.Degrees, 0.0);
+ Assert.NearlyEquals(leg.FemurServo.Angle.Degrees, 0.0);
+ Assert.NearlyEquals(leg.TibiaServo.Angle.Degrees, -90.0);
+
+
+
+
}
}
}
View
4 Robot.Micro.Core.Test/TestsMatrix4.cs
@@ -7,12 +7,12 @@ public class TestsMatrix4
{
public void TestMatrix4Equality()
{
- Assert.IsTrue(Matrix4.Identity() == Matrix4.Identity(), "Equality Fail");
+ Assert.IsTrue(Matrix4.Identity == Matrix4.Identity, "Equality Fail");
}
public void TestMatrix4Identity()
{
- Assert.IsTrue(Matrix4.Identity() == new Matrix4(new[]{
+ Assert.IsTrue(Matrix4.Identity== new Matrix4(new[]{
new[]{ 1.0, 0.0, 0.0, 0.0 },
new[]{ 0.0, 1.0, 0.0, 0.0 },
new[]{ 0.0, 0.0, 1.0, 0.0 },
View
32 Robot.Micro.Core/Devices/Bluetooth.cs
@@ -0,0 +1,32 @@
+using System;
+using Robot.Micro.Core.Messaging;
+using Robot.Micro.Core.Messaging.Messages;
+
+namespace Robot.Micro.Core.Devices
+{
+ public class Bluetooth : ExtendedSerialPort
+ {
+ //public event Message ReceiveMessage;
+ public Bluetooth(String port, int baudRate)
+ : base(port, baudRate)
+ {
+
+ /*
+ DataReceived += (sender, e) =>
+ {
+ if (e != null && e.EventType == SerialData.Chars && BytesToRead > 0 && ReceiveMessage != null)
+ ReceiveMessage(ParseMessage(Read()));
+ };*/
+ }
+
+ private IMessage ParseMessage(string data)
+ {
+ return new RemoteMessage{ Msg = data };
+ }
+ /*
+ public void SendMessage(IMessage message)
+ {
+ Write(_json.Encode(message));
+ }*/
+ }
+}
View
4 Robot.Micro.Core/Devices/CommunicationChannels/Bluetooth.cs
@@ -12,7 +12,7 @@ public class Bluetooth : ExtendedSerialPort, IChannel
{
public event Message ReceiveMessage;
- private JSON _json = new JSON();
+ private readonly JSON _json = new JSON();
public Bluetooth(String port, int baudRate)
: base(port, baudRate)
{
@@ -26,7 +26,7 @@ public Bluetooth(String port, int baudRate)
private IMessage ParseMessage(string data)
{
- return new DebugMessage{Msg = "Bluetooth Data:"+ data};
+ return new RemoteMessage{ Msg = data };
}
public void SendMessage(IMessage message)
View
7 Robot.Micro.Core/Devices/ExtendedSerialPort.cs
@@ -40,11 +40,14 @@ public ExtendedSerialPort(string portName, int baudRate, Parity parity, int data
public virtual bool Connect()
{
Open();
- if (PortName == "COM4" && IsOpen == true) RemapCOM4();
+ //if (PortName == "COM4" && IsOpen == true) RemapCOM4();
return IsOpen;
}
-
+ public void Write(Byte[] data)
+ {
+ Write(data, 0, data.Length);
+ }
public void Write(String data)
{
var buffer = Encoding.UTF8.GetBytes(data);
View
2  Robot.Micro.Core/Devices/SSC32.cs
@@ -65,7 +65,7 @@ public override bool Connect()
{
base.Connect();
Reset();
- if (Version.IndexOf("SSC32-V2.03XE") != -1)
+ if (Version != null && Version.IndexOf("SSC32-V2.03XE") != -1)
{
DataReceived += Target;
return true;
View
8 Robot.Micro.Core/IRobot.cs
@@ -1,17 +1,15 @@
using System;
-using Microsoft.SPOT;
-using Robot.Micro.Core.Devices.CommunicationChannels;
using Robot.Micro.Core.Messaging;
using Robot.Micro.Core.Timing;
namespace Robot.Micro.Core
{
- public interface IRobot
+ public interface IRobot:IDisposable
{
- MessageBus Bus { get; }
- CommunicationChannels Channels { get; }
+ IMessageBus Bus { get; }
ITimer Timer { get; }
bool IsRunning { get; }
void Run();
+ void Stop();
}
}
View
29 Robot.Micro.Core/Inject/Kernel.cs
@@ -0,0 +1,29 @@
+using System;
+using System.Reflection;
+using Microsoft.SPOT;
+
+namespace Robot.Micro.Core.Inject
+{
+
+ //cant get constuction parameters
+ //ConstructorInfo.GetParameters
+ public class Kernal : IKernel
+ {
+ public object Get(Type T)
+ {
+ ConstructorInfo constructor = T.GetConstructor(null);
+
+ if (constructor != null)
+ {
+ //var paraminfo = constructor.
+ }
+
+ return 4;
+ }
+ }
+
+ public interface IKernel
+ {
+ object Get(Type T);
+ }
+}
View
14 Robot.Micro.Core/Kinematics/Body.cs
@@ -1,15 +1,19 @@
-using System;
-using Microsoft.SPOT;
using Robot.Micro.Core.Maths;
namespace Robot.Micro.Core.Kinematics
{
- public class Body :IBody
+ public class Body:IBody
{
+ private Matrix4 _position;
public Matrix4 Position
{
- get { throw new NotImplementedException(); }
- set { throw new NotImplementedException(); }
+ get { return _position; }
+ set
+ {
+ _position = value;
+ if (PositionChanged != null) PositionChanged();
+ }
}
+ public event EventMatrix4 PositionChanged;
}
}
View
2  Robot.Micro.Core/Kinematics/IBody.cs
@@ -7,5 +7,7 @@ namespace Robot.Micro.Core.Kinematics
public interface IBody
{
Matrix4 Position { get; set; }
+ event EventMatrix4 PositionChanged;
}
+ public delegate void EventMatrix4();
}
View
123 Robot.Micro.Core/Kinematics/Leg3DOF.cs
@@ -7,54 +7,73 @@ namespace Robot.Micro.Core.Kinematics
{
public class Leg3DOF : ILeg
{
- private IBody _body;
+ private readonly IBody _body;
private Matrix4 _basePosition;
+ private Matrix4 _footPosition;
- public Matrix4 BasePosition { get; set; }
+ public Matrix4 BasePosition
+ {
+ get { return _body.Position * _basePosition; }
+ set { _basePosition = value; }
+ }
- private Matrix4 _footPosition;
public Matrix4 FootPosition
{
get
{
+ Forward();
return _footPosition;
}
set
{
_footPosition = value;
+ Inverse();
}
}
- public Servo CoxaServo = new Servo()
+ public readonly Servo CoxaServo = new Servo()
{
- Min = Angle.FromDegrees(-45),
- Max = Angle.FromDegrees(45),
+ Min = Angle.FromDegrees(-90),
+ Max = Angle.FromDegrees(90),
Angle = 0.0
};
- public Servo FemurServo = new Servo()
+ public readonly Servo FemurServo = new Servo()
{
- Min = Angle.FromDegrees(-45),
- Max = Angle.FromDegrees(45),
+ Min = Angle.FromDegrees(-90),
+ Max = Angle.FromDegrees(90),
Angle = 0.0
};
- public Servo TibiaServo = new Servo()
+ public readonly Servo TibiaServo = new Servo()
{
- Min = Angle.FromDegrees(-45),
- Max = Angle.FromDegrees(45),
+ Min = Angle.FromDegrees(-90),
+ Max = Angle.FromDegrees(90),
Angle = 0.0
};
public double CoxaLength { get; set; }
public double FemurLength { get; set; }
public double TibiaLength { get; set; }
+
+ public Angle CoxaOffset { get; set; }
+ public Angle FemurOffset { get; set; }
+ public Angle TibiaOffset { get; set; }
+
+ public bool CoxaInvert = false;
+ public bool FemurInvert = false;
+ public bool TibiaInvert = false;
public Leg3DOF(IBody body)
{
_body = body;
- _basePosition = Matrix4.Identity();
- _footPosition = Matrix4.Identity();
+ _body.PositionChanged += Inverse;
+ _basePosition = Matrix4.Identity;
+ _footPosition = Matrix4.Identity;
+
+ CoxaOffset = 0.0;
+ FemurOffset = 0.0;
+ TibiaOffset = 0.0;
}
public void Forward()
@@ -64,11 +83,81 @@ public void Forward()
public void Inverse()
{
- CoxaServo.Angle = MathsHelper.Atan2(_footPosition[2, 4] - _basePosition[2, 4], _footPosition[2, 1] - _basePosition[2, 1]);
+ /*
+ * Todo:
+ * - Elbow Up + Elbow Down
+ * - Reduce Calcs
+ *
+ * Jazar Pg. 331
+ */
+ var angle1 = MathsHelper.Atan2(_footPosition.Y - BasePosition.Y, _footPosition.X - BasePosition.X);
+ var bd = _footPosition.ToVector3() - (BasePosition * Matrix4.RotateZ(CoxaServo.Angle) * Matrix4.Translate(CoxaLength, 0, 0)).ToVector3();
+ //Debug.Print(bd.ToString());
+ var angle3 = 2 * MathsHelper.Atan2(
+ MathsHelper.Sqrt(MathsHelper.Pow(FemurLength + TibiaLength, 2) - (MathsHelper.Pow(bd.X, 2) + MathsHelper.Pow(bd.Z, 2))),
+ MathsHelper.Sqrt(MathsHelper.Pow(bd.X, 2) + MathsHelper.Pow(bd.Z, 2) - MathsHelper.Pow(FemurLength - TibiaLength, 2))
+ );
+ var angle2 = MathsHelper.Atan2(bd.Z, bd.X) + MathsHelper.Atan2(TibiaLength * MathsHelper.Sin(angle3), FemurLength + TibiaLength * MathsHelper.Cos(angle3));
+
+
+
+ CoxaServo.Angle = (CoxaInvert ? -1 : 1) * (angle1 + CoxaOffset);
+ FemurServo.Angle = (FemurInvert ? -1 : 1) * (angle2 + FemurOffset);
+ TibiaServo.Angle = (TibiaInvert ? -1 : 1) * (angle3 + TibiaOffset);
+
+ Debug.Print(CoxaServo.Angle.ToString());
+ Debug.Print(FemurServo.Angle.ToString());
+ Debug.Print(TibiaServo.Angle.ToString());
+ /**
+ *
+ Angle Theta1;
+ Angle Theta2;
+ Double E = _footPosition[1, 4] - _basePosition[1, 4];
+ Double D = _footPosition[3, 4] - _basePosition[3, 4];
+
+ Theta2 = 2*MathsHelper.Atan2(
+ MathsHelper.Sqrt(MathsHelper.Pow(FemurLength + TibiaLength, 2) - (MathsHelper.Pow(E, 2) + MathsHelper.Pow(D, 2))),
+ MathsHelper.Sqrt(MathsHelper.Pow(E, 2) + MathsHelper.Pow(D, 2) - MathsHelper.Pow(FemurLength - TibiaLength, 2))
+ );
+ Debug.Print(Theta2.ToString());
+ //Theta1 =
+
+ Theta1 = MathsHelper.Atan2(D, E) + MathsHelper.Atan2(TibiaLength * MathsHelper.Sin(Theta2), FemurLength + TibiaLength * MathsHelper.Cos(Theta2));
+ Debug.Print(Theta1.ToString());
+
+
+
+ //Hip H
+ Debug.Print(_footPosition.ToString());
+ Debug.Print(_basePosition.ToString());
+ CoxaServo.Angle = MathsHelper.Atan2(_footPosition[2, 4] - _basePosition[2, 4], _footPosition[1, 4] - _basePosition[1, 4]);
+ Debug.Print(CoxaServo.Angle.ToString());
+
+ Debug.Print("A:");
+ Vect3 A = _basePosition.ToVector3();
+ Debug.Print(A.ToString());
+ Debug.Print("B:");
+ Vect3 B = (_basePosition * Matrix4.RotateZ(CoxaServo.Angle) * Matrix4.Translate(CoxaLength, 0, 0)).ToVector3();
+ Debug.Print(B.ToString());
+ Debug.Print(B.Length.ToString());
+
+
+ Vect3 D = _footPosition.ToVector3();
+ Debug.Print("D:");
+ Debug.Print(D.ToString());
+
+ Vect3 BD = D - B;
+
- // FemurServo.Angle = MathsHelper.Atan2(BtoD.Pz, MathsHelper.Sqrt(MathsHelper.Pow(BtoD.Px, 2.0) + MathsHelper.Pow(BtoD.Py, 2.0))) + MathsHelper.Acos((MathsHelper.Pow(L1, 2.0) - MathsHelper.Pow(L2, 2.0) + MathsHelper.Pow(BtoD.Mag(), 2.0)) / (2 * L1 * BtoD.Mag()));
+ //Hip V
+ FemurServo.Angle = MathsHelper.Atan2(BD.Z, MathsHelper.Sqrt(MathsHelper.Pow(BD.X, 2.0) + MathsHelper.Pow(BD.Y, 2.0))) + MathsHelper.Acos((MathsHelper.Pow(FemurLength, 2.0) - MathsHelper.Pow(TibiaLength, 2.0) + MathsHelper.Pow(BD.Length, 2.0)) / (2 * FemurLength * BD.Length));
+ Debug.Print(FemurServo.Angle.ToString());
+ //Double q1 =
- TibiaServo.Angle = 0.0;
+ //
+ // Knee
+ TibiaServo.Angle = MathsHelper.Acos((MathsHelper.Pow(FemurLength, 2.0) + MathsHelper.Pow(TibiaLength, 2.0) - MathsHelper.Pow(BD.Length, 2.0)) / (2 * FemurLength * TibiaLength));
+ Debug.Print(TibiaServo.Angle.ToString());*/
}
}
}
View
12 Robot.Micro.Core/Kinematics/Leg4DOF.cs
@@ -0,0 +1,12 @@
+using System;
+using Microsoft.SPOT;
+
+namespace Robot.Micro.Core.Kinematics
+{
+ public class Leg4DOF:Leg3DOF
+ {
+ public Leg4DOF(IBody body) : base(body)
+ {
+ }
+ }
+}
View
27 Robot.Micro.Core/Messaging/Gateways/BluetoothGateway.cs
@@ -0,0 +1,27 @@
+using System;
+using System.IO.Ports;
+using Microsoft.SPOT;
+using Robot.Micro.Core.Devices;
+using Robot.Micro.Core.Reactive;
+using Robot.Micro.Core.Serialisation;
+
+namespace Robot.Micro.Core.Messaging.Gateways
+{
+ public class BluetoothGateway:Gateway
+ {
+ private readonly Bluetooth _device;
+ public BluetoothGateway(Bluetooth device, ISerialiser serialiser)
+ {
+ _device = device;
+ serialiser.Message += Add;
+ this.Where(obj => ((IMessage)obj).Remote).Subscribe(obj => _device.Write(serialiser.Serialise(obj as IMessage)));
+ _device.DataReceived += (sender, e) =>
+ {
+ if (e == null || e.EventType != SerialData.Chars || _device.BytesToRead <= 0) return;
+ var receiveBuffer = new byte[_device.BytesToRead];
+ _device.Read(receiveBuffer, 0, receiveBuffer.Length);
+ serialiser.Deserialise(receiveBuffer);
+ };
+ }
+ }
+}
View
14 Robot.Micro.Core/Messaging/Gateways/Gateway.cs
@@ -0,0 +1,14 @@
+using System;
+using Microsoft.SPOT;
+using Robot.Micro.Core.Reactive;
+
+namespace Robot.Micro.Core.Messaging.Gateways
+{
+ public abstract class Gateway : Observable, IGateway
+ {
+ public void Add(IMessage message)
+ {
+ OnNext(message);
+ }
+ }
+}
View
11 Robot.Micro.Core/Messaging/Gateways/IGateway.cs
@@ -0,0 +1,11 @@
+using System;
+using Microsoft.SPOT;
+using Robot.Micro.Core.Reactive;
+
+namespace Robot.Micro.Core.Messaging.Gateways
+{
+ public interface IGateway : IObservable
+ {
+ void Add(IMessage message);
+ }
+}
View
1  Robot.Micro.Core/Messaging/IMessage.cs
@@ -7,5 +7,6 @@ namespace Robot.Micro.Core.Messaging
public interface IMessage
{
DateTime Time { get; }
+ bool Remote { get; }
}
}
View
18 Robot.Micro.Core/Messaging/MessageBus.cs
@@ -1,13 +1,25 @@
-using System;
-using Microsoft.SPOT;
+using Robot.Micro.Core.Messaging.Gateways;
using Robot.Micro.Core.Reactive;
namespace Robot.Micro.Core.Messaging
{
- public class MessageBus : Observable
+ public interface IMessageBus : IObservable
+ {
+ void Add(IMessage message);
+ void AddGateway(IGateway gateway);
+
+ }
+
+ public class MessageBus : Observable, IMessageBus
{
public void Add(IMessage message)
{
OnNext(message);
}
+
+ public void AddGateway(IGateway gateway)
+ {
+ gateway.Subscribe(obj => Add(obj as IMessage));
+ this.Where(obj => ((IMessage)obj).Remote).Subscribe(obj => gateway.Add(obj as IMessage));
+ }
}
}
View
2  Robot.Micro.Core/Messaging/Messages/BaseMessage.cs
@@ -7,8 +7,10 @@ namespace Robot.Micro.Core.Messaging.Messages
public abstract class BaseMessage : IMessage
{
public DateTime Time { get; set; }
+ public bool Remote { get; set; }
protected BaseMessage()
{
+ Remote = false;
Time = DateTime.Now;
}
}
View
15 Robot.Micro.Core/Messaging/Messages/RemoteMessage.cs
@@ -0,0 +1,15 @@
+namespace Robot.Micro.Core.Messaging.Messages
+{
+ public class RemoteMessage : BaseMessage
+ {
+ public string Msg { get; set; }
+ public RemoteMessage()
+ {
+ Msg = "";
+ }
+ public override string ToString()
+ {
+ return Msg;
+ }
+ }
+}
View
14 Robot.Micro.Core/Robot.Micro.Core.csproj
@@ -56,26 +56,30 @@
</Compile>
<Compile Include="Devices\ExtendedSerialPort.cs" />
<Compile Include="Devices\PushButton.cs" />
- <Compile Include="Devices\CommunicationChannels\Bluetooth.cs" />
- <Compile Include="Devices\CommunicationChannels\CommunicationChannels.cs" />
- <Compile Include="Devices\CommunicationChannels\IChannel.cs" />
+ <Compile Include="Devices\Bluetooth.cs" />
<Compile Include="Devices\Displays\SerLED.cs" />
<Compile Include="Devices\LED.cs" />
<Compile Include="Devices\Sensors\DS18B20.cs" />
<Compile Include="Devices\Servo.cs" />
<Compile Include="Environment.cs" />
+ <Compile Include="Inject\Kernel.cs" />
<Compile Include="IRobot.cs" />
<Compile Include="Kinematics\Body.cs" />
<Compile Include="Kinematics\Leg3DOF.cs" />
<Compile Include="Kinematics\IBody.cs" />
<Compile Include="Kinematics\ILeg.cs" />
+ <Compile Include="Kinematics\Leg4DOF.cs" />
<Compile Include="Linq\Delegates.cs" />
<Compile Include="Linq\Filter.cs" />
<Compile Include="Linq\Extensions.cs" />
<Compile Include="Linq\ExtenstionAtrribute.cs" />
+ <Compile Include="Messaging\Gateways\BluetoothGateway.cs" />
+ <Compile Include="Messaging\Gateways\Gateway.cs" />
+ <Compile Include="Messaging\Gateways\IGateway.cs" />
<Compile Include="Messaging\IMessage.cs" />
<Compile Include="Messaging\MessageBus.cs" />
<Compile Include="Messaging\Messages\BaseMessage.cs" />
+ <Compile Include="Messaging\Messages\RemoteMessage.cs" />
<Compile Include="Messaging\Messages\DebugMessage.cs" />
<Compile Include="Messaging\Messages\RobotReadyMessage.cs" />
<Compile Include="Properties\AssemblyInfo.cs" />
@@ -88,7 +92,11 @@
<Compile Include="Reactive\ObservableExtensions.cs" />
<Compile Include="Reactive\Observer.cs" />
<Compile Include="Sensors\ISensor.cs" />
+ <Compile Include="Serialisation\BinarySerialiser.cs" />
+ <Compile Include="Serialisation\ASCIISerialiser.cs" />
+ <Compile Include="Serialisation\ISerialiser.cs" />
<Compile Include="Serialisation\JSON.cs" />
+ <Compile Include="Serialisation\Serialiser.cs" />
<Compile Include="Threading\Task.cs" />
<Compile Include="Timing\AsyncObservableTimer.cs" />
<Compile Include="Timing\Extensions.cs" />
View
24 Robot.Micro.Core/Serialisation/ASCIISerialiser.cs
@@ -0,0 +1,24 @@
+using System;
+using System.Text;
+using Microsoft.SPOT;
+using Robot.Micro.Core.Messaging;
+
+namespace Robot.Micro.Core.Serialisation
+{
+ public class ASCIISerialiser : ISerialiser
+ {
+ public event MessageDelegate Message;
+
+ private string _buffer = "";
+
+ public void Deserialise(byte[] data)
+ {
+
+ }
+
+ public byte[] Serialise(IMessage message)
+ {
+ return Encoding.UTF8.GetBytes(message.ToString());
+ }
+ }
+}
View
15 Robot.Micro.Core/Serialisation/BinarySerialiser.cs
@@ -0,0 +1,15 @@
+using System;
+using Microsoft.SPOT;
+
+namespace Robot.Micro.Core.Serialisation
+{
+ public class BinarySerialiser:ISerialiser
+ {
+ public event MessageDelegate Message;
+
+ public void Deserialise(byte[] data)
+ {
+
+ }
+ }
+}
View
15 Robot.Micro.Core/Serialisation/ISerialiser.cs
@@ -0,0 +1,15 @@
+using System;
+using Microsoft.SPOT;
+using Robot.Micro.Core.Messaging;
+
+namespace Robot.Micro.Core.Serialisation
+{
+ public delegate void MessageDelegate(IMessage message);
+
+ public interface ISerialiser
+ {
+ event MessageDelegate Message;
+ void Deserialise(Byte[] data);
+ Byte[] Serialise(IMessage message);
+ }
+}
View
10 Robot.Micro.Core/Serialisation/Serialiser.cs
@@ -0,0 +1,10 @@
+using System;
+using Microsoft.SPOT;
+
+namespace Robot.Micro.Core.Serialisation
+{
+ public abstract class Serialiser
+ {
+
+ }
+}
View
117 Robot.Micro.Limpy/Limpy.cs
@@ -3,10 +3,13 @@
using Microsoft.SPOT.Hardware;
using Robot.Micro.Core;
using Robot.Micro.Core.Devices;
-using Robot.Micro.Core.Devices.CommunicationChannels;
+
using Robot.Micro.Core.Maths;
using Robot.Micro.Core.Messaging;
+using Robot.Micro.Core.Messaging.Gateways;
using Robot.Micro.Core.Messaging.Messages;
+using Robot.Micro.Core.Reactive;
+using Robot.Micro.Core.Serialisation;
using Robot.Micro.Core.Timing;
using Robot.Micro.Core.Kinematics;
@@ -14,57 +17,72 @@ namespace Robot.Micro.Limpy
{
public class Limpy:IRobot
{
- public MessageBus Bus { get; private set; }
- public CommunicationChannels Channels { get; private set; }
+
+ public IMessageBus Bus { get; private set; }
+
public ITimer Timer { get; private set; }
public bool IsRunning { get; private set; }
//ISensor list
- readonly LED _led = new LED((Cpu.Pin)FEZ_Pin.Digital.LED);
+ readonly LED _led = new LED((Cpu.Pin)FEZ_Pin.Digital.Di13);
readonly PushButton _button = new PushButton((Cpu.Pin)FEZ_Pin.Interrupt.LDR);
- private readonly SSC32 _ssc = new SSC32("COM4", 115200);
+
+ //private readonly SSC32 _ssc = new SSC32("COM3", 115200);
private readonly Bluetooth _bt = new Bluetooth("COM1", 115200);
- readonly Servo _servo = new Servo()
- {
- Min = Angle.FromDegrees(-45),
- Max = Angle.FromDegrees(45),
- Angle = Angle.FromDegrees(0),
- };
-
-
- public Limpy()
+ //kinematics
+ private IBody _body;
+ private ILeg _legLeftFront;
+ private ILeg _legLeftMiddle;
+ private ILeg _legLeftRear;
+
+ private ILeg _legRightFront;
+ private ILeg _legRightMiddle;
+ private ILeg _legRightRear;
+
+
+ public Limpy(IMessageBus bus, ITimer timer)
{
- Bus = new MessageBus();
+ Bus = bus;
+ Bus.AddGateway(new BluetoothGateway(_bt, new BinarySerialiser()));
+ Timer = timer;
Bus.Subscribe(obj => Debug.Print(obj.ToString()));
- Channels = new CommunicationChannels(Bus);
- Timer = new AsyncObservableTimer();
_button.Pressed += (pushButton, state) =>
{
_led.Toggle();
Bus.Add(new RobotReadyMessage());
};
-
-
}
public void Run()
{
- Angle test = Angle.FromRadians(MathsHelper.Pi);
- MathsHelper.Cos(test);
-
Timer.Start();
- Channels.Add(_bt);
- //_ssc.Connect();
+ _body = new Body { Position = Matrix4.Identity };
+ _legLeftFront = new Leg4DOF(_body)
+ {
+ BasePosition = Matrix4.Translate(0.0, 0.0, 0.0),
+ CoxaLength = 35.0,
+ FemurLength = 52.0,
+ FemurInvert = true,
+ TibiaLength = 48.0,
+ TibiaOffset = Angle.FromDegrees(-90.0),
+ TibiaInvert = true,
+ FootPosition = Matrix4.Translate(35.0 +52.0, 0.0, -48.0),
+ };
-
+ //Bus.OfType(typeof(RemoteMessage)).Subscribe(obj => Move(obj as RemoteMessage));
+ //Angle test = Angle.FromRadians(MathsHelper.Pi);
+ //MathsHelper.Cos(test);
- _ssc.AddServo(0, _servo);
- //_servo.Angle = Angle.FromDegrees(90);
- _servo.Angle += Angle.FromDegrees(0);
- //_ssc.Move();
+
+ //_ssc.Connect();
+
+ //_ssc.AddServo(0, _leg.CoxaServo);
+ //_ssc.AddServo(1, _leg.FemurServo);
+ //_ssc.AddServo(2, _leg.TibiaServo);
+ //_ssc.Move();
//setup sensors
@@ -76,13 +94,52 @@ public void Run()
//MainLoop();
}
+ public void Stop()
+ {
+
+ }
+
+ private void Move(RemoteMessage remoteMessage)
+ {
+ switch (remoteMessage.Msg)
+ {
+ case "w":
+ _legLeftFront.FootPosition *= Matrix4.Translate(0.0, 0.0, 2.0);
+ break;
+ case "a":
+ _legLeftFront.FootPosition *= Matrix4.Translate(2.0, 0.0, 0.0);
+ break;
+ case "s":
+ _legLeftFront.FootPosition *= Matrix4.Translate(0.0, 0.0, -2.0);
+ break;
+ case "d":
+ _legLeftFront.FootPosition *= Matrix4.Translate(-2.0, 0.0, 0.0);
+ break;
+ case "q":
+ _legLeftFront.FootPosition *= Matrix4.Translate(0.0, -2.0, 0.0);
+ break;
+ case "e":
+ _legLeftFront.FootPosition *= Matrix4.Translate(0.0, 2.0, 0.0);
+ break;
+ default:
+ break;
+ }
+
+ //_ssc.Move();
+ }
+
+
private void MainLoop()
{
IsRunning = true;
while (IsRunning)
{
-
}
}
+
+ public void Dispose()
+ {
+
+ }
}
}
View
82 Robot.Micro.Limpy/Program.cs
@@ -1,11 +1,7 @@
-using System;
-using System.Threading;
-
+using System.Threading;
using Microsoft.SPOT;
-using Microsoft.SPOT.Hardware;
-
-//using GHIElectronics.NETMF.FEZ;
-
+using Robot.Micro.Core.Messaging;
+using Robot.Micro.Core.Timing;
namespace Robot.Micro.Limpy
{
public class Program
@@ -14,77 +10,11 @@ public static void Main()
{
Debug.GC(true);
Debug.EnableGCMessages(true);
- var robot = new Limpy();
- robot.Run();
- /*
- * var bt = new Bluetooth("COM1", 115200);
- bt.Open();
- bt.Write("Testing" + Convert.ToChar(13));
- bt.Messages.Subscribe(obj => Debug.Print((string)obj));
- Debug.GC(true);
-
- var bt = new Bluetooth("COM1", 115200);
- bt.Open();
- bt.Write("testing");
- bt.Messages.Subscribe(obj => Debug.Print((string)obj));
-
-
-
- var led = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.LED, true);
-
- var bus = new MessageBus();
-
- var timer = new AsyncObservableTimer();
- timer.SubSample(10)Subscribe(obj => led.Write(!led.Read()));
-
- bus.Subscribe(item => Debug.Print("A:" + item.ToString()));
- bus.SubSample(10).Subscribe(new Observer(item => Debug.Print("B:" + item.ToString())));
-
- var i = 0;
-
- timer.Subscribe(new Observer(tick => bus.Add(i++)));
- timer.Start();
- Debug.Print("test");
- Thread.Sleep(100000);
- timer.Stop();
- new Task(() =>
+ using (var robot = new Limpy(new MessageBus(), new AsyncObservableTimer()))
{
- while (true)
- {
- bus.Add("test");
- Thread.Sleep(1000);
- }
-
- }).Start();
- var test = new SSC32("COM1", 115200);
-
- if (!test.Connect()) return;
- test.Execute();
- test.Write("#0 P1500 S750");
- test.Execute();
- test.Write("#1 P1500 S750");
- test.Execute();
- test.Write("#2 P1500 S750");
- test.Execute();
- test.Write("#3 P1500 S750");
- test.Execute();
- test.Write("#4 P1500 S750");
- test.Execute();
- test.Write("#5 P1500 S750");
- test.Execute();
-
- Debug.Print("Connected");
-
- var a = new[] { 1, 2, 3, 4, 6, 8, 9, 9, 9 };
-
- var n = a.Any(item => ((int)item) == 9);
- var m = a.All(item => ((int)item) == 9);
- var t = a.Where(v => (int)v % 2 == 0).Count(o => true);
- Debug.Print(n + " " + m);
- Debug.Print("count " + t);
- */
+ robot.Run();
+ }
Thread.Sleep(Timeout.Infinite);
}
-
}
}

No commit comments for this range

Something went wrong with that request. Please try again.