c_meyer/pxt-ev3
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Differential drive (#164)

Browse Source 
* clear sync speed cmd

* differential drive model

* use cm/s

* fixed aggressive clearing of motor sync command

* better computation of turn artio

* improved robot dimensions

* moving block up

* hanbdle infinite case

* correct handling of inifinte time/step

* better stop handling
This commit is contained in:
Peli de Halleux 2018-01-03 00:27:05 -08:00 committed by GitHub
parent dd9cf9014f
commit 4d223374b5
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 96 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libs/core/_locales/core-jsdoc-strings.json
View File

@ -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.",

5
libs/core/_locales/core-strings.json
View File

@ -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",

103
libs/core/output.ts
View File

@ -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)) {

8
sim/state/motornode.ts
View File

@ -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;
}
}