114 lines
3.0 KiB
Swift
114 lines
3.0 KiB
Swift
//
|
|
// 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
|
|
|
|
public enum ServoError: Error{
|
|
case invalidMinPulseWidth
|
|
case invalidMaxPulseWidth
|
|
}
|
|
|
|
public 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()
|
|
}
|
|
|
|
open 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
|
|
|
|
public var minPulseWidth: Float {
|
|
get{
|
|
return minDc * frameWidth
|
|
}
|
|
}
|
|
|
|
public var maxPulseWidth: Float{
|
|
get{
|
|
return (dcRange * frameWidth) + minPulseWidth
|
|
}
|
|
}
|
|
|
|
public var pulseWidth: Float{
|
|
get{
|
|
return internalValue * frameWidth
|
|
}
|
|
}
|
|
|
|
public 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)
|
|
}
|
|
}
|
|
|
|
|
|
public 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)
|
|
}
|
|
|
|
public func min(){
|
|
self.value = -1
|
|
}
|
|
|
|
public func mid(){
|
|
self.value = 0
|
|
}
|
|
|
|
public func max() {
|
|
self.value = 1
|
|
}
|
|
|
|
public func detach(){
|
|
self.device.stopPWM()
|
|
}
|
|
}
|