-
Notifications
You must be signed in to change notification settings - Fork 51
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
43 changed files
with
1,814 additions
and
393 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,5 +1,17 @@ | ||
| import { mirabuf } from "@/proto/mirabuf" | ||
|
|
||
| abstract class Stimulus { | ||
| private _info?: mirabuf.IInfo | ||
|
|
||
| constructor(info?: mirabuf.IInfo) { | ||
| this._info = info | ||
| } | ||
|
|
||
| public abstract Update(deltaT: number): void | ||
|
|
||
| public get info() { | ||
| return this._info | ||
| } | ||
| } | ||
|
|
||
| export default Stimulus |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,69 @@ | ||
| import World from "@/systems/World" | ||
| import EncoderStimulus from "../stimulus/EncoderStimulus" | ||
| import { SimCANEncoder, SimGyro } from "./WPILibBrain" | ||
| import Mechanism from "@/systems/physics/Mechanism" | ||
| import Jolt from "@barclah/jolt-physics" | ||
| import JOLT from "@/util/loading/JoltSyncLoader" | ||
|
|
||
| export interface SimInput { | ||
| Update: (deltaT: number) => void | ||
| } | ||
|
|
||
| export class SimEncoderInput implements SimInput { | ||
| private _device: string | ||
| private _stimulus: EncoderStimulus | ||
|
|
||
| constructor(device: string, stimulus: EncoderStimulus) { | ||
| this._device = device | ||
| this._stimulus = stimulus | ||
| } | ||
|
|
||
| public Update(_deltaT: number) { | ||
| SimCANEncoder.SetPosition(this._device, this._stimulus.positionValue) | ||
| SimCANEncoder.SetVelocity(this._device, this._stimulus.velocityValue) | ||
| } | ||
| } | ||
|
|
||
| export class SimGyroInput implements SimInput { | ||
| private _device: string | ||
| private _robot: Mechanism | ||
| private _joltID?: Jolt.BodyID | ||
| private _joltBody?: Jolt.Body | ||
|
|
||
| private static AXIS_X: Jolt.Vec3 = new JOLT.Vec3(1, 0, 0) | ||
| private static AXIS_Y: Jolt.Vec3 = new JOLT.Vec3(0, 1, 0) | ||
| private static AXIS_Z: Jolt.Vec3 = new JOLT.Vec3(0, 0, 1) | ||
|
|
||
| constructor(device: string, robot: Mechanism) { | ||
| this._device = device | ||
| this._robot = robot | ||
| this._joltID = this._robot.nodeToBody.get(this._robot.rootBody) | ||
|
|
||
| if (this._joltID) this._joltBody = World.PhysicsSystem.GetBody(this._joltID) | ||
| } | ||
|
|
||
| private GetAxis(axis: Jolt.Vec3): number { | ||
| return ((this._joltBody?.GetRotation().GetRotationAngle(axis) ?? 0) * 180) / Math.PI | ||
| } | ||
|
|
||
| private GetX(): number { | ||
| return this.GetAxis(SimGyroInput.AXIS_X) | ||
| } | ||
|
|
||
| private GetY(): number { | ||
| return this.GetAxis(SimGyroInput.AXIS_Y) | ||
| } | ||
|
|
||
| private GetZ(): number { | ||
| return this.GetAxis(SimGyroInput.AXIS_Z) | ||
| } | ||
|
|
||
| public Update(_deltaT: number) { | ||
| const x = this.GetX() | ||
| const y = this.GetY() | ||
| const z = this.GetZ() | ||
| SimGyro.SetAngleX(this._device, x) | ||
| SimGyro.SetAngleY(this._device, y) | ||
| SimGyro.SetAngleZ(this._device, z) | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,74 @@ | ||
| import Driver from "../driver/Driver" | ||
| import HingeDriver from "../driver/HingeDriver" | ||
| import SliderDriver from "../driver/SliderDriver" | ||
| import WheelDriver from "../driver/WheelDriver" | ||
| import { SimCAN, SimPWM, SimType } from "./WPILibBrain" | ||
|
|
||
| // TODO: Averaging is probably not the right solution (if we want large output groups) | ||
| // We can keep averaging, but we need a better ui for creating one to one (or just small) output groups | ||
| // The issue is that if a drivetrain is one output group, then each driver is given the average of all the motors | ||
| // We instead want a system where every driver gets (a) unique motor(s) that control it | ||
| // That way a single driver might get the average of two motors or something, if it has two motors to control it | ||
| // A system where motors a drivers are visually "linked" with "threads" in the UI would work well in my opinion | ||
| export abstract class SimOutputGroup { | ||
| public name: string | ||
| public ports: number[] | ||
| public drivers: Driver[] | ||
| public type: SimType | ||
|
|
||
| public constructor(name: string, ports: number[], drivers: Driver[], type: SimType) { | ||
| this.name = name | ||
| this.ports = ports | ||
| this.drivers = drivers | ||
| this.type = type | ||
| } | ||
|
|
||
| public abstract Update(deltaT: number): void | ||
| } | ||
|
|
||
| export class PWMOutputGroup extends SimOutputGroup { | ||
| public constructor(name: string, ports: number[], drivers: Driver[]) { | ||
| super(name, ports, drivers, SimType.PWM) | ||
| } | ||
|
|
||
| public Update(_deltaT: number) { | ||
| const average = | ||
| this.ports.reduce((sum, port) => { | ||
| const speed = SimPWM.GetSpeed(`${port}`) ?? 0 | ||
| console.debug(port, speed) | ||
| return sum + speed | ||
| }, 0) / this.ports.length | ||
|
|
||
| this.drivers.forEach(d => { | ||
| if (d instanceof WheelDriver) { | ||
| d.accelerationDirection = average | ||
| } else if (d instanceof HingeDriver || d instanceof SliderDriver) { | ||
| d.accelerationDirection = average | ||
| } | ||
| d.Update(_deltaT) | ||
| }) | ||
| } | ||
| } | ||
|
|
||
| export class CANOutputGroup extends SimOutputGroup { | ||
| public constructor(name: string, ports: number[], drivers: Driver[]) { | ||
| super(name, ports, drivers, SimType.CANMotor) | ||
| } | ||
|
|
||
| public Update(deltaT: number): void { | ||
| const average = | ||
| this.ports.reduce((sum, port) => { | ||
| const device = SimCAN.GetDeviceWithID(port, SimType.CANMotor) | ||
| return sum + (device?.get("<percentOutput") ?? 0) | ||
| }, 0) / this.ports.length | ||
|
|
||
| this.drivers.forEach(d => { | ||
| if (d instanceof WheelDriver) { | ||
| d.accelerationDirection = average | ||
| } else if (d instanceof HingeDriver || d instanceof SliderDriver) { | ||
| d.accelerationDirection = average | ||
| } | ||
| d.Update(deltaT) | ||
| }) | ||
| } | ||
| } |
Oops, something went wrong.