diff --git a/app/src/lib/components/Visualization.svelte b/app/src/lib/components/Visualization.svelte index a1e838c..bdf66be 100644 --- a/app/src/lib/components/Visualization.svelte +++ b/app/src/lib/components/Visualization.svelte @@ -7,7 +7,7 @@ import { lerp, degToRad } from 'three/src/math/MathUtils'; import { GUI } from 'three/addons/libs/lil-gui.module.min.js'; import Kinematic, { type body_state_t } from '$lib/kinematic'; - import {EightPhaseWalkState, FourPhaseWalkState, IdleState, RestState, StandState} from '$lib/gait' + import {CalibrationState, EightPhaseWalkState, FourPhaseWalkState, IdleState, RestState, StandState} from '$lib/gait' import { radToDeg } from 'three/src/math/MathUtils.js'; import type { URDFRobot } from 'urdf-loader'; import { get } from 'svelte/store'; @@ -35,7 +35,9 @@ let kinematic = new Kinematic() let planners = { + [ModesEnum.Deactivated]: new IdleState(), [ModesEnum.Idle]: new IdleState(), + [ModesEnum.Calibration]: new CalibrationState(), [ModesEnum.Rest]: new RestState(), [ModesEnum.Stand]: new StandState(), [ModesEnum.Crawl]: new EightPhaseWalkState(), diff --git a/app/src/lib/gait.ts b/app/src/lib/gait.ts index 7c7ab18..b408c3d 100644 --- a/app/src/lib/gait.ts +++ b/app/src/lib/gait.ts @@ -66,6 +66,21 @@ export class IdleState extends GaitState { protected name = 'Idle'; } +export class CalibrationState extends GaitState { + protected name = 'Calibration'; + + step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) { + body_state.omega = 0; + body_state.phi = 0; + body_state.psi = 0; + body_state.xm = 0; + body_state.ym = this.default_height * 10; + body_state.zm = 0; + body_state.feet = this.default_feet_pos; + return body_state; + } +} + export class RestState extends GaitState { protected name = 'Rest';