Merge pull request #93 from huskyroboticsteam/Minibot

Minibot

Rover/Minibot/AdafruitMotor.cs

@@ -0,0 +1,192 @@
+using Scarlet.Components;
+using Scarlet.IO;
+using Scarlet.Utilities;
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading;
+using System.Threading.Tasks;
+
+namespace Minibot
+{
+    public class AdafruitMotor : IMotor
+    {
+        private IPWMOutput Output;
+
+        bool IMotor.TraceLogging { get => throw new NotImplementedException(); set => throw new NotImplementedException(); }
+
+        public AdafruitMotor(IPWMOutput Output)
+        {
+            Output.SetEnabled(true);
+            this.Output = Output;
+        }
+
+        public void EventTriggered(object Sender, EventArgs Event) { throw new NotImplementedException(); }
+
+        public void Stop() { Output.SetEnabled(false); }
+
+        //pre : -100 < speed < 100
+        public void SetSpeed(float speed)
+        {
+            if (Math.Abs(speed) > 100.0)
+            {
+                Log.Output(Log.Severity.DEBUG, Log.Source.MOTORS, "Cannot set Adafruit Motor to Speed larger than 100.0");
+                return;
+            }
+            // Scale output to 0 to 100
+            float NewOutput = (float)(speed + 100.0) / 2;
+            Output.SetOutput(NewOutput);
+        }
+
+        public void SetEnabled(bool enabled) {
+            Output.SetEnabled(enabled);
+        }
+    }
+
+    public class AdafruitMotorPWM : IPWMOutput
+    {
+
+        private II2CBus Bus;
+        private byte Address;
+        private Motors Motor;
+        private int Frequency = 60; // Default to 60
+
+        private bool P_Enabled;
+        private bool Enabled
+        {
+            get { return P_Enabled; }
+            set
+            {
+                if (!value) { Stop(); }
+                P_Enabled = value;
+            }
+        }
+
+        #region REGISTERS AND VALUES
+
+        private const byte PCA9685_MODE1 = 0x00;
+        private const byte PCA9685_MODE2 = 0x01;
+        private const byte PCA9685_PRESCALE = 0xFE;
+
+        private const byte LED0_ON_L = 0x06;
+        private const byte LED0_ON_H = 0x07;
+        private const byte LED0_OFF_L = 0x08;
+        private const byte LED0_OFF_H = 0x09;
+        private const byte ALL_LED_ON_L = 0xFA;
+        private const byte ALL_LED_ON_H = 0xFB;
+        private const byte ALL_LED_OFF_L = 0xFC;
+        private const byte ALL_LED_OFF_H = 0xFD;
+
+        private const byte SLEEP = 0x10;
+        private const byte ALLCALL = 0x01;
+        private const byte OUTDRV = 0x04;
+
+        #endregion
+
+        // Pin mappings for each motor: 0, 1, 2, 3
+        private static readonly int[][] PINS = new int[][] {
+            new int[] { 8, 9, 10 },
+            new int[] { 13, 12, 11 },
+            new int[] { 2, 4, 3 },
+            new int[] { 7, 6, 5 }
+        };
+
+        public enum Motors { Motor1, Motor2, Motor3, Motor4 }
+
+        public AdafruitMotorPWM(Motors Motor, II2CBus Bus, byte Address = 0x60)
+        {
+            this.Bus = Bus;
+            this.Address = Address;
+            this.Motor = Motor;
+
+            SetAllPWM(0, 0);
+            this.Bus.WriteRegister(this.Address, PCA9685_MODE2, new byte[] { OUTDRV });
+            this.Bus.WriteRegister(this.Address, PCA9685_MODE1, new byte[] { ALLCALL });
+            Thread.Sleep(5);
+
+            byte mode1 = this.Bus.ReadRegister(this.Address, PCA9685_MODE1, 1)[0];
+            this.Bus.WriteRegister(this.Address, PCA9685_MODE1, new byte[] { (byte)(mode1 & ~SLEEP) });
+            Thread.Sleep(5);
+
+            SetFrequency(Frequency);
+        }
+
+        public void Dispose() { throw new NotImplementedException(); }
+
+        public void SetEnabled(bool Enable) { Enabled = Enable; }
+
+        public void SetFrequency(float Frequency)
+        {
+            float Prescale = 25000000;
+            Prescale /= 4096;
+            Prescale /= Frequency;
+            Prescale -= 1;
+            byte AdjPrescale = (byte)Math.Floor(Prescale + 0.5);
+            byte OldMode = this.Bus.ReadRegister(Address, PCA9685_MODE1, 1)[0];
+            byte NewMode = (byte)((OldMode & 0x7F) | 0x10);
+
+            this.Bus.WriteRegister(Address, PCA9685_MODE1, new byte[] { NewMode });
+            this.Bus.WriteRegister(Address, PCA9685_PRESCALE, new byte[] { AdjPrescale });
+            this.Bus.WriteRegister(Address, PCA9685_MODE1, new byte[] { OldMode });
+            Thread.Sleep(5);
+
+            this.Bus.WriteRegister(Address, PCA9685_MODE1, new byte[] { (byte)(OldMode | 0x80) });
+
+            this.Frequency = (int) Frequency;
+        }
+
+        private void SetPWM(int Channel, short On, short Off)
+        {
+            this.Bus.WriteRegister(Address, (byte)(LED0_ON_L + 4 * Channel), new byte[] { (byte)(On & 0xFF) });
+            this.Bus.WriteRegister(Address, (byte)(LED0_ON_H + 4 * Channel), new byte[] { (byte)(On >> 8) });
+            this.Bus.WriteRegister(Address, (byte)(LED0_OFF_L + 4 * Channel), new byte[] { (byte)(Off & 0xFF) });
+            this.Bus.WriteRegister(Address, (byte)(LED0_OFF_H + 4 * Channel), new byte[] { (byte)(Off >> 8) });
+        }
+
+        private void SetAllPWM(byte On, byte Off)
+        {
+            this.Bus.WriteRegister(Address, ALL_LED_ON_L, new byte[] { (byte)(On & 0xFF) });
+            this.Bus.WriteRegister(Address, ALL_LED_ON_H, new byte[] { (byte)(On >> 8) });
+            this.Bus.WriteRegister(Address, ALL_LED_OFF_L, new byte[] { (byte)(Off & 0xFF) });
+            this.Bus.WriteRegister(Address, ALL_LED_OFF_H, new byte[] { (byte)(Off >> 8) });
+        }
+
+        private void Stop() { SetOutput(50.0f); }
+
+        // Takes in value -255 (reverse) to 255 (forward)
+        private void SetOutputExactly(int Value)
+        {
+            int ForwardPin = PINS[(int)Motor][1];
+            int ReversePin = PINS[(int)Motor][2];
+            int ThrottlePin = PINS[(int)Motor][0];
+
+            SetPWM(ForwardPin, 4096, 0);
+            SetPWM(ReversePin, 4096, 0);
+            SetPWM(ThrottlePin, (short)(2048 - Math.Abs(Value) * 8), (short)(2048 + Math.Abs(Value) * 8));
+
+            if (Value > 0) { SetPWM(ForwardPin, 0, 4096); }
+            else { SetPWM(ReversePin, 0, 4096); }
+        }
+
+        // 0.0 to 100.0
+        public void SetOutput(float DutyCycle)
+        {
+            if (!Enabled) { return; }
+            // Map 0.0 to 100.0 to -255 to 255
+            int MappedValue = (int)((DutyCycle - 50.0) * 5.1);
+            if (Math.Abs(MappedValue) > 255)
+            {
+                Log.Output(Log.Severity.DEBUG, Log.Source.MOTORS, "Cannot set AdafruitMotorPWM duty cycle to more than 100.0");
+                return;
+            }
+
+            SetOutputExactly(MappedValue);
+        }
+
+        public void SetDelay(float ClockDelay)
+        {
+            throw new NotImplementedException();
+        }
+    }
+}

Rover/Minibot/Minibot.csproj

@@ -0,0 +1,56 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project DefaultTargets="Build" ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <PropertyGroup>
+    <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
+    <Platform Condition=" '$(Platform)' == '' ">x86</Platform>
+    <ProjectGuid>{8FD9411A-B2C7-4C7C-9B0E-1E468B00E686}</ProjectGuid>
+    <OutputType>Exe</OutputType>
+    <RootNamespace>Minibot</RootNamespace>
+    <AssemblyName>Minibot</AssemblyName>
+    <TargetFrameworkVersion>v4.6.1</TargetFrameworkVersion>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|x86' ">
+    <DebugSymbols>true</DebugSymbols>
+    <DebugType>full</DebugType>
+    <Optimize>false</Optimize>
+    <OutputPath>bin\Debug</OutputPath>
+    <DefineConstants>DEBUG;</DefineConstants>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+    <ExternalConsole>true</ExternalConsole>
+    <PlatformTarget>x86</PlatformTarget>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|x86' ">
+    <Optimize>true</Optimize>
+    <OutputPath>bin\Release</OutputPath>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+    <ExternalConsole>true</ExternalConsole>
+    <PlatformTarget>x86</PlatformTarget>
+  </PropertyGroup>
+  <ItemGroup>
+    <Reference Include="BBBCSIO, Version=1.6.0.0, Culture=neutral, processorArchitecture=x86">
+      <HintPath>packages\HuskyRobotics.Scarlet.0.5.1\lib\BBBCSIO.dll</HintPath>
+    </Reference>
+    <Reference Include="OpenTK, Version=3.0.1.0, Culture=neutral, PublicKeyToken=bad199fe84eb3df4, processorArchitecture=MSIL">
+      <HintPath>packages\OpenTK.3.0.1\lib\net20\OpenTK.dll</HintPath>
+    </Reference>
+    <Reference Include="Scarlet, Version=0.5.1.0, Culture=neutral, processorArchitecture=MSIL">
+      <HintPath>packages\HuskyRobotics.Scarlet.0.5.1\lib\Scarlet.dll</HintPath>
+    </Reference>
+    <Reference Include="System" />
+    <Reference Include="System.Half, Version=0.0.0.0, Culture=neutral, processorArchitecture=MSIL">
+      <HintPath>packages\HuskyRobotics.Scarlet.0.5.1\lib\System.Half.dll</HintPath>
+    </Reference>
+  </ItemGroup>
+  <ItemGroup>
+    <Compile Include="Program.cs" />
+    <Compile Include="Properties\AssemblyInfo.cs" />
+    <Compile Include="AdafruitMotor.cs" />
+  </ItemGroup>
+  <ItemGroup>
+    <None Include="OpenTK.dll.config" />
+    <None Include="packages.config" />
+  </ItemGroup>
+  <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />
+</Project>

Rover/Minibot/Minibot.sln

@@ -0,0 +1,17 @@
+
+Microsoft Visual Studio Solution File, Format Version 12.00
+# Visual Studio 2012
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Minibot", "Minibot.csproj", "{8FD9411A-B2C7-4C7C-9B0E-1E468B00E686}"
+EndProject
+Global
+	GlobalSection(SolutionConfigurationPlatforms) = preSolution
+		Debug|x86 = Debug|x86
+		Release|x86 = Release|x86
+	EndGlobalSection
+	GlobalSection(ProjectConfigurationPlatforms) = postSolution
+		{8FD9411A-B2C7-4C7C-9B0E-1E468B00E686}.Debug|x86.ActiveCfg = Debug|x86
+		{8FD9411A-B2C7-4C7C-9B0E-1E468B00E686}.Debug|x86.Build.0 = Debug|x86
+		{8FD9411A-B2C7-4C7C-9B0E-1E468B00E686}.Release|x86.ActiveCfg = Release|x86
+		{8FD9411A-B2C7-4C7C-9B0E-1E468B00E686}.Release|x86.Build.0 = Release|x86
+	EndGlobalSection
+EndGlobal

Rover/Minibot/OpenTK.dll.config

@@ -0,0 +1,25 @@
+<configuration>
+  <dllmap os="linux" dll="opengl32.dll" target="libGL.so.1"/>
+  <dllmap os="linux" dll="glu32.dll" target="libGLU.so.1"/>
+  <dllmap os="linux" dll="openal32.dll" target="libopenal.so.1"/>
+  <dllmap os="linux" dll="alut.dll" target="libalut.so.0"/>
+  <dllmap os="linux" dll="opencl.dll" target="libOpenCL.so"/>
+  <dllmap os="linux" dll="libX11" target="libX11.so.6"/>
+  <dllmap os="linux" dll="libXi" target="libXi.so.6"/>
+  <dllmap os="linux" dll="SDL2.dll" target="libSDL2-2.0.so.0"/>
+  <dllmap os="osx" dll="opengl32.dll" target="/System/Library/Frameworks/OpenGL.framework/OpenGL"/>
+  <dllmap os="osx" dll="openal32.dll" target="/System/Library/Frameworks/OpenAL.framework/OpenAL" />
+  <dllmap os="osx" dll="alut.dll" target="/System/Library/Frameworks/OpenAL.framework/OpenAL" />
+  <dllmap os="osx" dll="libGLES.dll" target="/System/Library/Frameworks/OpenGLES.framework/OpenGLES" />
+  <dllmap os="osx" dll="libGLESv1_CM.dll" target="/System/Library/Frameworks/OpenGLES.framework/OpenGLES" />
+  <dllmap os="osx" dll="libGLESv2.dll" target="/System/Library/Frameworks/OpenGLES.framework/OpenGLES" />
+  <dllmap os="osx" dll="opencl.dll" target="/System/Library/Frameworks/OpenCL.framework/OpenCL"/>
+  <dllmap os="osx" dll="SDL2.dll" target="libSDL2.dylib"/>
+  <!-- XQuartz compatibility (X11 on Mac) -->
+  <dllmap os="osx" dll="libGL.so.1" target="/usr/X11/lib/libGL.dylib"/>
+  <dllmap os="osx" dll="libX11" target="/usr/X11/lib/libX11.dylib"/>
+  <dllmap os="osx" dll="libXcursor.so.1" target="/usr/X11/lib/libXcursor.dylib"/>
+  <dllmap os="osx" dll="libXi" target="/usr/X11/lib/libXi.dylib"/>
+  <dllmap os="osx" dll="libXinerama" target="/usr/X11/lib/libXinerama.dylib"/>
+  <dllmap os="osx" dll="libXrandr.so.2" target="/usr/X11/lib/libXrandr.dylib"/>
+</configuration>