Initial Commit

Sources/Swift2dCar/Servo.swift

@@ -0,0 +1,113 @@
+//
+//  Servo.swift
+//
+//  Simple wrappers for PwmOutput I'll create as I go along, to match gpioZero's implementation I'm currently using.
+//  See gpiozero code here: https://github.com/gpiozero/gpiozero/blob/master/gpiozero/output_devices.py
+//
+//  Created by Michael Pivato on 9/5/20.
+//
+
+import Foundation
+import SwiftyGPIO
+
+enum ServoError: Error{
+    case invalidMinPulseWidth
+    case invalidMaxPulseWidth
+}
+
+protocol Servo {
+    var value: Float {get set}
+    
+    var minPulseWidth: Float {get}
+    var maxPulseWidth: Float {get}
+    var pulseWidth: Float {get}
+    
+    func min()
+    func mid()
+    func max()
+    func detach()
+}
+
+public class PWMHardwareServo : Servo{
+    private (set) var frameWidth : Float
+    private var minDc: Float
+    private var dcRange: Float
+    private var minValue: Float
+    private var valueRange: Float
+    private var internalValue: Float
+    
+    // TODO: Use a factory to provide the correct pwm rather than passing the pin in.
+    private var device: PWMOutput
+    private var currentPulseWIdth: Float
+    
+    var minPulseWidth: Float {
+        get{
+            return minDc * frameWidth
+        }
+    }
+    
+    var maxPulseWidth: Float{
+        get{
+            return (dcRange * frameWidth) + minPulseWidth
+        }
+    }
+    
+    var pulseWidth: Float{
+        get{
+            return internalValue * frameWidth
+        }
+    }
+    
+    var value: Float{
+        get{
+            internalValue
+        }
+        set(newValue){
+            internalValue = newValue
+            device.startPWM(period: Int(frameWidth),
+                            // Multiply by 100 to get into percentage. I don't like that...
+                duty: minDc + dcRange * ((internalValue - minValue) / valueRange) * 100)
+        }
+    }
+
+    
+    init?(forPin pin: PWMOutput, startingAt initialValue: Float = 0, withMinPulseWidth minPulseWidth: Float = 1000000,
+          withMaxPulseWidth maxPulseWidth: Float = 2000000, withFrameWidth frameWidth: Float = 20000000) throws {
+        if(minPulseWidth >= maxPulseWidth){
+            throw ServoError.invalidMinPulseWidth
+        }
+        
+        if(maxPulseWidth >= frameWidth){
+            throw ServoError.invalidMaxPulseWidth
+        }
+        self.frameWidth = frameWidth
+        self.minDc = minPulseWidth / frameWidth
+        self.dcRange = (maxPulseWidth - minPulseWidth) / frameWidth
+        self.minValue = -1
+        self.valueRange = 2
+        
+        self.currentPulseWIdth = 0
+        
+        // Initialise pin immediately.
+        self.device = pin
+        self.device.initPWM()
+        self.internalValue = initialValue
+        self.device.startPWM(period: Int(self.frameWidth), duty: self.minDc + self.dcRange * ((internalValue - self.minValue) / self.valueRange) * 100)
+    }
+    
+    func min(){
+        self.value = -1
+    }
+    
+    func mid(){
+        self.value = 0
+    }
+    
+    func max() {
+        self.value = 1
+    }
+    
+    func detach(){
+        self.device.stopPWM()
+    }
+}