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
4 changed files with 96 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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