diff --git a/libs/core/_locales/core-jsdoc-strings.json b/libs/core/_locales/core-jsdoc-strings.json index 3d4c57e7..c198899e 100644 --- a/libs/core/_locales/core-jsdoc-strings.json +++ b/libs/core/_locales/core-jsdoc-strings.json @@ -75,9 +75,15 @@ "motors.MotorBase.setSpeed": "Sets the speed of the motor.", "motors.MotorBase.setSpeed|param|speed": "the speed from ``100`` full forward to ``-100`` full backward, eg: 50", "motors.MotorBase.stop": "Stops the motor(s).", + "motors.SynchedMotorPair.drive": "Makes a differential drive robot move with a given speed (%) and rotation rate (deg/s)\nusing a unicycle model.", + "motors.SynchedMotorPair.drive|param|rotationSpeed": "rotation of the robot around the center point, eg: 30", + "motors.SynchedMotorPair.drive|param|speed": "speed of the center point between motors, eg: 10", + "motors.SynchedMotorPair.drive|param|value": "the amount of movement, eg: 2", + "motors.SynchedMotorPair.setDimensions": "Sets the wheels radius and base length of a directional drive robot", + "motors.SynchedMotorPair.setDimensions|param|wheelRadius": "@param baseLength ", "motors.SynchedMotorPair.steer": "Turns the motor and the follower motor by a number of rotations", "motors.SynchedMotorPair.steer|param|speed": "the speed from ``100`` full forward to ``-100`` full backward, eg: 50", - "motors.SynchedMotorPair.steer|param|turnRatio": "the ratio of power sent to the follower motor, from ``-200`` to ``200``, eg: 100", + "motors.SynchedMotorPair.steer|param|turnRatio": "the ratio of power sent to the follower motor, from ``-200`` to ``200``, eg: 0", "motors.SynchedMotorPair.steer|param|unit": "the meaning of the value", "motors.SynchedMotorPair.steer|param|value": "the move quantity, eg: 2", "motors.SynchedMotorPair.tank": "The Move Tank block can make a robot drive forward, backward, turn, or stop. \nUse the Move Tank block for robot vehicles that have two Large Motors, \nwith one motor driving the left side of the vehicle and the other the right side. \nYou can make the two motors go at different speeds or in different directions \nto make your robot turn.", diff --git a/libs/core/_locales/core-strings.json b/libs/core/_locales/core-strings.json index 1d67c37b..95afbc34 100644 --- a/libs/core/_locales/core-strings.json +++ b/libs/core/_locales/core-strings.json @@ -54,8 +54,9 @@ "motors.MotorBase.setBrake|block": "set %motor|brake %brake", "motors.MotorBase.setReversed|block": "set %motor|reversed %reversed", "motors.MotorBase.setSpeed|block": "set speed of %motor|to %speed|%", - "motors.SynchedMotorPair.steer|block": "steer %chassis|%turnRatio|%|at speed %speed|%|by %value|%unit", - "motors.SynchedMotorPair.tank|block": "tank %chassis|left %speedLeft|%|right %speedRight|%|by %value|%unit", + "motors.SynchedMotorPair.drive|block": "drive %chassis|at %speed|cm/s|turning %rotationSpeed|deg/s|for %value|%unit", + "motors.SynchedMotorPair.steer|block": "steer %chassis turn by|%turnRatio|at speed %speed|%|for %value|%unit", + "motors.SynchedMotorPair.tank|block": "tank %chassis|left %speedLeft|%|right %speedRight|%|for %value|%unit", "motors.largeAB|block": "large A+B", "motors.largeAD|block": "large A+D", "motors.largeA|block": "large A", diff --git a/libs/core/output.ts b/libs/core/output.ts index 108013df..700e9a73 100644 --- a/libs/core/output.ts +++ b/libs/core/output.ts @@ -399,9 +399,13 @@ namespace motors { //% fixedInstances export class SynchedMotorPair extends MotorBase { + private wheelRadius: number; + private baseLength: number; constructor(ports: Output) { super(ports, () => this.__init(), (speed) => this.__setSpeed(speed), (steps, stepsOrTime, speed) => this.__move(steps, stepsOrTime, speed)); + this.wheelRadius = 3; + this.baseLength = 12; this.markUsed(); } @@ -437,15 +441,79 @@ namespace motors { }); } + /** + * The Move Tank block can make a robot drive forward, backward, turn, or stop. + * Use the Move Tank block for robot vehicles that have two Large Motors, + * with one motor driving the left side of the vehicle and the other the right side. + * You can make the two motors go at different speeds or in different directions + * to make your robot turn. + * @param value the amount of movement, eg: 2 + * @param unit + * @param speedLeft the speed on the left motor, eg: 50 + * @param speedRight the speed on the right motor, eg: 50 + */ + //% blockId=motorPairTank block="tank %chassis|left %speedLeft|%|right %speedRight|%|for %value|%unit" + //% weight=9 blockGap=8 + //% speedLeft.min=-100 speedLeft=100 + //% speedRight.min=-100 speedRight=100 + //% inlineInputMode=inline + //% group="Chassis" + tank(speedLeft: number, speedRight: number, value: number, unit: MoveUnit) { + this.init(); + + speedLeft = Math.clamp(-100, 100, speedLeft >> 0); + speedRight = Math.clamp(-100, 100, speedRight >> 0); + + const speed = Math.abs(speedLeft) > Math.abs(speedRight) ? speedLeft : speedRight; + const turnRatio = speedLeft == speed + ? (100 - speedRight / speedLeft * 100) + : (speedLeft / speedRight * 100 - 100); + this.steer(turnRatio, speed, value, unit); + } + + /** + * Makes a differential drive robot move with a given speed (%) and rotation rate (deg/s) + * using a unicycle model. + * @param speed speed of the center point between motors, eg: 10 + * @param rotationSpeed rotation of the robot around the center point, eg: 30 + * @param value the amount of movement, eg: 2 + * @param unit + */ + //% blockId=motorDrive block="drive %chassis|at %speed|cm/s|turning %rotationSpeed|deg/s|for %value|%unit" + //% inlineInputMode=inline + //% group="Chassis" + //% weight=8 blockGap=8 + drive(speed: number, rotationSpeed: number, value: number, unit: MoveUnit) { + this.init(); + + // speed is expressed in % + const R = this.wheelRadius; // cm + const L = this.baseLength; // cm + const PI = 3.14; + const maxw = 170 / 60 * 2 * PI; // rad / s + const maxv = maxw * R; // cm / s + // speed is cm / s + const v = speed; // cm / s + const w = rotationSpeed / 360 * 2 * PI; // rad / s + + const vr = (2 * v + w * L) / (2 * R); // rad / s + const vl = (2 * v - w * L) / (2 * R); // rad / s + + const sr = vr / maxw * 100; // % + const sl = vl / maxw * 100; // % + + this.tank(sr, sl, value, unit) + } + /** * Turns the motor and the follower motor by a number of rotations - * @param turnRatio the ratio of power sent to the follower motor, from ``-200`` to ``200``, eg: 100 + * @param turnRatio the ratio of power sent to the follower motor, from ``-200`` to ``200``, eg: 0 * @param speed the speed from ``100`` full forward to ``-100`` full backward, eg: 50 * @param value the move quantity, eg: 2 * @param unit the meaning of the value */ - //% blockId=motorPairTurn block="steer %chassis|%turnRatio|%|at speed %speed|%|by %value|%unit" - //% weight=9 blockGap=8 + //% blockId=motorPairTurn block="steer %chassis turn by|%turnRatio|at speed %speed|%|for %value|%unit" + //% weight=6 blockGap=8 //% turnRatio.min=-200 turnRatio=200 //% inlineInputMode=inline //% group="Chassis" @@ -482,30 +550,17 @@ namespace motors { stepsOrTime: stepsOrTime, useBrake: this._brake }); - } + } /** - * The Move Tank block can make a robot drive forward, backward, turn, or stop. - * Use the Move Tank block for robot vehicles that have two Large Motors, - * with one motor driving the left side of the vehicle and the other the right side. - * You can make the two motors go at different speeds or in different directions - * to make your robot turn. - * @param value the amount of movement, eg: 2 - * @param unit - * @param speedLeft the speed on the left motor, eg: 50 - * @param speedRight the speed on the right motor, eg: 50 + * Sets the wheels radius and base length of a directional drive robot + * @param wheelRadius + * @param baseLength */ - //% blockId=motorPairTank block="tank %chassis|left %speedLeft|%|right %speedRight|%|by %value|%unit" - //% weight=9 blockGap=8 - //% speedLeft.min=-100 speedLeft=100 - //% speedRight.min=-100 speedRight=100 - //% inlineInputMode=inline //% group="Chassis" - tank(speedLeft: number, speedRight: number, value: number, unit: MoveUnit) { - speedLeft = Math.clamp(-100, 100, speedLeft >> 0); - speedRight = Math.clamp(-100, 100, speedRight >> 0); - const turnRatio = speedLeft == 0 ? 0 : speedRight / speedLeft * 100; - this.steer(turnRatio, speedLeft, value, unit); + setDimensions(wheelRadius: number, baseLength: number): void { + this.wheelRadius = wheelRadius; + this.baseLength = baseLength; } /** @@ -513,6 +568,8 @@ namespace motors { */ //% toString(): string { + this.init(); + let r = outputToName(this._port); for (let i = 0; i < DAL.NUM_OUTPUTS; ++i) { if (this._port & (1 << i)) { diff --git a/sim/state/motornode.ts b/sim/state/motornode.ts index 3d80d389..cb805db5 100644 --- a/sim/state/motornode.ts +++ b/sim/state/motornode.ts @@ -80,6 +80,7 @@ namespace pxsim { stop() { this.started = false; + this.clearSpeedCmd(); } start() { @@ -145,14 +146,17 @@ namespace pxsim { const dstep = this.speedCmd == DAL.opOutputTimeSync ? pxsim.U.now() - this.speedCmdTime : this.tacho - this.speedCmdTacho; - if (dstep < stepsOrTime) + // 0 is special case, run infinite + if (!stepsOrTime || dstep < stepsOrTime) this.speed = speed; else { if (brake) this.speed = 0; this.clearSpeedCmd(); } // send synched motor state - otherMotor.speed = Math.floor(this.speed * turnRatio / 100); + otherMotor.speed = Math.floor(this.speed * turnRatio / 100); + if (!this._synchedMotor) + otherMotor.clearSpeedCmd(); break; } }