path-generator
Version:
A easy motion profile path generator
90 lines (89 loc) • 3.9 kB
JavaScript
;
Object.defineProperty(exports, "__esModule", { value: true });
var setpoint_1 = require("../motionProfiling/setpoint");
var coord_1 = require("../motionProfiling/coord");
var util_1 = require("../util");
var TankModifier = /** @class */ (function () {
function TankModifier(source, coords, startAngle, pathConfig) {
this._rightSetpoints = [];
this._leftSetpoints = [];
this._rightCoords = [];
this._leftCoords = [];
this.source = [];
this._coords = [];
this.startAngle = 0;
this.source = source;
this._coords = coords;
this.startAngle = startAngle;
this.pathConfig = pathConfig;
}
Object.defineProperty(TankModifier.prototype, "leftSetpoints", {
get: function () {
return this._rightSetpoints;
},
enumerable: false,
configurable: true
});
Object.defineProperty(TankModifier.prototype, "rightSetpoints", {
get: function () {
return this._leftSetpoints;
},
enumerable: false,
configurable: true
});
Object.defineProperty(TankModifier.prototype, "coords", {
get: function () {
return this._coords;
},
enumerable: false,
configurable: true
});
TankModifier.prototype.turnInPlace = function () {
var _this = this;
this.source.forEach(function (setpoint, index) {
_this._rightSetpoints.push(Object.assign(new setpoint_1.default(), setpoint));
_this._leftSetpoints.push(Object.assign(new setpoint_1.default(), setpoint));
_this._leftSetpoints[index].changeDirection();
});
this.updateCoordsAngle();
};
TankModifier.prototype.spline = function () {
var _this = this;
var robotWidth = this.pathConfig.radios / 2;
this.source.forEach(function (setpoint, index) {
_this._leftSetpoints.push(Object.assign(new setpoint_1.default(), setpoint));
_this._rightSetpoints.push(Object.assign(new setpoint_1.default(), setpoint));
_this._leftCoords.push(_this.getCoord(_this._coords[index], robotWidth));
_this._rightCoords.push(_this.getCoord(_this._coords[index], -robotWidth));
if (index > 0) {
_this.calculateSetpoint(_this._leftSetpoints, _this._leftCoords, index);
_this.calculateSetpoint(_this._rightSetpoints, _this._rightCoords, index);
}
});
this.updateCoordsAngle();
};
TankModifier.prototype.getCoord = function (coord, robotWidth) {
var cos_angle = Math.cos(coord.angle);
var sin_angle = Math.sin(coord.angle);
return new coord_1.default(coord.x - robotWidth * sin_angle, coord.y + robotWidth * cos_angle, coord.angle);
};
TankModifier.prototype.calculateSetpoint = function (setpoints, coords, index) {
var setpoint = setpoints[index];
var coord = coords[index];
var lastSetpoint = setpoints[index - 1];
var lastCoord = coords[index - 1];
var distance = Math.sqrt((coord.x - lastCoord.x) * (coord.x - lastCoord.x) + (coord.y - lastCoord.y) * (coord.y - lastCoord.y));
setpoint.position = lastSetpoint.position + distance;
setpoint.velocity = distance / this.pathConfig.robotLoopTime;
setpoint.acceleration = (setpoint.velocity - lastSetpoint.velocity) / this.pathConfig.robotLoopTime;
};
TankModifier.prototype.updateCoordsAngle = function () {
var _this = this;
this._coords.forEach(function (coord, index) {
var distance = _this.leftSetpoints[index].position - _this.rightSetpoints[index].position;
coord.angle = _this.startAngle + util_1.r2d(distance / _this.pathConfig.radios);
});
};
return TankModifier;
}());
exports.default = TankModifier;