diff --git a/robot/blocks.ts b/robot/blocks.ts index 8a474bc1..a775fedb 100644 --- a/robot/blocks.ts +++ b/robot/blocks.ts @@ -6,27 +6,12 @@ namespace microcode { throw "Add 'robot start' block" } - /** - * Sets both motors of the robot to the speed in percent. - */ - //% block="robot run at %speed=speedPicker \\%" - //% blockid="microcoderobotmotorrun" - //% group="Motors" - //% weight=100 - //% speed.defl=100 - //% speed.min=-100 - //% speed.max=100 - export function motorRun(speed: number) { - checkRobotDriver() - robot.motorRun(speed) - } - /** * Turns the robot. */ //% weight=98 //% group="Motors" - //% block="robot turn $turnRatio at $speed \\%" + //% block="robot run with turn $turnRatio at speed $speed \\%" //% blockid="microcoderobotmotorturn" //% speed.defl=100 //% speed.min=-100 @@ -35,10 +20,9 @@ namespace microcode { //% turnRatio.shadow=turnRatioPicker //% turnRatio.min=-200 //% turnRatio.max=200 - //% turnRatio.defl=100 - export function motorTurn(turnRatio: number, speed: number) { + export function motorRun(turnRatio: number, speed: number) { checkRobotDriver() - robot.motorTurn(turnRatio, speed) + robot.motorRun(turnRatio, speed) } /** @@ -75,6 +59,6 @@ namespace microcode { //% group="Sensors" export function detectLines(state: RobotLineState): boolean { checkRobotDriver() - return robot.lineState() === state + return robot.currentLineState === state } } \ No newline at end of file diff --git a/robot/cutebot.ts b/robot/cutebot.ts index 3e74200a..aadd43c2 100644 --- a/robot/cutebot.ts +++ b/robot/cutebot.ts @@ -73,11 +73,7 @@ namespace microcode { constructor() { super() this.musicVolume = 168 - this.maxRunSpeed = 100 - this.maxBackSpeed = 30 - this.maxTurnSpeed = 60 - 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 b213d5da..232f01ac 100644 --- a/robot/cutebotpro.ts +++ b/robot/cutebotpro.ts @@ -216,11 +216,7 @@ namespace microcode { constructor() { super() this.musicVolume = 168 - this.maxRunSpeed = 50 - this.maxBackSpeed = 50 - this.maxTurnSpeed = 50 - 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 e23e55d1..d56c8f28 100644 --- a/robot/messages.ts +++ b/robot/messages.ts @@ -1,163 +1 @@ -namespace microcode.robots { - const MESSAGE_MAGIC = 0xf498 - - /** - * List of commands supported by the micro:bit robot program - */ - export const enum RobotCommand { - /** - * Runs the robot forward and backward. - * speed: i16 % - */ - MotorRun = 0x01, - /** - * Turn the robot left and right. Right is positive values. - * turnRatio: i16 [-200,200] - * speed: i16 % - */ - 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.MotorRun - payload = Buffer.create(3) - 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, - 0, - speed - ) - if(msg !== RobotCompactCommand.MotorRunForwardFast) - payload[2] = 1 - } - break - } - case RobotCompactCommand.MotorSpinLeft: - case RobotCompactCommand.MotorSpinRight: - case RobotCompactCommand.MotorTurnLeft: - case RobotCompactCommand.MotorTurnRight: { - cmd = RobotCommand.MotorTurn - payload = Buffer.create(4) - 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 = 80; break; - case RobotCompactCommand.MotorSpinRight: turnRatio = 200; speed = 80; break; - } - payload.setNumber( - NumberFormat.Int16LE, - 0, - turnRatio - ) - payload.setNumber( - NumberFormat.Int16LE, - 2, - speed - ) - 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 f093af5c..c34614e7 100644 --- a/robot/robot.ts +++ b/robot/robot.ts @@ -2,11 +2,7 @@ namespace microcode.robots { export class Robot { musicVolume = 64 - maxRunSpeed = 80 - maxBackSpeed = 50 - maxTurnSpeed = 60 - maxLineRunSpeed = 40 - maxLineTurnSpeed = 50 + maxLineSpeed = 40 constructor() { diff --git a/robot/robotdriver.ts b/robot/robotdriver.ts index be2a8a26..b9ae7cfb 100644 --- a/robot/robotdriver.ts +++ b/robot/robotdriver.ts @@ -4,17 +4,13 @@ //% color="#ff6800" icon="\uf1b9" weight=15 namespace microcode { const RUN_STOP_THRESHOLD = 2 - const MODE_SWITCH_THRESHOLD = 2 const TARGET_SPEED_THRESHOLD = 4 - const MODE_TRANSITION_ALPHA = 0.2 - const SPEED_TRANSITION_ALPHA = 0.915 + const SPEED_TRANSITION_ALPHA = 0.97 + const SPEED_BRAKE_TRANSITION_ALPHA = 0.8 + const TARGET_TURN_RATIO_THRESHOLD = 20 + const TURN_RATIO_TRANSITION_ALPHA = 0.2 const ULTRASONIC_MIN_READING = 1 - const LINE_TURN_ALPHA = 0.7 - - enum RobotMotorMode { - Run, - Turn, - } + const LINE_ASSIST_LOST_THRESHOLD = 72 /** * @@ -29,11 +25,12 @@ namespace microcode { private configDrift = false private currentArmAperture = -1 private currentSpeed: number = 0 - private currentMotorMode = RobotMotorMode.Run private targetSpeed: number = 0 - turnRatio = 0 - private targetMotorMode = RobotMotorMode.Run + private currentTurnRatio = 0 + private targetTurnRatio: number = 0 + currentLineState: RobotLineState = RobotLineState.None + private currentLineStateCounter = 0 private stopToneMillis: number = 0 lineAssist = false @@ -133,65 +130,63 @@ namespace microcode { //}) radio.setTransmitSerialNumber(true); radio.onReceivedNumber(code => { - const msg = robots.decodeRobotCompactCommand(code) - this.dispatch(msg) + this.decodeRobotCompactCommand(code) }) } private updateSpeed() { - // transition from one mode to the other, robot should stop - if (this.currentMotorMode !== this.targetMotorMode) { - const alpha = MODE_TRANSITION_ALPHA - this.currentSpeed = this.currentSpeed * alpha - if (Math.abs(this.currentSpeed) < MODE_SWITCH_THRESHOLD) { - this.currentSpeed = 0 - this.currentMotorMode = this.targetMotorMode - } - } else { - const alpha = SPEED_TRANSITION_ALPHA + // smooth update of speed + { + const accelerating = this.targetSpeed > 0 && this.currentSpeed < this.targetSpeed + const alpha = accelerating ? SPEED_TRANSITION_ALPHA : SPEED_BRAKE_TRANSITION_ALPHA this.currentSpeed = this.currentSpeed * alpha + this.targetSpeed * (1 - alpha) - if (Math.abs(this.currentSpeed - this.targetSpeed) < TARGET_SPEED_THRESHOLD) { - this.currentSpeed = this.targetSpeed + if (this.lineAssist && this.currentSpeed > 0) { + if (this.currentLineState // left, right, front + || this.currentLineStateCounter < LINE_ASSIST_LOST_THRESHOLD) // recently lost line + this.currentSpeed = Math.min(this.currentSpeed, this.robot.maxLineSpeed) } + if (Math.abs(this.currentSpeed - this.targetSpeed) < TARGET_SPEED_THRESHOLD) + this.currentSpeed = this.targetSpeed + } + // smoth update of turn ratio + { + const alpha = TURN_RATIO_TRANSITION_ALPHA + this.currentTurnRatio = + this.currentTurnRatio * alpha + this.targetTurnRatio * (1 - alpha) + if (Math.abs(this.currentTurnRatio - this.targetTurnRatio) < TARGET_TURN_RATIO_THRESHOLD) + this.currentTurnRatio = this.targetTurnRatio } - const lines = this.currentLineState - if (this.currentMotorMode === RobotMotorMode.Run || this.turnRatio === 0) { - let s = - Math.abs(this.currentSpeed) < RUN_STOP_THRESHOLD - ? 0 - : this.currentSpeed - const d = Math.abs(s) > Math.abs(this.runDrift) ? this.runDrift / 2 : 0 - let left = s - d - let right = s + d - if (this.lineAssist && lines && s > 0) { // going forward - this.currentSpeed = s = Math.min(s, this.robot.maxLineRunSpeed) - } - this.setMotorState(left, right) - } else { - console.log(`turnratio: ${this.turnRatio}`) + if (Math.abs(this.currentSpeed) < RUN_STOP_THRESHOLD) + this.setMotorState(0, 0) + else { let s = this.currentSpeed - if (this.lineAssist && lines) - s = Math.sign(s) * Math.min(Math.abs(s), this.robot.maxLineTurnSpeed) const ns = Math.abs(s) - let left: number - let right: number + let left = 0 + let right = 0 // apply turn ratio - if (this.turnRatio < 0) { - right = s - left = s * (1 + (this.turnRatio / 100)) + if (this.currentTurnRatio < 0) { + right += s + left += s * (1 + (this.currentTurnRatio / 100)) } else { - left = s - right = s * (1 - (this.turnRatio / 100)) + left += s + right += s * (1 - (this.currentTurnRatio / 100)) } // clamp left = Math.clamp(-ns, ns, Math.round(left)) right = Math.clamp(-ns, ns, Math.round(right)) - console.log(`l: ${left}`) + // apply drift + const drift = this.runDrift / 2 + left -= drift + right += drift + + // clamp again + left = Math.clamp(-100, 100, Math.round(left)) + right = Math.clamp(-100, 100, Math.round(right)) this.setMotorState(left, right) } } @@ -199,8 +194,6 @@ namespace microcode { private setMotorState(left: number, right: number) { this.robot.motorRun(left, right) if (this.showConfiguration) return - console.log(`left: ${left}`) - console.log(`right: ${right}`) this.showSingleMotorState(3, left) this.showSingleMotorState(1, right) } @@ -273,44 +266,19 @@ namespace microcode { this.currentArmAperture = Math.clamp(-1, 100, Math.round(aperture)) } - motorRun(speed: number) { - this.start() - speed = - speed > 0 - ? Math.min(this.robot.maxRunSpeed, speed) - : Math.max(-this.robot.maxBackSpeed, speed) - if (this.targetMotorMode !== RobotMotorMode.Run || this.targetSpeed !== speed) { - this.setHeadlingSpeedColor(speed) - this.targetMotorMode = RobotMotorMode.Run - this.targetSpeed = speed - } - } - - motorTurn(turnRatio: number, speed: number) { + motorRun(turnRatio: number, speed: number) { this.start() turnRatio = Math.clamp(-200, 200, turnRatio) - if (turnRatio === 0) { // special case - this.motorRun(speed) - return - } - - speed = - speed > 0 - ? Math.min(this.robot.maxTurnSpeed, speed) - : Math.max(-this.robot.maxTurnSpeed, speed) - const newMode = this.targetMotorMode !== RobotMotorMode.Turn - if (newMode || this.targetSpeed !== speed || this.turnRatio !== turnRatio) { + speed = Math.clamp(-100, 100, Math.round(speed)) + if (this.targetSpeed !== speed || this.currentTurnRatio !== turnRatio) { this.setHeadlingSpeedColor(speed) - this.targetMotorMode = RobotMotorMode.Turn this.targetSpeed = speed - this.turnRatio = turnRatio - if (newMode) - this.currentSpeed = 0 + this.targetTurnRatio = turnRatio } } motorStop() { - this.motorRun(0) + this.motorRun(0, 0) } ultrasonicDistance() { @@ -325,13 +293,15 @@ namespace microcode { return this.currentUltrasonicDistance } - lineState(): RobotLineState { + private lineState(): RobotLineState { const ls = this.robot.lineState() if (ls !== this.currentLineState) { this.currentLineState = ls + this.currentLineStateCounter = 0 const msg = microcode.robots.RobotCompactCommand.LineState | this.currentLineState microcode.robots.sendCompactCommand(msg) } + this.currentLineStateCounter++ return ls } @@ -358,48 +328,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.MotorRun: { - const speed = Math.clamp( - -100, - 100, - payload.getNumber(NumberFormat.Int16LE, 0) - ) - this.lineAssist = !!payload[2] - this.motorRun(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 = 70; break; + case microcode.robots.RobotCompactCommand.MotorRunForwardFast: speed = 100; break; + case microcode.robots.RobotCompactCommand.MotorRunBackward: speed = -50; break; + case microcode.robots.RobotCompactCommand.MotorTurnLeft: turnRatio = -50; speed = 70; break; + case microcode.robots.RobotCompactCommand.MotorTurnRight: turnRatio = 50; speed = 70; break; + case microcode.robots.RobotCompactCommand.MotorSpinLeft: turnRatio = -200; speed = 60; break; + case microcode.robots.RobotCompactCommand.MotorSpinRight: turnRatio = 200; speed = 60; break; + } + this.motorRun(turnRatio, speed); this.playTone(440, 50) break } - case robots.RobotCommand.MotorTurn: { - const turnRatio = payload.getNumber(NumberFormat.Int16LE, 0) - const speed = payload.getNumber(NumberFormat.Int16LE, 2) - this.motorTurn(turnRatio, speed) - this.inRadioMessageId++ - this.playTone(932, 50) + case microcode.robots.RobotCompactCommand.MotorArmClose: { + this.armOpen(0) 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.MotorArmOpen: { + this.armOpen(100) break } } } } -} +} \ No newline at end of file diff --git a/robot/test.ts b/robot/test.ts index a229c4ee..f8cca216 100644 --- a/robot/test.ts +++ b/robot/test.ts @@ -5,12 +5,62 @@ microcode.elecfreaksCuteBot.start() //microcode.setMotorDrift(6) const r = microcode.robot +r.lineAssist = true +/* +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(-150, 100) +}) +*/ + +/* +// calibrate +basic.forever(() => { + r.motorRun(0, 50) + pause(2000) + r.motorStop() + pause(1000) + r.motorRun(200, 50) + pause(400) + r.motorStop() + pause(1000) +}) +*/ + +/* +// test motors +basic.forever(() => { + r.motorRun(0, 80) + pause(400) + r.motorRun(0, -80) + pause(400) + r.motorRun(0, 50) + pause(800) + r.motorRun(0, -50) + pause(800) + r.motorRun(80, 80) + pause(400) + r.motorRun(-80, 80) + pause(400) + r.motorRun(200, 50) + pause(1000) + r.motorRun(-200, 50) + pause(1000) +}) +*/ /* let tr = 0 input.onButtonPressed(Button.A, () => { tr -= 10 - microcode.robotDriver.motorTurn(tr, 50) + microcode.robotDriver.motorTurn(tr, 50) }) input.onButtonPressed(Button.B, () => { tr += 10 diff --git a/robot/yahboomtinybit.ts b/robot/yahboomtinybit.ts index 7fc04ef5..5f0737ff 100644 --- a/robot/yahboomtinybit.ts +++ b/robot/yahboomtinybit.ts @@ -118,11 +118,7 @@ namespace microcode { pins.setPull(DigitalPin.P13, PinPullMode.PullNone) pins.setPull(DigitalPin.P14, PinPullMode.PullNone) - this.maxLineRunSpeed = 64 - this.maxLineTurnSpeed = 90 - this.maxRunSpeed = 75 - this.maxBackSpeed = 64 - this.maxTurnSpeed = 90 + this.maxLineSpeed = 64 } motorRun(left: number, right: number): void {