From c1f09627eb4ea7dc14b365e323bb3a32807ba97d Mon Sep 17 00:00:00 2001 From: Piv <18462828+Piv200@users.noreply.github.com> Date: Sun, 5 Jul 2020 19:59:36 +0930 Subject: [PATCH] Add required methods, core method implementation without exceptions. Took a bit to figure out how swift arrays/Data works to simplify things down... --- Sources/SwiftRPLidar/SwiftRPLidar.swift | 222 ++++++++++++++++++++---- 1 file changed, 191 insertions(+), 31 deletions(-) diff --git a/Sources/SwiftRPLidar/SwiftRPLidar.swift b/Sources/SwiftRPLidar/SwiftRPLidar.swift index 0c53d76..e95614d 100644 --- a/Sources/SwiftRPLidar/SwiftRPLidar.swift +++ b/Sources/SwiftRPLidar/SwiftRPLidar.swift @@ -1,29 +1,30 @@ -import foundation +import Foundation +import SwiftSerial struct Constants{ - static let SYNC = 0xA5 - static let SYNC2 = 0x5A - - static let GET_INFO = 0x50 - static let GET_HEALTH = 0x52 - - static let STOP = 0x25 - static let RESET = 0x40 - - static let SCAN = 0x20 - static let FORCE_SCAN = 0x21 - + static let SYNC: UInt8 = 0xA5 + static let SYNC2: UInt8 = 0x5A + + static let GET_INFO: UInt8 = 0x50 + static let GET_HEALTH: UInt8 = 0x52 + + static let STOP: UInt8 = 0x25 + static let RESET: UInt8 = 0x40 + + static let SCAN: UInt8 = 0x20 + static let FORCE_SCAN: UInt8 = 0x21 + static let DESCRIPTOR_LEN = 7 static let INFO_LEN = 20 static let HEALTH_LEN = 3 - - static let INFO_TYPE = 4 - static let HEALTH_TYPE = 6 - static let SCAN_TYPE = 129 - + + static let INFO_TYPE: UInt8 = 4 + static let HEALTH_TYPE: UInt8 = 6 + static let SCAN_TYPE: UInt8 = 129 + + static let SET_PWM_BYTE: UInt8 = 0xF0 static let MAX_MOTOR_PWM = 1023 static let DEFAULT_MOTOR_PWM = 660 - static let SET_PWM_BYTE = 0xF0 } enum HEALTH_STATUSES { @@ -34,18 +35,177 @@ struct SwiftRPLidar { var text = "Hello, World!" } +func processScan(raw: Data){ + +} + +struct LidarScan{ + var quality: Float + var angle: Float + var distance: Float +} + + + +typealias MeasurementHandler = (_ scan: LidarScan) -> Void +typealias ScanHandler = (_ scans: [LidarScan]) -> Void + class RPLidar{ - private var serialPort: String - private var timeout - private var motor - private var baudrate - - init?(port: String, baudrate: Int = 115200, timeout: Int = 1){ - self.serialPort = port - self.baudrate = baudrate - self.timeout = timeout + private var motor: Bool = false + private var serialPort: SerialPort? = nil + private var motorRunning = false + + + init(serialPort: SerialPort) throws { + self.serialPort = serialPort } - - func connect(){ + + deinit { + if(serialPort != nil){ + serialPort?.closePort() + } } -} \ No newline at end of file + + public func connect() throws { + disconnect() + try serialPort!.openPort() + } + + public func disconnect(){ + if(serialPort != nil){ + // Need to close, SwiftSerial is blocking. + serialPort!.closePort() + } + } + + public func startMotor() throws{ + // A1 + serialPort?.dtr = true + // A2 + try self.setPwm(Constants.DEFAULT_MOTOR_PWM) + self.motorRunning = true + } + + public func stopMotor() throws{ + try setPwm(0) + serialPort?.dtr = false + motorRunning = false + } + + public func setPwm(_ pwm: Int) throws{ + assert(0 <= pwm && pwm <= Constants.MAX_MOTOR_PWM) + try self.sendPayloadCommand(Constants.SET_PWM_BYTE, payload: pwm.asData()) + } + + public func getInfo() throws -> (UInt8, UInt8, UInt8, String){ + try sendCommand(Data([Constants.GET_HEALTH])) + let (dataSize, isSingle, dataType) = try readDescriptor()! + if (dataSize != Constants.INFO_LEN){ + + } + if(!isSingle){ + + } + if(dataType != Constants.INFO_TYPE){ + + } + + let raw = try readResponse(dataSize) + let serialNumber = String(bytes: raw[4...], encoding: .ascii)! + return (raw[0], raw[2], raw[3], serialNumber) + } + + public func getHealth(){ + + } + + public func clearInput(){ + + } + + public func stop(){ + + } + + public func reset(){ + + } + + public func iterMeasurements(maxBufferMeasurements: Int = 500, _ onMeasure: MeasurementHandler) { + + } + + public func iterScans(maxBufferMeasurements: Int = 500, minLength: Int = 5, _ onScan: ScanHandler){ + + } + + private func sendPayloadCommand(_ cmd: Int, payload: Data) throws{ + let size = UInt8(payload.count) + var req = Constants.SYNC.asData() + cmd.asData() + size.asData() + payload + let checksum = calcChecksum(req) + req += checksum.asData() + try serialPort?.writeData(req) + } + + private func sendPayloadCommand(_ cmd: UInt8, payload: Data) throws { + return try sendPayloadCommand(Int(cmd), payload: payload) + } + + private func calcChecksum(_ data: Data) -> Int{ + var checksum = 0; + data.forEach{ body in + checksum ^= Int(body) + } + return checksum + } + + private func sendCommand(_ command: Data) throws{ + var req = Constants.SYNC.asData() + req.append(command) + try serialPort?.writeData(req) + } + + private func readDescriptor() throws -> (UInt8, Bool, UInt8)?{ + guard let descriptor = try serialPort?.readData(ofLength: Constants.DESCRIPTOR_LEN) else { + return nil + } + if (descriptor.count != Constants.DESCRIPTOR_LEN){ + + } + else if (descriptor[0...1] != Data([Constants.SYNC, Constants.SYNC2])){ + + } + let isSingle = descriptor[descriptor.count - 2] == 0 + return (descriptor[2], isSingle, descriptor[descriptor.count - 1]) + } + + private func readResponse(_ dataSize: Int) throws -> Data { + guard let data = try serialPort?.readData(ofLength: dataSize) else{ + return Data() + } + if(data.count != dataSize){ + + } + return data + } + + private func readResponse(_ dataSize: UInt8) throws -> Data { + return try readResponse(Int(dataSize)) + } + + +} + +extension Int{ + func asData() -> Data { + var int = self + return Data(bytes: &int, count: int / Int(UINT8_MAX)) + } +} + +extension UInt8 { + func asData() -> Data { + return Data([self]) + } +} +