c_meyer/pxt-ev3
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Re-work sensor framework

Browse Source
This commit is contained in:
Michal Moskal 2017-07-10 11:29:42 +01:00
parent b446e0baac
commit 0a1ab0a90f
2 changed files with 503 additions and 195 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

672
libs/core/analog.ts
View File

@ -1,5 +1,111 @@
namespace input {
const enum IrSensorMode {
None = -1,
Proximity = 0,
Seek = 1,
RemoteControl = 2,
}
const enum IrRemoteChannel {
Ch0 = 0, // top
Ch1 = 1,
Ch2 = 2,
Ch3 = 3,
}
const enum IrRemoteButton {
None = 0x00,
CenterBeacon = 0x01,
TopLeft = 0x02,
BottomLeft = 0x04,
TopRight = 0x08,
BottomRight = 0x10,
}
namespace core {
let nextComponentId = 20000;
export class Component {
protected _id: number;
constructor(id = 0) {
if (!id) id = ++nextComponentId
this._id = id
}
getId() {
return this._id;
}
}
}
const enum LMS {
NUM_INPUTS = 4,
LCD_WIDTH = 178,
LCD_HEIGHT = 128,
DEVICE_TYPE_NXT_TOUCH = 1,
DEVICE_TYPE_NXT_LIGHT = 2,
DEVICE_TYPE_NXT_SOUND = 3,
DEVICE_TYPE_NXT_COLOR = 4,
DEVICE_TYPE_TACHO = 7,
DEVICE_TYPE_MINITACHO = 8,
DEVICE_TYPE_NEWTACHO = 9,
DEVICE_TYPE_TOUCH = 16,
DEVICE_TYPE_THIRD_PARTY_START = 50,
DEVICE_TYPE_THIRD_PARTY_END = 99,
DEVICE_TYPE_IIC_UNKNOWN = 100,
DEVICE_TYPE_NXT_TEST = 101,
DEVICE_TYPE_NXT_IIC = 123,
DEVICE_TYPE_TERMINAL = 124,
DEVICE_TYPE_UNKNOWN = 125,
DEVICE_TYPE_NONE = 126,
DEVICE_TYPE_ERROR = 127,
MAX_DEVICE_DATALENGTH = 32,
MAX_DEVICE_MODES = 8,
UART_BUFFER_SIZE = 64,
TYPE_NAME_LENGTH = 11,
SYMBOL_LENGTH = 4,
DEVICE_LOGBUF_SIZE = 300,
IIC_NAME_LENGTH = 8,
CONN_UNKNOWN = 111,
CONN_DAISYCHAIN = 117,
CONN_NXT_COLOR = 118,
CONN_NXT_DUMB = 119,
CONN_NXT_IIC = 120,
CONN_INPUT_DUMB = 121,
CONN_INPUT_UART = 122,
CONN_OUTPUT_DUMB = 123,
CONN_OUTPUT_INTELLIGENT = 124,
CONN_OUTPUT_TACHO = 125,
CONN_NONE = 126,
CONN_ERROR = 127,
opOutputGetType = 0xA0,
opOutputSetType = 0xA1,
opOutputReset = 0xA2,
opOutputStop = 0xA3,
opOutputPower = 0xA4,
opOutputSpeed = 0xA5,
opOutputStart = 0xA6,
opOutputPolarity = 0xA7,
opOutputRead = 0xA8,
opOutputTest = 0xA9,
opOutputReady = 0xAA,
opOutputPosition = 0xAB,
opOutputStepPower = 0xAC,
opOutputTimePower = 0xAD,
opOutputStepSpeed = 0xAE,
opOutputTimeSpeed = 0xAF,
opOutputStepSync = 0xB0,
opOutputTimeSync = 0xB1,
opOutputClearCount = 0xB2,
opOutputGetCount = 0xB3,
opOutputProgramStop = 0xB4,
DEVICE_EVT_ANY = 0,
DEVICE_ID_NOTIFY = 10000,
DEVICE_ID_NOTIFY_ONE = 10001,
}
namespace inputint {
//% shim=pxt::unsafePollForChanges
function unsafePollForChanges(
periodMs: int32,
@ -8,6 +114,300 @@ namespace input {
) { }
let analogMM: MMap
let uartMM: MMap
let devcon: Buffer
let sensors: SensorInfo[]
let autoSensors: Sensor[]
class SensorInfo {
port: number
sensor: Sensor
connType: number
devType: number
manual: boolean
constructor(p: number) {
this.port = p
this.connType = LMS.CONN_NONE
this.devType = LMS.DEVICE_TYPE_NONE
this.sensor = null
this.manual = false
}
}
function init() {
if (sensors) return
sensors = []
for (let i = 0; i < LMS.NUM_INPUTS; ++i) sensors.push(new SensorInfo(i))
autoSensors = []
devcon = output.createBuffer(DevConOff.Size)
analogMM = control.mmap("/dev/lms_analog", AnalogOff.Size, 0)
if (!analogMM) control.fail("no analog sensor")
uartMM = control.mmap("/dev/lms_uart", UartOff.Size, 0)
if (!uartMM) control.fail("no uart sensor")
loops.forever(() => {
detectDevices()
loops.pause(500)
})
for (let info_ of sensors) {
let info = info_
unsafePollForChanges(50, () => {
if (info.sensor) return info.sensor._query()
return 0
}, (prev, curr) => {
if (info.sensor) info.sensor._update(prev, curr)
})
}
}
function readUartInfo(port: number, mode: number) {
let buf = output.createBuffer(UartCtlOff.Size)
buf[UartCtlOff.Port] = port
buf[UartCtlOff.Mode] = mode
uartMM.ioctl(IO.UART_READ_MODE_INFO, buf)
return buf
//let info = `t:${buf[TypesOff.Type]} c:${buf[TypesOff.Connection]} m:${buf[TypesOff.Mode]} n:${buf.slice(0, 12).toHex()}`
//serial.writeLine("UART " + port + " / " + mode + " - " + info)
}
function detectDevices() {
let conns = analogMM.slice(AnalogOff.InConn, LMS.NUM_INPUTS)
let numChanged = 0
for (let info of sensors) {
let newConn = conns[info.port]
if (newConn == info.connType)
continue
numChanged++
info.connType = newConn
info.devType = LMS.DEVICE_TYPE_NONE
if (newConn == LMS.CONN_INPUT_UART) {
setUartMode(info.port, 0)
let uinfo = readUartInfo(info.port, 0)
info.devType = uinfo[TypesOff.Type]
} else if (newConn == LMS.CONN_INPUT_DUMB) {
// TODO? for now assume touch
info.devType = LMS.DEVICE_TYPE_TOUCH
} else if (newConn == LMS.CONN_NONE) {
// OK
} else {
// ???
}
}
if (numChanged == 0)
return
let autos = sensors.filter(s => !s.manual)
// first free up disconnected sensors
for (let info of autos) {
if (info.sensor && info.devType == LMS.DEVICE_TYPE_NONE)
info.sensor._setPort(0)
}
for (let info of autos) {
if (!info.sensor && info.devType != LMS.DEVICE_TYPE_NONE) {
for (let s of autoSensors) {
if (s.getPort() == 0 && s._deviceType() == info.devType) {
s._setPort(info.port + 1)
break
}
}
}
}
}
export class Sensor extends core.Component {
protected port: number
constructor() {
super()
init()
this.port = -1
let tp = this._deviceType()
if (autoSensors.filter(s => s._deviceType() == tp).length == 0) {
autoSensors.push(this)
}
}
// 0 - disable, 1-4 port number
_setPort(port: number, manual = false) {
port = Math.clamp(0, 4, port | 0) - 1;
if (port == this.port) return
for (let i = 0; i < sensors.length; ++i) {
if (i != this.port && sensors[i].sensor == this) {
sensors[i] = null
sensors[i].manual = false
}
}
if (this.port > 0) {
let prev = sensors[this.port].sensor
if (prev && prev != this)
prev._setPort(0)
sensors[this.port].sensor = this
sensors[this.port].manual = manual
}
this._portUpdated()
}
protected _portUpdated() { }
setPort(port: number) {
this._setPort(port, true)
}
getPort() {
return this.port + 1
}
isManual() {
return this.port >= 0 && sensors[this.port].manual
}
_query() {
return 0
}
_update(prev: number, curr: number) {
}
_deviceType() {
return 0
}
}
export class AnalogSensor extends Sensor {
constructor() {
super()
}
_readPin6() {
if (this.port < 0) return 0
return analogMM.getNumber(NumberFormat.Int16LE, AnalogOff.InPin6 + 2 * this.port)
}
}
export class UartSensor extends Sensor {
protected mode: number
constructor() {
super()
}
protected _portUpdated() {
this.mode = -1
if (this.port > 0) {
if (this.isManual()) {
uartReset(this.port)
} else {
this.mode = 0
}
}
}
protected _setMode(m: number) {
if (this.port < 0) return
let v = m | 0
if (v != this.mode) {
this.mode = v
setUartMode(this.port, v)
}
}
getBytes(): Buffer {
return getUartBytes(this.port)
}
getNumber(fmt: NumberFormat, off: number) {
return getUartNumber(fmt, off, this.port)
}
}
function uartReset(port: number) {
if (port < 0) return
devcon.setNumber(NumberFormat.Int8LE, DevConOff.Connection + port, LMS.CONN_NONE)
devcon.setNumber(NumberFormat.Int8LE, DevConOff.Type + port, 0)
devcon.setNumber(NumberFormat.Int8LE, DevConOff.Mode + port, 0)
uartMM.ioctl(IO.UART_SET_CONN, devcon)
}
function getUartStatus(port: number) {
if (port < 0) return 0
return uartMM.getNumber(NumberFormat.Int8LE, UartOff.Status + port)
}
function waitNonZeroUartStatus(port: number) {
while (true) {
if (port < 0) return 0
let s = getUartStatus(port)
if (s) return s
loops.pause(25)
}
}
function uartClearChange(port: number) {
const UART_DATA_READY = 8
const UART_PORT_CHANGED = 1
while (true) {
let status = getUartStatus(port)
if (port < 0) break
if ((status & UART_DATA_READY) != 0 && (status & UART_PORT_CHANGED) == 0)
break
devcon.setNumber(NumberFormat.Int8LE, DevConOff.Connection + port, LMS.CONN_INPUT_UART)
devcon.setNumber(NumberFormat.Int8LE, DevConOff.Type + port, 0)
devcon.setNumber(NumberFormat.Int8LE, DevConOff.Mode + port, 0)
uartMM.ioctl(IO.UART_CLEAR_CHANGED, devcon)
uartMM.setNumber(NumberFormat.Int8LE, UartOff.Status + port,
getUartStatus(port) & 0xfffe)
loops.pause(10)
}
}
function setUartMode(port: number, mode: number) {
const UART_PORT_CHANGED = 1
while (true) {
if (port < 0) return
devcon.setNumber(NumberFormat.Int8LE, DevConOff.Connection + port, LMS.CONN_INPUT_UART)
devcon.setNumber(NumberFormat.Int8LE, DevConOff.Type + port, 33)
devcon.setNumber(NumberFormat.Int8LE, DevConOff.Mode + port, mode)
uartMM.ioctl(IO.UART_SET_CONN, devcon)
let status = waitNonZeroUartStatus(port)
if (status & UART_PORT_CHANGED) {
uartClearChange(port)
} else {
break
}
loops.pause(10)
}
}
function getUartBytes(port: number): Buffer {
if (port < 0) return output.createBuffer(LMS.MAX_DEVICE_DATALENGTH)
let index = uartMM.getNumber(NumberFormat.UInt16LE, UartOff.Actual + port * 2)
return uartMM.slice(
UartOff.Raw + LMS.MAX_DEVICE_DATALENGTH * 300 * port + LMS.MAX_DEVICE_DATALENGTH * index,
LMS.MAX_DEVICE_DATALENGTH)
}
function getUartNumber(fmt: NumberFormat, off: number, port: number) {
if (port < 0) return 0
let index = uartMM.getNumber(NumberFormat.UInt16LE, UartOff.Actual + port * 2)
return uartMM.getNumber(fmt,
UartOff.Raw + LMS.MAX_DEVICE_DATALENGTH * 300 * port + LMS.MAX_DEVICE_DATALENGTH * index + off)
}
const enum NxtColOff {
Calibration = 0, // uint32[4][3]
@ -41,104 +441,6 @@ namespace input {
Size = 5172
}
function init() {
if (analogMM) return
analogMM = control.mmap("/dev/lms_analog", AnalogOff.Size, 0)
if (!analogMM) control.fail("no analog sensor")
}
export function portState() {
init()
return analogMM.slice(AnalogOff.Updated, 12)
}
export class AnalogSensor {
protected port: number
protected id: number
protected getPin6() {
return readAnalogPin6(this.port)
}
constructor(port: number) {
this.port = Math.clamp(1, 4, port | 0) - 1;
this.id = 200 + port;
init()
}
}
export class TouchSensor extends AnalogSensor {
private downTime: number;
constructor(port: number) {
super(port)
unsafePollForChanges(50,
() => this.isPressed() ? 1 : 0,
(prev, curr) => {
if (prev == curr) return
if (curr) {
this.downTime = control.millis()
control.raiseEvent(this.id, ButtonEvent.Down)
} else {
control.raiseEvent(this.id, ButtonEvent.Up)
let delta = control.millis() - this.downTime
control.raiseEvent(this.id, delta > 500 ? ButtonEvent.LongClick : ButtonEvent.Click)
}
})
}
isPressed() {
return this.getPin6() > 2500
}
/**
* Do something when a touch sensor is clicked, double clicked, etc...
* @param button the button that needs to be clicked or used
* @param event the kind of button gesture that needs to be detected
* @param body code to run when the event is raised
*/
onEvent(ev: ButtonEvent, body: () => void) {
control.onEvent(this.id, ev, body)
}
}
function readAnalogPin6(port: number) {
init()
port--
port = Math.clamp(0, 3, port | 0)
return analogMM.getNumber(NumberFormat.Int16LE, AnalogOff.InPin6 + 2 * port)
}
}
const enum IrSensorMode {
None = -1,
Proximity = 0,
Seek = 1,
RemoteControl = 2,
}
const enum IrRemoteChannel {
Ch0 = 0, // top
Ch1 = 1,
Ch2 = 2,
Ch3 = 3,
}
const enum IrRemoteButton {
None = 0x00,
CenterBeacon = 0x01,
TopLeft = 0x02,
BottomLeft = 0x04,
TopRight = 0x08,
BottomRight = 0x10,
}
namespace input {
let uartMM: MMap
let devcon: Buffer
const enum DevConOff {
Connection = 0, // int8[4]
Type = 4, // int8[4]
@ -209,94 +511,76 @@ namespace input {
TST_UART_READ = 0xc0487409,
TST_UART_WRITE = 0xc048740a,
}
function init() {
if (uartMM) return
uartMM = control.mmap("/dev/lms_uart", UartOff.Size, 0)
if (!uartMM) control.fail("no uart sensor")
devcon = output.createBuffer(DevConOff.Size)
}
function uartReset(port: number) {
port = Math.clamp(0, 3, port)
devcon.setNumber(NumberFormat.Int8LE, DevConOff.Connection + port, DAL.CONN_NONE)
devcon.setNumber(NumberFormat.Int8LE, DevConOff.Type + port, 0)
devcon.setNumber(NumberFormat.Int8LE, DevConOff.Mode + port, 0)
uartMM.ioctl(IO.UART_SET_CONN, devcon)
namespace input {
export class TouchSensor extends inputint.AnalogSensor {
button: ButtonWrapper;
constructor() {
super()
this.button = new ButtonWrapper()
}
function getUartStatus(port: number) {
return uartMM.getNumber(NumberFormat.Int8LE, UartOff.Status + port)
_query() {
return this._readPin6() > 2500 ? 1 : 0
}
function waitNonZeroUartStatus(port: number) {
while (true) {
let s = getUartStatus(port)
if (s) return s
loops.pause(25)
_update(prev: number, curr: number) {
this.button.update(curr > 0)
}
}
function uartClearChange(port: number) {
const UART_DATA_READY = 8
const UART_PORT_CHANGED = 1
while (true) {
let status = getUartStatus(port)
export class ButtonWrapper extends core.Component {
private downTime: number;
private _isPressed: boolean;
private _wasPressed: boolean;
if ((status & UART_DATA_READY) != 0 && (status & UART_PORT_CHANGED) == 0)
break
devcon.setNumber(NumberFormat.Int8LE, DevConOff.Connection + port, DAL.CONN_INPUT_UART)
devcon.setNumber(NumberFormat.Int8LE, DevConOff.Type + port, 0)
devcon.setNumber(NumberFormat.Int8LE, DevConOff.Mode + port, 0)
uartMM.ioctl(IO.UART_CLEAR_CHANGED, devcon)
uartMM.setNumber(NumberFormat.Int8LE, UartOff.Status + port,
getUartStatus(port) & 0xfffe)
loops.pause(10)
}
constructor() {
super()
this.downTime = 0
this._isPressed = false
this._wasPressed = false
}
function setUartMode(port: number, mode: number) {
const UART_PORT_CHANGED = 1
port = Math.clamp(0, 3, port)
loops.pause(100)
while (true) {
devcon.setNumber(NumberFormat.Int8LE, DevConOff.Connection + port, DAL.CONN_INPUT_UART)
devcon.setNumber(NumberFormat.Int8LE, DevConOff.Type + port, 33)
devcon.setNumber(NumberFormat.Int8LE, DevConOff.Mode + port, mode)
uartMM.ioctl(IO.UART_SET_CONN, devcon)
let status = waitNonZeroUartStatus(port)
if (status & UART_PORT_CHANGED) {
uartClearChange(port)
//% hidden
update(curr: boolean) {
if (this._isPressed == curr) return
this._isPressed = curr
if (curr) {
this.downTime = control.millis()
control.raiseEvent(this._id, ButtonEvent.Down)
} else {
break
}
loops.pause(10)
control.raiseEvent(this._id, ButtonEvent.Up)
let delta = control.millis() - this.downTime
control.raiseEvent(this._id, delta > 500 ? ButtonEvent.LongClick : ButtonEvent.Click)
}
}
function getUartBytes(port: number): Buffer {
let index = uartMM.getNumber(NumberFormat.UInt16LE, UartOff.Actual + port * 2)
return uartMM.slice(UartOff.Raw + 32 * 300 * port + 32 * index, 32)
/**
* Check if button is currently pressed.
*/
isPressed() {
return this._isPressed
}
function getUartNumber(fmt: NumberFormat, off: number, port: number) {
let index = uartMM.getNumber(NumberFormat.UInt16LE, UartOff.Actual + port * 2)
return uartMM.getNumber(fmt, UartOff.Raw + 32 * 300 * port + 32 * index + off)
/**
* Check if button was pressed since last check.
*/
wasPressed() {
const r = this._wasPressed
this._wasPressed = false
return r
}
export class UartSensor {
port: number
id: number
constructor(port: number) {
this.port = Math.clamp(1, 4, port | 0) - 1;
this.id = 210 + port;
init()
uartReset(this.port)
/**
* Do something when a touch sensor is clicked, double clicked, etc...
* @param button the button that needs to be clicked or used
* @param event the kind of button gesture that needs to be detected
* @param body code to run when the event is raised
*/
onEvent(ev: ButtonEvent, body: () => void) {
control.onEvent(this._id, ev, body)
}
}
@ -318,16 +602,41 @@ namespace input {
}
}
export class IrSensor extends UartSensor {
private mode: IrSensorMode
export class IrSensor extends inputint.UartSensor {
private channel: IrRemoteChannel
private pollRunning: boolean
private buttons: ButtonWrapper[];
constructor(port: number) {
super(port)
this.mode = IrSensorMode.None
constructor() {
super()
this.channel = IrRemoteChannel.Ch0
this.pollRunning = false
this.buttons = []
for (let i = 0; i < 5; ++i) {
this.buttons.push(new ButtonWrapper())
}
}
button(id: IrRemoteButton) {
let num = -1
while (id) {
id >>= 1;
num++;
}
num = Math.clamp(0, this.buttons.length - 1, num)
return this.buttons[num]
}
_query() {
if (this.mode == IrSensorMode.RemoteControl)
return mapButton(this.getNumber(NumberFormat.UInt8LE, this.channel))
return 0
}
_update(prev: number, curr: number) {
for (let i = 0; i < this.buttons.length; ++i) {
let v = !!(curr & (1 << i))
this.buttons[i].update(v)
}
}
setRemoteChannel(c: IrRemoteChannel) {
@ -337,29 +646,28 @@ namespace input {
}
setMode(m: IrSensorMode) {
let v = Math.clamp(0, 2, m | 0)
if (v != this.mode) {
this.mode = v
setUartMode(this.port, v)
}
this._setMode(m)
}
getDistance() {
this.setMode(IrSensorMode.Proximity)
return getUartNumber(NumberFormat.UInt8LE, 0, this.port)
return this.getNumber(NumberFormat.UInt8LE, 0)
}
getRemoteCommand() {
this.setMode(IrSensorMode.RemoteControl)
let v = getUartNumber(NumberFormat.UInt8LE, this.channel, this.port)
return v
return this.getNumber(NumberFormat.UInt8LE, this.channel)
}
getDirectionAndDistance() {
this.setMode(IrSensorMode.Seek)
return getUartNumber(NumberFormat.UInt16LE, this.channel * 2, this.port)
return this.getNumber(NumberFormat.UInt16LE, this.channel * 2)
}
}
//% whenUsed
export const touch: TouchSensor = new TouchSensor()
//% whenUsed
export const ir: IrSensor = new IrSensor()
}

18
libs/core/output.ts
View File

@ -24,35 +24,35 @@ namespace output {
}
export function stop(out: Output, useBreak = false) {
let b = mkCmd(out, DAL.opOutputStop, 1)
let b = mkCmd(out, LMS.opOutputStop, 1)
b.setNumber(NumberFormat.UInt8LE, 2, useBreak ? 1 : 0)
writePWM(b)
}
export function start(out: Output) {
let b = mkCmd(out, DAL.opOutputStart, 0)
let b = mkCmd(out, LMS.opOutputStart, 0)
writePWM(b)
}
export function reset(out: Output) {
let b = mkCmd(out, DAL.opOutputReset, 0)
let b = mkCmd(out, LMS.opOutputReset, 0)
writePWM(b)
}
export function setSpeed(out: Output, speed: number) {
let b = mkCmd(out, DAL.opOutputSpeed, 1)
let b = mkCmd(out, LMS.opOutputSpeed, 1)
b.setNumber(NumberFormat.Int8LE, 2, Math.clamp(-100, 100, speed))
writePWM(b)
}
export function setPower(out: Output, power: number) {
let b = mkCmd(out, DAL.opOutputPower, 1)
let b = mkCmd(out, LMS.opOutputPower, 1)
b.setNumber(NumberFormat.Int8LE, 2, Math.clamp(-100, 100, power))
writePWM(b)
}
export function setPolarity(out: Output, polarity: number) {
let b = mkCmd(out, DAL.opOutputPolarity, 1)
let b = mkCmd(out, LMS.opOutputPolarity, 1)
b.setNumber(NumberFormat.Int8LE, 2, Math.clamp(-1, 1, polarity))
writePWM(b)
}
@ -68,11 +68,11 @@ namespace output {
}
export function step(out: Output, opts: StepOptions) {
let op = opts.useSteps ? DAL.opOutputStepSpeed : DAL.opOutputTimeSpeed
let op = opts.useSteps ? LMS.opOutputStepSpeed : LMS.opOutputTimeSpeed
let speed = opts.speed
if (speed == null) {
speed = opts.power
op = opts.useSteps ? DAL.opOutputStepPower : DAL.opOutputTimePower
op = opts.useSteps ? LMS.opOutputStepPower : LMS.opOutputTimePower
if (speed == null)
return
}
@ -90,7 +90,7 @@ namespace output {
const types = [0, 0, 0, 0]
export function setType(out: Output, type: OutputType) {
let b = mkCmd(out, DAL.opOutputSetType, 3)
let b = mkCmd(out, LMS.opOutputSetType, 3)
for (let i = 0; i < 4; ++i) {
if (out & (1 << i)) {
types[i] = type