diff --git a/robot/blocks.ts b/robot/blocks.ts index 8a474bc1..f0baf0fb 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) } /** diff --git a/robot/cutebot.ts b/robot/cutebot.ts index 3e74200a..fac9308a 100644 --- a/robot/cutebot.ts +++ b/robot/cutebot.ts @@ -73,9 +73,6 @@ namespace microcode { constructor() { super() this.musicVolume = 168 - this.maxRunSpeed = 100 - this.maxBackSpeed = 30 - this.maxTurnSpeed = 60 this.maxLineRunSpeed = 28 this.maxLineTurnSpeed = 60 diff --git a/robot/cutebotpro.ts b/robot/cutebotpro.ts index b213d5da..126372d1 100644 --- a/robot/cutebotpro.ts +++ b/robot/cutebotpro.ts @@ -216,9 +216,6 @@ namespace microcode { constructor() { super() this.musicVolume = 168 - this.maxRunSpeed = 50 - this.maxBackSpeed = 50 - this.maxTurnSpeed = 50 this.maxLineRunSpeed = 30 this.maxLineTurnSpeed = 30 diff --git a/robot/messages.ts b/robot/messages.ts index e23e55d1..08b648e4 100644 --- a/robot/messages.ts +++ b/robot/messages.ts @@ -5,15 +5,11 @@ namespace microcode.robots { * 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 % + * lineAssist: bool (u8) */ MotorTurn = 0x02, @@ -102,8 +98,8 @@ namespace microcode.robots { case RobotCompactCommand.MotorRunForward: case RobotCompactCommand.MotorRunBackward: case RobotCompactCommand.MotorStop: { - cmd = RobotCommand.MotorRun - payload = Buffer.create(3) + cmd = RobotCommand.MotorTurn + payload = Buffer.create(5) if (msg !== RobotCompactCommand.MotorStop) { let speed = 0 switch (msg) { @@ -113,11 +109,11 @@ namespace microcode.robots { } payload.setNumber( NumberFormat.Int16LE, - 0, + 2, speed ) - if(msg !== RobotCompactCommand.MotorRunForwardFast) - payload[2] = 1 + if (msg !== RobotCompactCommand.MotorRunForwardFast) + payload[4] = 1 } break } @@ -126,14 +122,14 @@ namespace microcode.robots { case RobotCompactCommand.MotorTurnLeft: case RobotCompactCommand.MotorTurnRight: { cmd = RobotCommand.MotorTurn - payload = Buffer.create(4) + 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 = 80; break; - case RobotCompactCommand.MotorSpinRight: turnRatio = 200; speed = 80; break; + case RobotCompactCommand.MotorSpinLeft: turnRatio = -200; speed = 70; break; + case RobotCompactCommand.MotorSpinRight: turnRatio = 200; speed = 70; break; } payload.setNumber( NumberFormat.Int16LE, @@ -145,6 +141,7 @@ namespace microcode.robots { 2, speed ) + payload[4] = 1 break } case RobotCompactCommand.MotorArmClose: diff --git a/robot/robot.ts b/robot/robot.ts index f093af5c..7dd4ee60 100644 --- a/robot/robot.ts +++ b/robot/robot.ts @@ -2,9 +2,6 @@ namespace microcode.robots { export class Robot { musicVolume = 64 - maxRunSpeed = 80 - maxBackSpeed = 50 - maxTurnSpeed = 60 maxLineRunSpeed = 40 maxLineTurnSpeed = 50 diff --git a/robot/robotdriver.ts b/robot/robotdriver.ts index be2a8a26..3e6abbd3 100644 --- a/robot/robotdriver.ts +++ b/robot/robotdriver.ts @@ -4,17 +4,11 @@ //% 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 TARGET_TURN_RATIO_THRESHOLD = 20 + const TURN_RATIO_TRANSITION_ALPHA = 0.8 const ULTRASONIC_MIN_READING = 1 - const LINE_TURN_ALPHA = 0.7 - - enum RobotMotorMode { - Run, - Turn, - } /** * @@ -29,10 +23,10 @@ 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 stopToneMillis: number = 0 @@ -139,59 +133,55 @@ namespace microcode { } 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 { + // smooth update of speed + { const alpha = SPEED_TRANSITION_ALPHA this.currentSpeed = this.currentSpeed * alpha + this.targetSpeed * (1 - alpha) - if (Math.abs(this.currentSpeed - this.targetSpeed) < TARGET_SPEED_THRESHOLD) { + 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) + if (this.lineAssist && this.currentLineState && s > 0) + 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 +189,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 +261,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() { @@ -372,24 +335,12 @@ namespace microcode { 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++ - 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.motorRun(turnRatio, speed) this.inRadioMessageId++ - this.playTone(932, 50) + this.playTone(440, 50) break } case robots.RobotCommand.MotorArm: { diff --git a/robot/test.ts b/robot/test.ts index a229c4ee..0cbfa5a3 100644 --- a/robot/test.ts +++ b/robot/test.ts @@ -5,12 +5,44 @@ microcode.elecfreaksCuteBot.start() //microcode.setMotorDrift(6) const r = microcode.robot +r.lineAssist = false +basic.forever(() => { + r.motorRun(0, 50) + pause(2000) + r.motorStop() + pause(1000) + r.motorRun(200, 50) + pause(400) + r.motorStop() + pause(1000) +}) + +/* +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..28f5a7c9 100644 --- a/robot/yahboomtinybit.ts +++ b/robot/yahboomtinybit.ts @@ -120,9 +120,6 @@ namespace microcode { this.maxLineRunSpeed = 64 this.maxLineTurnSpeed = 90 - this.maxRunSpeed = 75 - this.maxBackSpeed = 64 - this.maxTurnSpeed = 90 } motorRun(left: number, right: number): void {