diff --git a/robot/cutebot.ts b/robot/cutebot.ts index fac9308a..aadd43c2 100644 --- a/robot/cutebot.ts +++ b/robot/cutebot.ts @@ -73,8 +73,7 @@ namespace microcode { constructor() { super() this.musicVolume = 168 - this.maxLineRunSpeed = 28 - this.maxLineTurnSpeed = 60 + this.maxLineSpeed = 28 pins.setPull(DigitalPin.P8, PinPullMode.PullNone) pins.setPull(DigitalPin.P13, PinPullMode.PullNone) diff --git a/robot/cutebotpro.ts b/robot/cutebotpro.ts index 126372d1..232f01ac 100644 --- a/robot/cutebotpro.ts +++ b/robot/cutebotpro.ts @@ -216,8 +216,7 @@ namespace microcode { constructor() { super() this.musicVolume = 168 - this.maxLineRunSpeed = 30 - this.maxLineTurnSpeed = 30 + this.maxLineSpeed = 30 const v = readVersions() console.log(`cutebot pro version: ${v}`) diff --git a/robot/messages.ts b/robot/messages.ts index 08b648e4..d56c8f28 100644 --- a/robot/messages.ts +++ b/robot/messages.ts @@ -1,160 +1 @@ -namespace microcode.robots { - const MESSAGE_MAGIC = 0xf498 - - /** - * List of commands supported by the micro:bit robot program - */ - export const enum RobotCommand { - /** - * Turn the robot left and right. Right is positive values. - * turnRatio: i16 [-200,200] - * speed: i16 % - * lineAssist: bool (u8) - */ - MotorTurn = 0x02, - - /** - * Controls the opening angle of the arm - */ - MotorArm = 0x03, - - /** - * Report ultrasonic distance in cm - * distance: f32 - */ - UltrasonicDistance = 0x10, - - /** - * The line sensor state changed - * state: RobotLineState - */ - LineState = 0x11, - } - - export interface RobotMessage { - /** - * message identifier to drop repeated messages; u8 - */ - messageId: number - /** - * Robot command - */ - cmd: RobotCommand - /** - * Command payload - */ - payload: Buffer - } - - /** - * Encodes the command and payload into a buffer that can be sent via radio - * - * 0 magic, u16 - * 2 message id, u8 - * 3 cmd, u8 - * 4 payload, u8[] - */ - export function encodeRobotMessage(msg: RobotMessage) { - const payload = msg.payload - const messageid = msg.messageId - const cmd = msg.cmd - - const buf = pins.createBuffer(6 + payload.length) - buf.setNumber(NumberFormat.UInt16LE, 0, MESSAGE_MAGIC) - buf.setNumber(NumberFormat.UInt8LE, 2, messageid) - buf.setNumber(NumberFormat.UInt8LE, 3, cmd) - buf.write(4, payload) - return buf - } - - /** - * Decodes message buffer - */ - export function decodeRobotCommand(msg: Buffer): RobotMessage { - if (!msg || msg.length < 4) return undefined - - const magic = msg.getNumber(NumberFormat.UInt16LE, 0) - if (magic !== MESSAGE_MAGIC) return undefined - - const messageId = msg.getNumber(NumberFormat.UInt8LE, 2) - const cmd = msg.getNumber(NumberFormat.UInt8LE, 3) - const payload = msg.slice(4) - return { - messageId: messageId, - cmd: cmd, - payload: payload, - } - } - - /** - * Decode compact radio message - */ - export function decodeRobotCompactCommand(msg: number): RobotMessage { - const messageId = control.micros() - let cmd: RobotCommand - let payload: Buffer - switch (msg) { - case RobotCompactCommand.MotorRunForwardFast: - case RobotCompactCommand.MotorRunForward: - case RobotCompactCommand.MotorRunBackward: - case RobotCompactCommand.MotorStop: { - cmd = RobotCommand.MotorTurn - payload = Buffer.create(5) - if (msg !== RobotCompactCommand.MotorStop) { - let speed = 0 - switch (msg) { - case RobotCompactCommand.MotorRunForward: speed = 40; break; - case RobotCompactCommand.MotorRunForwardFast: speed = 100; break; - case RobotCompactCommand.MotorRunBackward: speed = -100; break; - } - payload.setNumber( - NumberFormat.Int16LE, - 2, - speed - ) - if (msg !== RobotCompactCommand.MotorRunForwardFast) - payload[4] = 1 - } - break - } - case RobotCompactCommand.MotorSpinLeft: - case RobotCompactCommand.MotorSpinRight: - case RobotCompactCommand.MotorTurnLeft: - case RobotCompactCommand.MotorTurnRight: { - cmd = RobotCommand.MotorTurn - payload = Buffer.create(5) - let turnRatio = 0 - let speed = 100 - switch (msg) { - case RobotCompactCommand.MotorTurnLeft: turnRatio = -50; speed = 100; break; - case RobotCompactCommand.MotorTurnRight: turnRatio = 50; speed = 100; break; - case RobotCompactCommand.MotorSpinLeft: turnRatio = -200; speed = 70; break; - case RobotCompactCommand.MotorSpinRight: turnRatio = 200; speed = 70; break; - } - payload.setNumber( - NumberFormat.Int16LE, - 0, - turnRatio - ) - payload.setNumber( - NumberFormat.Int16LE, - 2, - speed - ) - payload[4] = 1 - break - } - case RobotCompactCommand.MotorArmClose: - case RobotCompactCommand.MotorArmOpen: { - cmd = RobotCommand.MotorArm - payload = Buffer.create(2) - payload.setNumber(NumberFormat.UInt16LE, 0, msg === RobotCompactCommand.MotorArmClose ? 0 : 100) - break - } - default: - return undefined - } - - return { messageId, cmd, payload } - } -} +// obsolete \ No newline at end of file diff --git a/robot/radio.ts b/robot/radio.ts index 8e89ec0c..d4a329ab 100644 --- a/robot/radio.ts +++ b/robot/radio.ts @@ -17,17 +17,6 @@ namespace microcode.robots { radio.setGroup(radioGroup) } - let nextMessageId = 0 - export function sendCommand(cmd: RobotCommand, payload: Buffer) { - nextMessageId = (nextMessageId + 1) % 0xff - const buf = encodeRobotMessage({ - messageId: nextMessageId, - cmd, - payload, - }) - radio.sendBuffer(buf) - } - export function sendCompactCommand(cmd: RobotCompactCommand) { radio.sendNumber(cmd) } diff --git a/robot/robot.ts b/robot/robot.ts index 7dd4ee60..c34614e7 100644 --- a/robot/robot.ts +++ b/robot/robot.ts @@ -2,8 +2,7 @@ namespace microcode.robots { export class Robot { musicVolume = 64 - maxLineRunSpeed = 40 - maxLineTurnSpeed = 50 + maxLineSpeed = 40 constructor() { diff --git a/robot/robotdriver.ts b/robot/robotdriver.ts index 3e6abbd3..e0837308 100644 --- a/robot/robotdriver.ts +++ b/robot/robotdriver.ts @@ -127,8 +127,7 @@ namespace microcode { //}) radio.setTransmitSerialNumber(true); radio.onReceivedNumber(code => { - const msg = robots.decodeRobotCompactCommand(code) - this.dispatch(msg) + this.decodeRobotCompactCommand(code) }) } @@ -150,13 +149,12 @@ namespace microcode { this.currentTurnRatio = this.targetTurnRatio } - if (Math.abs(this.currentSpeed) < RUN_STOP_THRESHOLD) { + if (Math.abs(this.currentSpeed) < RUN_STOP_THRESHOLD) this.setMotorState(0, 0) - } else { let s = this.currentSpeed - if (this.lineAssist && this.currentLineState && s > 0) - s = Math.min(Math.abs(s), this.robot.maxLineTurnSpeed) + if (this.lineAssist && s > 0) + s = Math.min(Math.abs(s), this.robot.maxLineSpeed) const ns = Math.abs(s) let left = 0 @@ -321,36 +319,41 @@ namespace microcode { } } - dispatch(msg: robots.RobotMessage) { - if (!msg) return - - const messageId = msg.messageId - if (this.lastReceivedMessageId === messageId) { - return // duplicate - } - - // decode message - this.lastReceivedMessageId = messageId - const cmd = msg.cmd - const payload = msg.payload - - switch (cmd) { - case robots.RobotCommand.MotorTurn: { - const turnRatio = payload.getNumber(NumberFormat.Int16LE, 0) - const speed = payload.getNumber(NumberFormat.Int16LE, 2) - this.motorRun(turnRatio, speed) - this.inRadioMessageId++ + private decodeRobotCompactCommand(msg: number) { + this.inRadioMessageId++ + switch (msg) { + case microcode.robots.RobotCompactCommand.MotorStop: + case microcode.robots.RobotCompactCommand.MotorTurnLeft: + case microcode.robots.RobotCompactCommand.MotorTurnRight: + case microcode.robots.RobotCompactCommand.MotorSpinLeft: + case microcode.robots.RobotCompactCommand.MotorSpinRight: + case microcode.robots.RobotCompactCommand.MotorRunForwardFast: + case microcode.robots.RobotCompactCommand.MotorRunForward: + case microcode.robots.RobotCompactCommand.MotorRunBackward: { + let turnRatio = 0 + let speed = 0 + switch (msg) { + case microcode.robots.RobotCompactCommand.MotorRunForward: speed = 40; break; + case microcode.robots.RobotCompactCommand.MotorRunForwardFast: speed = 100; break; + case microcode.robots.RobotCompactCommand.MotorRunBackward: speed = -100; break; + case microcode.robots.RobotCompactCommand.MotorTurnLeft: turnRatio = -50; speed = 100; break; + case microcode.robots.RobotCompactCommand.MotorTurnRight: turnRatio = 50; speed = 100; break; + case microcode.robots.RobotCompactCommand.MotorSpinLeft: turnRatio = -200; speed = 70; break; + case microcode.robots.RobotCompactCommand.MotorSpinRight: turnRatio = 200; speed = 70; break; + } + this.motorRun(turnRatio, speed); this.playTone(440, 50) break } - case robots.RobotCommand.MotorArm: { - const aperture = payload.getNumber(NumberFormat.Int16LE, 0) - this.armOpen(aperture) - this.inRadioMessageId++ - this.playTone(1132, 50) + case microcode.robots.RobotCompactCommand.MotorArmClose: { + this.armOpen(0) + break + } + case microcode.robots.RobotCompactCommand.MotorArmOpen: { + this.armOpen(100) break } } } } -} +} \ No newline at end of file diff --git a/robot/test.ts b/robot/test.ts index 0cbfa5a3..4f877edb 100644 --- a/robot/test.ts +++ b/robot/test.ts @@ -5,8 +5,27 @@ microcode.elecfreaksCuteBot.start() //microcode.setMotorDrift(6) const r = microcode.robot -r.lineAssist = false +r.lineAssist = true +r.motorRun(200, 100) +pause(400) +r.motorStop() +pause(1000) + +basic.forever(() => { + const lines = r.currentLineState + if (lines === RobotLineState.Left) + r.motorRun(-80, 100) + else if (lines === RobotLineState.Right) + r.motorRun(80, 100) + else if (lines === RobotLineState.Both) + r.motorRun(0, 100) + else + r.motorRun(-50, 100) +}) + +/* +// calibrate basic.forever(() => { r.motorRun(0, 50) pause(2000) @@ -17,8 +36,10 @@ basic.forever(() => { r.motorStop() pause(1000) }) +*/ /* +// test motors basic.forever(() => { r.motorRun(0, 80) pause(400) diff --git a/robot/yahboomtinybit.ts b/robot/yahboomtinybit.ts index 28f5a7c9..5f0737ff 100644 --- a/robot/yahboomtinybit.ts +++ b/robot/yahboomtinybit.ts @@ -118,8 +118,7 @@ namespace microcode { pins.setPull(DigitalPin.P13, PinPullMode.PullNone) pins.setPull(DigitalPin.P14, PinPullMode.PullNone) - this.maxLineRunSpeed = 64 - this.maxLineTurnSpeed = 90 + this.maxLineSpeed = 64 } motorRun(left: number, right: number): void {