Skip to content

Commit

Permalink
Merge branch 'main' of https://github.com/microsoft/microcode into bi…
Browse files Browse the repository at this point in the history 
…naryFormat
  • Loading branch information
@tballmsft
tballmsft committed Oct 14, 2023
2 parents b44e0cb + c41c5d1 commit 33685f8
Show file tree
Hide file tree
Showing 8 changed files with 90 additions and 138 deletions.
22 changes: 3 additions & 19 deletions robot/blocks.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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)
}

/**
Expand Down
3 changes: 0 additions & 3 deletions robot/cutebot.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
3 changes: 0 additions & 3 deletions robot/cutebotpro.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
23 changes: 10 additions & 13 deletions robot/messages.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,

Expand Down Expand Up @@ -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) {
Expand All @@ -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
}
Expand All @@ -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,
Expand All @@ -145,6 +141,7 @@ namespace microcode.robots {
2,
speed
)
payload[4] = 1
break
}
case RobotCompactCommand.MotorArmClose:
Expand Down
3 changes: 0 additions & 3 deletions robot/robot.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,6 @@ namespace microcode.robots {

export class Robot {
musicVolume = 64
maxRunSpeed = 80
maxBackSpeed = 50
maxTurnSpeed = 60
maxLineRunSpeed = 40
maxLineTurnSpeed = 50

Expand Down
137 changes: 44 additions & 93 deletions robot/robotdriver.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
}

/**
*
Expand All @@ -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
Expand Down Expand Up @@ -139,68 +133,62 @@ 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)
}
}

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)
}
Expand Down Expand Up @@ -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() {
Expand Down Expand Up @@ -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: {
Expand Down
34 changes: 33 additions & 1 deletion robot/test.ts
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Loading

0 comments on commit 33685f8

Please sign in to comment.