Compare commits

...
Author SHA1 Message Date
snacj dc78ab80cb Fixed Formatting 2025-10-01 14:55:14 +02:00
snacj af21d05e6a Fixed merge issues 2025-10-01 14:49:51 +02:00
snacj aa54f94e94 fixed merge issues 2025-10-01 14:14:53 +02:00
snacj e00b0d2c3e Added Buffer Logic 2025-10-01 14:13:22 +02:00
snacj b338d71874 Changed Buffer Winder Mode 2025-10-01 14:13:22 +02:00
snacj fb650216d2 Added Buffer Mode to Winder frontend 2025-10-01 14:12:45 +02:00
snacj da823197d5 Finished Homing Routine 2025-10-01 14:12:45 +02:00
snacj fe5f109c89 Fixed step converter errors 2025-10-01 14:11:53 +02:00
snacj 1d86a42fb6 Fixed Merge Conflicts 2025-10-01 14:11:11 +02:00
snacj fb8eee4b8d Resolved merge conflicts 2025-10-01 14:09:59 +02:00
snacj 24cd1d857f Implemented Buffer state for Winder 2025-10-01 14:07:38 +02:00
snacj b7c4e1939d Changed Formatting and Removed unused imports 2025-10-01 14:06:35 +02:00
snacj 7d0a683c4b Added Puller frontend functions, hooks and Controlpage for Buffer 2025-10-01 14:04:25 +02:00
snacj fde51a95c8 Refactored to clean Act function in Winder2 2025-10-01 14:00:58 +02:00
snacj d88f45f81f Implemented Buffer Business Logic 2025-10-01 13:59:51 +02:00
snacj 8e0005d447 Added start of business logic for buffer lift 2025-10-01 13:56:22 +02:00
snacj 2bfa75810f Fixed Formatting 2025-10-01 13:30:52 +02:00
snacj b86130ec0d Fixed Rebase Issues and removed unused imports 2025-10-01 12:41:34 +02:00
snacj ad9305e1fd Fixed Formatting 2025-10-01 12:33:02 +02:00
snacj 191d022270 Fixed target speed communication between buffer and winder. 2025-10-01 12:33:02 +02:00
snacj 2d3a69852f Fixed merge issue 2025-10-01 12:33:02 +02:00
snacj d145bf547e Finished Homing Routine 2025-10-01 12:33:02 +02:00
snacj ed90b253f7 Removed unused top and bottom limits 2025-10-01 12:32:48 +02:00
snacj 4e78c6c912 Removed "Traverse Bar" 2025-10-01 12:32:48 +02:00
snacj 1efebc4a27 Fixed mode 2025-10-01 12:32:48 +02:00
snacj ea7d799cfc Fixed step converter errors 2025-10-01 12:32:48 +02:00
snacj 54ac5678ad Added Capability checks for moving 2025-10-01 12:32:48 +02:00
snacj 19ebbb77ac Fixed Comments 2025-10-01 12:32:48 +02:00
snacj 4bb2a73f6c Fixed State 2025-10-01 12:32:48 +02:00
snacj 972562b1d6 Added Control Functions 2025-10-01 12:32:48 +02:00
snacj 2a68a2da8f Adjusted Values and restructured controls 2025-10-01 12:32:48 +02:00
snacj 2d7b71ef5d Added Lift State frontend 2025-10-01 12:32:48 +02:00
snacj 0398bd0242 Added Lift Control functions 2025-10-01 12:32:48 +02:00
snacj 74e83d7cf5 Added Lift State and Mutations 2025-10-01 12:32:48 +02:00
snacj 25a1d3515d Added Lift State and Mutations 2025-10-01 12:32:48 +02:00
snacj 81d4b162e9 Fixed update speed function 2025-10-01 12:32:48 +02:00
snacj a790f15db9 Added Homing Routine Functions 2025-10-01 12:32:48 +02:00
snacj 50d7373f02 Fixed Merge Conflicts 2025-10-01 12:32:48 +02:00
snacj f1cd0c6216 Resolved merge conflicts 2025-10-01 12:32:32 +02:00
snacj eb7724a756 Implemented Buffer state for Winder 2025-10-01 12:31:56 +02:00
snacj df88eded18 Updated End Switch Logic 2025-10-01 12:31:34 +02:00
snacj 503a6b493e Fixed Formatting 2025-10-01 12:31:34 +02:00
snacj 4a7bb2abfd Added Digital Input switch for end Switch (Top) at Buffer 2025-10-01 12:31:34 +02:00
snacj 12a42eb0a6 Changed Formatting and Removed unused imports 2025-10-01 12:31:34 +02:00
snacj d3981d7276 Changed Mode names for Buffer + Added Hold Mode 2025-10-01 12:30:49 +02:00
snacj 89e702275e Added Comment 2025-10-01 12:30:49 +02:00
snacj 4a1239fa27 Update sync of puller speed and buffer input speed & Added description to helper function 2025-10-01 12:30:49 +02:00
snacj bd22c39b4a Added settings page to buffer route 2025-10-01 12:30:49 +02:00
snacj 96edca9f66 Added helper function to get Connected Machine reference. --Needs testing 2025-10-01 12:30:28 +02:00
snacj 2d198e0a44 Added Puller frontend functions, hooks and Controlpage for Buffer 2025-10-01 12:30:28 +02:00
snacj 9107e1372a Implemented Puller functions in Buffer Mod 2025-10-01 12:30:01 +02:00
snacj 8268eff39b Added pullerSpeed State Event 2025-10-01 12:30:01 +02:00
snacj 63910ab5be Renamed Function 2025-10-01 12:30:01 +02:00
snacj 91fdd7eb1e Added call to winder on buffering 2025-10-01 12:30:01 +02:00
snacj 8aa86a91ff Fixed Formatting 2025-10-01 12:30:01 +02:00
snacj 6f125ff0ab Refactored to clean Act function in Winder2 2025-10-01 12:30:01 +02:00
snacj 256c032b7c Changed updating speed logic to use JerkSpeedController 2025-10-01 12:29:36 +02:00
snacj 46a794d323 Implemented Buffer Business Logic 2025-10-01 12:29:36 +02:00
snacj a18b10bf50 Updated Act function to implement puller sync and buffer input speed 2025-10-01 12:28:01 +02:00
snacj 405a1f3c2f Added Puller Speed Controller to Buffer 2025-10-01 12:28:01 +02:00
snacj 3e09a00000 Added start of business logic for buffer lift 2025-10-01 12:28:01 +02:00
snacj b4ee86a88c Added current_input_speed state to test the calculation of buffer_lift_speed 2025-10-01 12:27:15 +02:00
snacj 1eeb3f8190 Changed name buffer_tower_controller -> buffer-lift_controller 2025-10-01 12:27:15 +02:00
19 changed files with 1619 additions and 112 deletions

No files matched your search

@@ -5,15 +5,37 @@ import React from "react";
import { useBuffer1 } from "./useBuffer1";
import { ControlCard } from "@/control/ControlCard";
import { SelectionGroup } from "@/control/SelectionGroup";
import { EditValue } from "@/control/EditValue";
import { TimeSeriesValueNumeric } from "@/control/TimeSeriesValue";
import { roundToDecimals } from "@/lib/decimal";
import { Label } from "@/control/Label";
import { TouchButton } from "@/components/touch/TouchButton";
import { StatusBadge } from "@/control/StatusBadge";
export function Buffer1ControlPage() {
const { state, setBufferMode } = useBuffer1();
const {
state,
defaultState,
setBufferMode,
setCurrentInputSpeed,
pullerSpeed,
setPullerRegulationMode,
setPullerTargetSpeed,
liftPosition,
gotoLiftHome,
isLoading,
isDisabled,
} = useBuffer1();
const current_input_speed =
state?.current_input_speed_state.current_input_speed ?? 0.0;
const default_speed = 0.0;
return (
<Page>
<ControlGrid>
<ControlCard className="bg-red" title="Mode">
<SelectionGroup<"Standby" | "FillingBuffer" | "EmptyingBuffer">
<SelectionGroup<"Standby" | "Hold" | "Filling" | "Emptying">
value={state?.mode_state.mode}
orientation="vertical"
className="grid h-full grid-cols-2 gap-2"
@@ -24,13 +46,19 @@ export function Buffer1ControlPage() {
isActiveClassName: "bg-green-600",
className: "h-full",
},
FillingBuffer: {
children: "FillingBuffer",
Hold: {
children: "Hold",
icon: "lu:Pause",
isActiveClassName: "bg-green-600",
className: "h-full",
},
Filling: {
children: "Filling",
icon: "lu:Flame",
isActiveClassName: "bg-green-600",
className: "h-full",
},
EmptyingBuffer: {
Emptying: {
children: "EmptyingBuffer",
icon: "lu:ArrowBigLeftDash",
isActiveClassName: "bg-green-600",
@@ -40,6 +68,83 @@ export function Buffer1ControlPage() {
onChange={setBufferMode}
/>
</ControlCard>
<ControlCard title="Set Current Input Speed - (For Debugging)">
<EditValue
title="Input Speed"
unit="m/min"
value={current_input_speed}
defaultValue={default_speed}
min={0.0}
max={60.0}
step={0.1}
renderValue={(value) => roundToDecimals(value, 1)}
onChange={setCurrentInputSpeed}
/>
</ControlCard>
<ControlCard className="bg-red" height={2} title="Lift">
<TimeSeriesValueNumeric
label="Position"
unit="mm"
timeseries={liftPosition}
renderValue={(value) => roundToDecimals(value, 1)}
/>
<Label label="Home">
<TouchButton
variant="outline"
icon="lu:House"
onClick={() => gotoLiftHome()}
disabled={isDisabled}
isLoading={isLoading || state?.lift_state?.is_going_home}
>
Go to Home
</TouchButton>
{state?.lift_state?.is_homed !== true ? (
<StatusBadge variant={"error"}>{"Not Homed"}</StatusBadge>
) : null}
</Label>
</ControlCard>
<ControlCard className="bg-red" title="Puller">
<TimeSeriesValueNumeric
label="Speed"
unit="m/min"
timeseries={pullerSpeed}
renderValue={(value) => roundToDecimals(value, 1)}
/>
<Label label="Regulation">
<SelectionGroup
value={state?.puller_state?.regulation}
options={{
Speed: {
children: "Speed",
icon: "lu:Gauge",
},
Diameter: {
children: "Diameter",
icon: "lu:Sun",
disabled: true,
},
}}
onChange={setPullerRegulationMode}
disabled={isDisabled}
loading={isLoading}
/>
</Label>
<Label label="Target Speed">
<EditValue
value={state?.puller_state?.target_speed}
unit="m/min"
title="Target Speed"
defaultValue={defaultState?.puller_state?.target_speed}
min={0}
max={75}
step={0.1}
renderValue={(value) => roundToDecimals(value, 1)}
onChange={setPullerTargetSpeed}
/>
</Label>
</ControlCard>
</ControlGrid>
</Page>
);
@@ -16,23 +16,55 @@ import {
ThrottledStoreUpdater,
} from "../../../client/socketioStore";
import { MachineIdentificationUnique } from "@/machines/types";
import {
createTimeSeries,
TimeSeries,
TimeSeriesValue,
} from "@/lib/timeseries";
// ========== Event Schema Definitions ==========
/**
* Machine operation mode enum
*/
export const modeSchema = z.enum([
"Standby",
"FillingBuffer",
"EmptyingBuffer",
]);
export const modeSchema = z.enum(["Standby", "Hold", "Filling", "Emptying"]);
export type Mode = z.infer<typeof modeSchema>;
/**
* Consolidated live values event schema (30FPS data)
*/
export const liveValuesEventDataSchema = z.object({});
export const liveValuesEventDataSchema = z.object({
lift_position: z.number().nullable(),
puller_speed: z.number(),
});
/**
* Puller regulation type enum
*/
export const pullerRegulationSchema = z.enum(["Speed", "Diameter"]);
export type PullerRegulation = z.infer<typeof pullerRegulationSchema>;
/**
* Puller state event schmema
*/
export const pullerStateSchema = z.object({
regulation: pullerRegulationSchema,
target_speed: z.number(),
target_diameter: z.number(),
forward: z.boolean(),
});
/**
* Lift state schema
*/
export const liftStateSchema = z.object({
is_going_up: z.boolean(),
is_going_down: z.boolean(),
is_homed: z.boolean(),
is_going_home: z.boolean(),
is_buffering: z.boolean(),
step_size: z.number(),
});
/**
* Mode state event schema
@@ -41,6 +73,10 @@ export const modeStateSchema = z.object({
mode: modeSchema,
});
export const currentInputSpeedSchema = z.object({
current_input_speed: z.number(),
});
/**
* Connected machine state scheme
*/
@@ -65,7 +101,10 @@ export const connectedMachineStateSchema = z.object({
export const stateEventDataSchema = z.object({
mode_state: modeStateSchema,
lift_state: liftStateSchema,
puller_state: pullerStateSchema,
connected_machine_state: connectedMachineStateSchema,
current_input_speed_state: currentInputSpeedSchema,
});
// ========== Event Schemas with Wrappers ==========
@@ -80,8 +119,24 @@ export type StateEvent = z.infer<typeof stateEventSchema>;
export type Buffer1NamespaceStore = {
// State events (latest only)
state: StateEvent | null;
defaultState: StateEvent | null;
// Time series data for live values
liftPosition: TimeSeries;
pullerSpeed: TimeSeries;
};
// Constants for time durations
const TWENTY_MILLISECOND = 20;
const ONE_SECOND = 1000;
const FIVE_SECOND = 5 * ONE_SECOND;
const ONE_HOUR = 60 * 60 * ONE_SECOND;
const { initialTimeSeries: pullerSpeed, insert: addPullerSpeed } =
createTimeSeries(TWENTY_MILLISECOND, ONE_SECOND, FIVE_SECOND, ONE_HOUR);
const { initialTimeSeries: liftPosition, insert: addLiftPosition } =
createTimeSeries(TWENTY_MILLISECOND, ONE_SECOND, FIVE_SECOND, ONE_HOUR);
/**
* Creates a message handler for Buffer1 namespace events with validation and appropriate caching strategies
* @param store The store to update when messages are received
@@ -110,12 +165,41 @@ export function buffer1MessageHandler(
...state,
state: stateEvent,
}));
console.log(stateEvent);
} else if (eventName === "LiveValuesEvent") {
// Parse and validate the live values event
const liveValuesEvent = liveValuesEventSchema.parse(event);
const timestamp = event.ts;
updateStore((state) => ({
...state,
}));
// Extract values and add to time series
const { lift_position, puller_speed } = liveValuesEvent.data;
const timestamp = liveValuesEvent.ts;
updateStore((state) => {
const newState = { ...state };
// Add lift position if not null
if (lift_position !== null) {
const timeseriesValue: TimeSeriesValue = {
value: lift_position,
timestamp,
};
newState.liftPosition = addLiftPosition(
state.liftPosition,
timeseriesValue,
);
}
// Add puller speed
const pullerSpeedValue: TimeSeriesValue = {
value: puller_speed,
timestamp,
};
newState.pullerSpeed = addPullerSpeed(
state.pullerSpeed,
pullerSpeedValue,
);
return newState;
});
} else {
handleUnhandledEventError(eventName);
}
@@ -134,7 +218,13 @@ export const createBuffer1NamespaceStore =
(): StoreApi<Buffer1NamespaceStore> =>
create<Buffer1NamespaceStore>(() => {
return {
// State event from server
state: null,
defaultState: null,
// Time series data for live values
liftPosition,
pullerSpeed,
};
});
@@ -8,7 +8,13 @@ import {
import { buffer1SerialRoute } from "@/routes/routes";
import { z } from "zod";
import { useEffect, useMemo } from "react";
import { StateEvent, Mode, useBuffer1Namespace } from "./buffer1Namespace";
import {
StateEvent,
Mode,
useBuffer1Namespace,
pullerRegulationSchema,
PullerRegulation,
} from "./buffer1Namespace";
import { useStateOptimistic } from "@/lib/useStateOptimistic";
import { produce } from "immer";
import { useMachines } from "@/client/useMachines";
@@ -45,7 +51,8 @@ export function useBuffer1() {
const machine_identification_unique = machineIdentification;
// Get consolidated state and live values from namespace
const { state } = useBuffer1Namespace(machineIdentification);
const { state, defaultState, liftPosition, pullerSpeed } =
useBuffer1Namespace(machineIdentification);
// Single optimistic state for all state management
const stateOptimistic = useStateOptimistic<StateEvent>();
@@ -122,10 +129,102 @@ export function useBuffer1() {
);
};
const setCurrentInputSpeed = (current_input_speed: number) => {
updateStateOptimistically(
(current) => {
current.data.current_input_speed_state.current_input_speed =
current_input_speed;
},
() =>
requestSetCurrentInputSpeed({
machine_identification_unique,
data: { SetCurrentInputSpeed: current_input_speed },
}),
);
};
const setPullerTargetSpeed = (targetSpeed: number) => {
updateStateOptimistically(
(current) => {
current.data.puller_state.target_speed = targetSpeed;
},
() =>
requestPullerSetTargetSpeed({
machine_identification_unique: machineIdentification,
data: { SetPullerTargetSpeed: targetSpeed },
}),
);
};
const setPullerTargetDiameter = (targetDiameter: number) => {
updateStateOptimistically(
(current) => {
current.data.puller_state.target_diameter = targetDiameter;
},
() =>
requestPullerSetTargetDiameter({
machine_identification_unique: machineIdentification,
data: { SetPullerTargetDiameter: targetDiameter },
}),
);
};
const setPullerRegulationMode = (regulationMode: PullerRegulation) => {
updateStateOptimistically(
(current) => {
current.data.puller_state.regulation = regulationMode;
},
() =>
requestPullerSetRegulationMode({
machine_identification_unique: machineIdentification,
data: { SetPullerRegulationMode: regulationMode },
}),
);
};
const setPullerForward = (forward: boolean) => {
updateStateOptimistically(
(current) => {
current.data.puller_state.forward = forward;
},
() =>
requestPullerSetForward({
machine_identification_unique: machineIdentification,
data: { SetPullerForward: forward },
}),
);
};
const gotoLiftHome = () => {
updateStateOptimistically(
(current) => {
current.data.lift_state.is_going_home = true;
},
() =>
requestLiftGotoHome({
machine_identification_unique: machineIdentification,
data: "GotoLiftHome",
}),
);
};
const setLiftStepSize = (stepSize: number) => {
updateStateOptimistically(
(current) => {
current.data.lift_state.step_size = stepSize;
},
() =>
requestLiftSetStepSize({
machine_identification_unique: machineIdentification,
data: { SetLiftStepSize: stepSize },
}),
);
};
// Mutation hooks
const { request: requestBufferMode } = useMachineMutation(
z.object({
SetBufferMode: z.enum(["Standby", "FillingBuffer", "EmptyingBuffer"]),
SetBufferMode: z.enum(["Standby", "Hold", "Filling", "Emptying"]),
}),
);
@@ -141,6 +240,38 @@ export function useBuffer1() {
}),
);
const { request: requestSetCurrentInputSpeed } = useMachineMutation(
z.object({
SetCurrentInputSpeed: z.number(),
}),
);
const { request: requestPullerSetTargetSpeed } = useMachineMutation(
z.object({ SetPullerTargetSpeed: z.number() }),
);
const { request: requestPullerSetTargetDiameter } = useMachineMutation(
z.object({ SetPullerTargetDiameter: z.number() }),
);
const { request: requestPullerSetRegulationMode } = useMachineMutation(
z.object({
SetPullerRegulationMode: pullerRegulationSchema,
}),
);
const { request: requestPullerSetForward } = useMachineMutation(
z.object({ SetPullerForward: z.boolean() }),
);
const { request: requestLiftGotoHome } = useMachineMutation(
z.literal("GotoLiftHome"),
);
const { request: requestLiftSetStepSize } = useMachineMutation(
z.object({ SetLiftStepSize: z.number() }),
);
// Calculate loading states
const isLoading = stateOptimistic.isOptimistic;
const isDisabled = !stateOptimistic.isInitialized;
// General Helper functions
const machines = useMachines();
// Filter machines for the correct type
@@ -176,11 +307,27 @@ export function useBuffer1() {
filteredMachines,
selectedMachine,
// Default state for initial values
defaultState: defaultState?.data,
// Individual live values (TimeSeries)
liftPosition,
pullerSpeed,
// Loading states
isLoading,
isDisabled,
// Action functions (verb-first)
setBufferMode,
gotoLiftHome,
setLiftStepSize,
setConnectedMachine,
disconnectMachine,
setCurrentInputSpeed,
setPullerTargetSpeed,
setPullerTargetDiameter,
setPullerRegulationMode,
setPullerForward,
};
}
@@ -50,6 +50,7 @@ export function Winder2ControlPage() {
gotoTraverseHome,
setSpoolAutomaticRequiredMeters,
setSpoolAutomaticAction,
startBuffering,
isLoading,
isDisabled,
} = useWinder2();
@@ -343,6 +344,18 @@ export function Winder2ControlPage() {
/>
</Label>
</ControlCard>
{state?.connected_machine_state.is_available && (
<ControlCard>
<TouchButton
variant="outline"
onClick={startBuffering}
disabled={isDisabled}
isLoading={isLoading}
>
Change Spool
</TouchButton>
</ControlCard>
)}
</ControlGrid>
{/* Reset Progress Confirmation Dialog */}
@@ -154,22 +154,22 @@ export function useWinder2() {
const { request: requestSpoolResetProgress } = useMachineMutation(
z.literal("ResetSpoolProgress"),
);
const { request: requestSpoolAutomaticAction } = useMachineMutation(
z.object({ SetSpoolAutomaticAction: spoolAutomaticActionModeSchema }),
);
const { request: requestConnectedMachine } = useMachineMutation(
z.object({
SetConnectedMachine: machineIdentificationUnique,
}),
);
const { request: requestDisconnectedMachine } = useMachineMutation(
z.object({
DisconnectMachine: machineIdentificationUnique,
}),
);
const { request: requestStartBuffering } = useMachineMutation(
z.literal("StartBuffering"),
);
// Helper function for optimistic updates using produce
const updateStateOptimistically = (
@@ -550,6 +550,19 @@ export function useWinder2() {
);
};
const startBuffering = () => {
updateStateOptimistically(
(current) => {
current.data.buffer_state.buffering = true;
},
() =>
requestStartBuffering({
machine_identification_unique: machineIdentification,
data: "StartBuffering",
}),
);
};
// Calculate loading states
const isLoading = stateOptimistic.isOptimistic;
const isDisabled = !stateOptimistic.isInitialized;
@@ -631,5 +644,6 @@ export function useWinder2() {
setSpoolAdaptiveDeaccelerationUrgencyMultiplier,
setConnectedMachine,
disconnectMachine,
startBuffering,
};
}
@@ -133,6 +133,13 @@ export const connectedMachineStateSchema = z.object({
is_available: z.boolean(),
});
/**
* Buffer state schema
*/
export const bufferStateSchema = z.object({
buffering: z.boolean(),
});
/**
* Tension arm state schema
*/
@@ -166,6 +173,7 @@ export const stateEventDataSchema = z.object({
spool_speed_controller_state: spoolSpeedControllerStateSchema,
spool_automatic_action_state: spoolAutomaticActionStateSchema,
connected_machine_state: connectedMachineStateSchema,
buffer_state: bufferStateSchema,
});
// ========== Event Schemas with Wrappers ==========
+10 -7
View File
@@ -192,12 +192,6 @@ export const buffer1ControlRoute = createRoute({
component: () => <Buffer1ControlPage />,
});
export const mock1PresetsRoute = createRoute({
getParentRoute: () => mock1SerialRoute,
path: "presets",
component: () => <Mock1PresetsPage />,
});
export const aquapath1SerialRoute = createRoute({
getParentRoute: () => machinesRoute,
path: "aquapath1/$serial",
@@ -222,6 +216,12 @@ export const buffer1SettingsRoute = createRoute({
component: () => <Buffer1SettingsPage />,
});
export const mock1PresetsRoute = createRoute({
getParentRoute: () => mock1SerialRoute,
path: "presets",
component: () => <Mock1PresetsPage />,
});
export const setupRoute = createRoute({
getParentRoute: () => sidebarRoute,
path: "setup",
@@ -342,7 +342,10 @@ export const rootTree = RootRoute.addChildren([
mock1PresetsRoute,
]),
buffer1SerialRoute.addChildren([buffer1ControlRoute]),
buffer1SerialRoute.addChildren([
buffer1ControlRoute,
buffer1SettingsRoute,
]),
]),
]),
]);
+16 -2
View File
@@ -4,9 +4,23 @@ use super::BufferV1;
use control_core::machines::new::MachineAct;
impl MachineAct for BufferV1 {
fn act(&mut self, now: Instant) {
// sync the lift speed
self.sync_lift_speed(now);
// sync the puller speed
self.sync_puller_speed(now);
// sync the puller speed of winder
self.sync_winder_puller();
// check if lift state changed
if self.buffer_lift_controller.did_change_state() {
self.emit_state();
}
// if last measurement is older than 1 second, emit a new measurement
if now.duration_since(self.last_measurement_emit) > Duration::from_secs_f64(1.0 / 30.0) {
// Emit live values at 30 FPS
if now.duration_since(self.last_measurement_emit) > Duration::from_secs_f64(1.0 / 60.0) {
// Emit live values at 60 FPS
self.emit_live_values();
self.last_measurement_emit = now;
+73 -3
View File
@@ -1,5 +1,7 @@
use std::{sync::Arc, time::Duration};
use crate::machines::buffer1::puller_speed_controller::PullerRegulationMode;
use super::{BufferV1, BufferV1Mode};
use control_core::{
machines::{api::MachineApi, identification::MachineIdentificationUnique},
@@ -18,7 +20,12 @@ use socketioxide::extract::SocketRef;
use tracing::instrument;
#[derive(Serialize, Debug, Clone, Default)]
pub struct LiveValuesEvent {}
pub struct LiveValuesEvent {
/// lift position in mm
pub lift_position: Option<f64>,
/// puller speed in m/min
pub puller_speed: f64,
}
impl LiveValuesEvent {
pub fn build(&self) -> Event<Self> {
@@ -30,6 +37,10 @@ impl LiveValuesEvent {
pub struct StateEvent {
/// mode state
pub mode_state: ModeState,
/// lift state
pub lift_state: LiftState,
/// puller state
pub puller_state: PullerState,
/// connected machine state
pub connected_machine_state: ConnectedMachineState,
}
@@ -53,8 +64,9 @@ pub struct ModeState {
#[derive(Serialize, Deserialize, Debug, Clone, PartialEq, Eq)]
pub enum Mode {
Standby,
FillingBuffer,
EmptyingBuffer,
Hold,
Filling,
Emptying,
}
#[derive(Serialize, Deserialize, Debug, Clone, PartialEq, Eq)]
@@ -64,6 +76,50 @@ pub struct ConnectedMachineState {
pub is_available: bool,
}
#[derive(Serialize, Deserialize, Debug, Clone)]
pub struct PullerState {
/// regulation type
pub regulation: PullerRegulationMode,
/// target speed in m/min
pub target_speed: f64,
/// target diameter in mm
pub target_diameter: f64,
/// forward rotation direction
pub forward: bool,
}
#[derive(Serialize, Deserialize, Debug, Clone, PartialEq)]
pub struct LiftState {
/// min position in mm
pub limit_top: f64,
/// max position in mm
pub limit_bottom: f64,
/// position in mm
pub position_in: f64,
/// position out in mm
pub position_out: f64,
/// is going to position in
pub is_going_up: bool,
/// is going to position out
pub is_going_down: bool,
/// if is homed
pub is_homed: bool,
/// if is homing
pub is_going_home: bool,
/// if is buffering
pub is_buffering: bool,
/// step size in mm
pub step_size: f64,
/// padding in mm
pub padding: f64,
/// can go top (to top limit)
pub can_go_top: bool,
/// can go bottom (to bottom limit)
pub can_go_bottom: bool,
/// can home
pub can_go_home: bool,
}
#[derive(Deserialize, Serialize)]
enum Mutation {
// Mode
@@ -74,6 +130,15 @@ enum Mutation {
// Disconnect Machine
DisconnectMachine(MachineIdentificationUnique),
// Lift
GotoLiftHome,
// Puller
SetPullerRegulationMode(PullerRegulationMode),
SetPullerTargetSpeed(f64),
SetPullerTargetDiameter(f64),
SetPullerForward(bool),
}
#[derive(Debug)]
@@ -128,6 +193,11 @@ impl MachineApi for BufferV1 {
Mutation::DisconnectMachine(machine_identification_unique) => {
self.disconnect_winder(machine_identification_unique);
}
Mutation::SetPullerRegulationMode(regulation) => self.puller_set_regulation(regulation),
Mutation::SetPullerTargetSpeed(value) => self.puller_set_target_speed(value),
Mutation::SetPullerTargetDiameter(_) => todo!(),
Mutation::SetPullerForward(value) => self.puller_set_forward(value),
Mutation::GotoLiftHome => self.buffer_lift_controller.goto_home(),
}
Ok(())
}
@@ -0,0 +1,542 @@
use std::time::Instant;
use control_core::{
controllers::first_degree_motion::linear_acceleration_speed_controller::LinearAccelerationLimitingController,
converters::linear_step_converter::LinearStepConverter,
};
use ethercat_hal::io::{
digital_input::DigitalInput, stepper_velocity_el70x1::StepperVelocityEL70x1,
};
use tracing::info;
use uom::{
ConstZero,
si::{
acceleration::centimeter_per_second_squared,
f64::{Acceleration, Length, Velocity},
length::millimeter,
velocity::millimeter_per_second,
},
};
#[derive(Debug)]
pub struct BufferLiftController {
/// Whether the speed controller is enabled or not
enabled: bool,
position: Length,
limit_top: Length,
limit_bottom: Length,
step_size: Length,
padding: Length,
/// Stepper driver. Controls buffer stepper motor
pub stepper_driver: StepperVelocityEL70x1,
// Step Converter
pub fullstep_converter: LinearStepConverter,
pub microstep_converter: LinearStepConverter,
/// Linear acceleration controller to dampen speed change
acceleration_controller: LinearAccelerationLimitingController,
/// Forward rotation direction. If false, applies negative sign to speed
forward: bool,
state: State,
/// Fixed constants
spool_amount: u8,
/// Variables
current_input_speed: Velocity,
target_output_speed: Velocity,
lift_speed: Velocity,
// A sticky flag if the [`State`] changed (not the sub states)
// Needed to send state updates to the UI
did_change_state: bool,
}
#[derive(Debug, PartialEq, Eq, Clone)]
pub enum State {
/// Initial state
NotHomed,
/// Doing nothing
/// Already homed
Idle,
/// Going to upper limit
///
/// After reaching the upper limit, the state will change to [`State::Idle`]
GoingUp,
/// Going to lower limit
///
/// After reaching the lower limit, the state will change to [`State::Idle`]
GoingDown,
/// Homing in progress
///
/// After homing is done, the state will change to [`State::Idle`]
Homing(HomingState),
/// Move at target speed to Buffer
Buffering(BufferingState),
}
#[derive(Debug, PartialEq, Eq, Clone)]
pub enum BufferingState {
GoingUp,
Filling,
Emptying,
}
#[derive(Debug, PartialEq, Eq, Clone)]
pub enum HomingState {
/// In this state the lift is not moving but checks if the endstop is triggered
/// If the endstop is triggered we go into [`HomingState::EscapeEndstop`]
/// If the endstop is not triggered we go into [`HomingState::FindEndstop`]
Initialize,
/// In this state the lift is moving out away from the endstop until it's not triggered anymore
/// The it goes into [`HomingState::FindEnstopFineDistancing`]
EscapeEndstop,
/// Moving out away from the endstop
/// Then Transition into [`HomingState::FindEndtopFine`]
FindEnstopFineDistancing,
/// In this state the lift is fast until it reaches the endstop
FindEndstopCoarse,
/// In this state the lift is moving slowly until it reaches the endstop
FindEndStopFine,
/// In this state we check if th current position is actually 0.0, if not we redo the homing routine
Validate(Instant),
}
impl BufferLiftController {
pub fn new(
driver: StepperVelocityEL70x1,
limit_top: Length,
limit_bot: Length,
microsteps: u8,
) -> Self {
Self {
enabled: false,
position: Length::ZERO,
limit_top,
limit_bottom: limit_bot,
step_size: Length::new::<millimeter>(1.75), // Default padding
padding: Length::new::<millimeter>(0.9), // Default padding
did_change_state: false,
stepper_driver: driver,
fullstep_converter: LinearStepConverter::from_diameter(
200,
Length::new::<millimeter>(32.22),
),
microstep_converter: LinearStepConverter::from_diameter(
200 * microsteps as i16,
Length::new::<millimeter>(32.22),
),
spool_amount: 13,
state: State::NotHomed,
forward: true,
acceleration_controller: LinearAccelerationLimitingController::new_simple(
Acceleration::new::<centimeter_per_second_squared>(1.0),
Velocity::new::<millimeter_per_second>(5.0),
),
current_input_speed: Velocity::ZERO,
target_output_speed: Velocity::ZERO,
lift_speed: Velocity::ZERO,
}
}
}
impl BufferLiftController {
/// Calculate the speed of the buffer lift from current input speed
///
/// Formula: input_speed / ( 2 * spool_amount - 1 )
pub fn calculate_buffer_lift_speed(&self) -> Velocity {
let lift_speed = Velocity::new::<millimeter_per_second>(
(self.current_input_speed.get::<millimeter_per_second>()
- self.target_output_speed.get::<millimeter_per_second>())
/ (2.0 * self.spool_amount as f64 - 1.0),
);
lift_speed
}
pub fn update_speed(
&mut self,
stepper_driver: &mut StepperVelocityEL70x1,
lift_end_stop: &DigitalInput,
t: Instant,
) -> Velocity {
let speed = match self.enabled {
true => self.get_speed(stepper_driver, lift_end_stop),
false => Velocity::ZERO,
};
// Stepper is installed in reverse direction so we change to sign
let speed = speed;
self.acceleration_controller.update(speed, t)
}
}
/// Getter & Setter
impl BufferLiftController {
pub fn set_enabled(&mut self, enabled: bool) {
self.enabled = enabled;
self.stepper_driver.set_enabled(enabled);
}
pub fn set_limit_top(&mut self, limit: Length) {
self.limit_top = limit;
}
pub fn set_limit_bottom(&mut self, limit: Length) {
self.limit_bottom = limit;
}
pub fn set_step_size(&mut self, step_size: Length) {
self.step_size = step_size;
}
pub fn set_padding(&mut self, padding: Length) {
self.padding = padding;
}
pub fn get_limit_top(&self) -> Length {
self.limit_top
}
pub fn get_limit_bottom(&self) -> Length {
self.limit_bottom
}
pub fn get_step_size(&self) -> Length {
self.step_size
}
pub fn get_padding(&self) -> Length {
self.padding
}
pub fn set_forward(&mut self, forward: bool) {
self.forward = forward;
}
pub fn get_current_position(&self) -> Option<Length> {
match self.is_homed() {
true => Some(self.position),
false => None,
}
}
pub fn did_change_state(&mut self) -> bool {
let did_change = self.did_change_state;
// Reset the flag
self.did_change_state = false;
did_change
}
pub fn set_current_input_speed(&mut self, speed: Velocity) {
self.current_input_speed = speed;
}
pub fn set_target_output_speed(&mut self, speed: Velocity) {
self.target_output_speed = speed;
}
pub fn get_current_input_speed(&self) -> Velocity {
self.current_input_speed
}
pub fn get_target_output_speed(&self) -> Velocity {
self.target_output_speed
}
pub fn get_lift_speed(&self) -> Velocity {
self.lift_speed
}
}
/// State management
impl BufferLiftController {
pub fn goto_limit_top(&mut self) {
self.state = State::GoingUp
}
pub fn goto_limit_bottom(&mut self) {
self.state = State::GoingDown
}
pub fn goto_home(&mut self) {
self.state = State::Homing(HomingState::Initialize);
}
pub fn start_buffering(&mut self) {
self.state = State::Buffering(BufferingState::GoingUp);
}
pub fn is_going_up(&self) -> bool {
// [`State::GoingUp`]
matches!(self.state, State::GoingUp)
}
pub fn is_going_down(&self) -> bool {
// [`State::GoingDown`]
matches!(self.state, State::GoingDown)
}
pub fn is_homed(&self) -> bool {
// if not [`State::NotHomed`], then it is homed
!matches!(self.state, State::NotHomed)
}
pub fn is_filling(&self) -> bool {
// [`State::Filling`]
matches!(self.state, State::Buffering(BufferingState::Filling))
}
pub fn is_emptying(&self) -> bool {
// [`State::Emptying`]
matches!(self.state, State::Buffering(BufferingState::Emptying))
}
pub fn is_going_home(&self) -> bool {
// [`State::Homing`]
matches!(self.state, State::Homing(_))
}
pub fn is_buffering(&self) -> bool {
// [`State::Buffering`]
matches!(self.state, State::Buffering(_))
}
}
impl BufferLiftController {
// If at inner limit within a tolerance
fn is_at_position(&self, target_position: Length, tolerance: Length) -> bool {
let upper_tolerance = target_position + tolerance.abs();
let lower_tolerance = target_position - tolerance.abs();
if self.position >= lower_tolerance && self.position <= upper_tolerance {
return true;
} else {
return false;
}
}
/// Calculates distance to position
fn distance_to_position(&self, target_position: Length) -> Length {
if self.position > target_position {
return self.position - target_position;
} else if self.position < target_position {
return target_position - self.position;
} else {
return Length::ZERO;
}
}
// Changes the direction of the speed based on the current position and target position
fn speed_to_position(&self, target_position: Length, absolute_speed: Velocity) -> Velocity {
// If we are over the target position we need to move negative
if self.position > target_position {
return -absolute_speed.abs();
} else if self.position < target_position {
return absolute_speed.abs();
} else {
return Velocity::ZERO;
}
}
/// Gets the current lift position as a [`Length`].
pub fn sync_position(&mut self, stepper_driver: &StepperVelocityEL70x1) {
let steps = stepper_driver.get_position();
self.position = self.microstep_converter.steps_to_distance(steps as f64);
}
/// Update the [`did_change_state`] flag
/// Only consider the major state not the sub states
fn update_did_change_state(&mut self, old_state: &State) -> bool {
match self.state {
State::NotHomed => !matches!(old_state, State::NotHomed),
State::Idle => !matches!(old_state, State::Idle),
State::GoingUp => !matches!(old_state, State::GoingUp),
State::GoingDown => !matches!(old_state, State::GoingDown),
State::Homing(_) => !matches!(old_state, State::Homing(_)),
State::Buffering(_) => !matches!(old_state, State::Buffering(_)),
}
}
/// Calculates a desired speed based on the current state and the end stop status.
///
/// Positive speed moves Up, negative speed moves down
fn get_speed(
&mut self,
stepper_driver: &mut StepperVelocityEL70x1,
lift_end_stop: &DigitalInput,
) -> Velocity {
// Don't move if not enabled or in a state that doesn't result in movement
if !self.enabled {
return Velocity::ZERO;
}
self.sync_position(stepper_driver);
// save state before
let old_state = self.state.clone();
// Automatic Transitions
match &self.state {
State::NotHomed => {}
State::Idle => {}
State::GoingDown => {
// If lower limit is reached
if self.is_at_position(self.limit_bottom, Length::new::<millimeter>(0.01)) {
// Put Into Idle
self.state = State::Idle;
}
}
State::GoingUp => {
// If upper limit is reached
if self.is_at_position(self.limit_top, Length::new::<millimeter>(0.01)) {
// Put Into Idle
self.state = State::Idle;
}
}
State::Homing(homing_state) => match homing_state {
HomingState::Initialize => {
// If endstop is triggered, escape the endstop
if lift_end_stop.get_value().unwrap_or(false) == true {
self.state = State::Homing(HomingState::EscapeEndstop);
} else {
// If endstop is not triggered, move to the endstop
self.state = State::Homing(HomingState::FindEndstopCoarse);
}
}
HomingState::EscapeEndstop => {
// Move out until endstop is not triggered anymore
if lift_end_stop.get_value().unwrap_or(false) == false {
self.state = State::Homing(HomingState::FindEnstopFineDistancing);
}
}
HomingState::FindEnstopFineDistancing => {
// Move out until endstop is not triggered anymore
if lift_end_stop.get_value().unwrap_or(false) == false {
// Find endstop fine
self.state = State::Homing(HomingState::FindEndStopFine);
}
}
HomingState::FindEndStopFine => {
// If endstop is reached change to idle
if lift_end_stop.get_value().unwrap_or(false) == true {
// Set poition of lift to 0
stepper_driver.set_position(0);
// Put Into Idle
self.state = State::Homing(HomingState::Validate(Instant::now()));
}
}
HomingState::FindEndstopCoarse => {
// Move to endstop
if lift_end_stop.get_value().unwrap_or(false) == true {
// Move awaiy from endstop
self.state = State::Homing(HomingState::FindEnstopFineDistancing);
}
}
HomingState::Validate(instant) => {
// If 100ms have passed check if position is actually 0.0
if instant.elapsed().as_millis() > 100 {
if self.is_at_position(Length::ZERO, Length::new::<millimeter>(0.01)) {
// If position is 0.0, put into idle
self.state = State::Idle;
} else {
// If position is not 0.0, redo homing
self.state = State::Homing(HomingState::Initialize);
}
}
}
},
// If state changed we
State::Buffering(buffering_state) => match buffering_state {
BufferingState::GoingUp => {}
BufferingState::Emptying => {}
BufferingState::Filling => {}
},
}
// Set the [`did_change_state`] flag
if self.did_change_state == false {
self.did_change_state = self.update_did_change_state(&old_state);
info!("{:?}", self.state);
}
// Speed
let speed = match &self.state {
State::NotHomed => Velocity::ZERO, // Not homed, no movement
State::Idle => Velocity::ZERO, // No movement in idle state
State::GoingDown => {
// Move down at a speed of 10-100 mm/s
self.speed_to_position(
self.limit_bottom,
match self.distance_to_position(self.limit_bottom).abs()
> Length::new::<millimeter>(1.0)
{
true => Velocity::new::<millimeter_per_second>(100.0),
false => Velocity::new::<millimeter_per_second>(10.0),
},
)
}
State::GoingUp => {
// Move up at a speed of 10-100 mm/s
self.speed_to_position(
self.limit_top,
match self.distance_to_position(self.limit_top).abs()
> Length::new::<millimeter>(1.0)
{
true => Velocity::new::<millimeter_per_second>(100.0),
false => Velocity::new::<millimeter_per_second>(10.0),
},
)
}
State::Homing(homing_state) => match homing_state {
HomingState::Initialize => Velocity::ZERO,
HomingState::EscapeEndstop => {
// Move down at a speed of 10 mm/s
Velocity::new::<millimeter_per_second>(10.0)
}
HomingState::FindEnstopFineDistancing => {
// Move up at a speed of 2 mm/s
Velocity::new::<millimeter_per_second>(2.0)
}
HomingState::FindEndstopCoarse => {
// Move in at a speed of -100 mm/s
Velocity::new::<millimeter_per_second>(-100.0)
}
HomingState::FindEndStopFine => {
// move into the endstop at 2 mm/s
Velocity::new::<millimeter_per_second>(-2.0)
}
HomingState::Validate(_) => {
// We stand still until the validation cooldown has passed
Velocity::ZERO
}
}, // Homing speed
State::Buffering(buffering_state) => match buffering_state {
BufferingState::GoingUp => {
// Move top at a speed of 100 mm/s
self.speed_to_position(
self.limit_top - self.padding + Length::new::<millimeter>(0.01),
Velocity::new::<millimeter_per_second>(100.0),
)
}
BufferingState::Filling => self.speed_to_position(
self.limit_top + self.padding - Length::new::<millimeter>(0.01),
Self::calculate_buffer_lift_speed(self),
),
BufferingState::Emptying => self.speed_to_position(
self.limit_bottom - self.padding + Length::new::<millimeter>(0.01),
Self::calculate_buffer_lift_speed(self),
),
},
};
info!("{}", speed.get::<millimeter_per_second>());
speed
}
}
@@ -1,27 +0,0 @@
use ethercat_hal::io::stepper_velocity_el70x1::StepperVelocityEL70x1;
#[derive(Debug)]
pub struct BufferTowerController {
enabled: bool,
/// Stepper driver. Controls buffer stepper motor
pub stepper_driver: StepperVelocityEL70x1,
}
impl BufferTowerController {
pub const fn new(driver: StepperVelocityEL70x1) -> Self {
Self {
enabled: false,
stepper_driver: driver,
}
}
pub fn set_enabled(&mut self, enabled: bool) {
self.enabled = enabled;
if enabled {
self.stepper_driver.set_enabled(true);
let _ = self.stepper_driver.set_speed(10.0);
} else {
self.stepper_driver.set_enabled(false);
let _ = self.stepper_driver.set_speed(0.0);
}
}
}
+327 -24
View File
@@ -1,35 +1,62 @@
pub mod act;
pub mod api;
pub mod buffer_tower_controller;
pub mod buffer_lift_controller;
pub mod new;
pub mod puller_speed_controller;
use api::{Buffer1Namespace, BufferV1Events, LiveValuesEvent, ModeState, StateEvent};
use buffer_tower_controller::BufferTowerController;
use buffer_lift_controller::BufferLiftController;
use control_core::{
converters::linear_step_converter::LinearStepConverter,
machines::{
ConnectedMachine, ConnectedMachineData, downcast_machine,
identification::{MachineIdentification, MachineIdentificationUnique},
manager::MachineManager,
},
socketio::namespace::NamespaceCacheingLogic,
uom_extensions::velocity::meter_per_minute,
};
use control_core_derive::Machine;
use ethercat_hal::io::{
digital_input::DigitalInput, stepper_velocity_el70x1::StepperVelocityEL70x1,
};
use futures::executor::block_on;
use puller_speed_controller::PullerRegulationMode;
use serde::{Deserialize, Serialize};
use smol::lock::{Mutex, RwLock};
use std::{
sync::{Arc, Weak},
time::Instant,
};
use uom::{
ConstZero,
si::{
f64::{Length, Velocity},
length::millimeter,
},
};
use crate::machines::{
MACHINE_BUFFER_V1, VENDOR_QITECH, buffer1::api::ConnectedMachineState, winder2::Winder2,
MACHINE_BUFFER_V1, VENDOR_QITECH,
buffer1::{
api::{ConnectedMachineState, LiftState, PullerState},
puller_speed_controller::PullerSpeedController,
},
winder2::{BufferState, Winder2, Winder2Mode},
};
#[derive(Debug, Machine)]
pub struct BufferV1 {
// drivers
pub lift: StepperVelocityEL70x1,
pub lift_end_stop: DigitalInput,
pub puller: StepperVelocityEL70x1,
// controllers
pub buffer_tower_controller: BufferTowerController,
pub buffer_lift_controller: BufferLiftController,
pub puller_speed_controller: PullerSpeedController,
pub lift_step_converter: LinearStepConverter,
// socketio
namespace: Buffer1Namespace,
@@ -61,7 +88,24 @@ impl BufferV1 {
impl BufferV1 {
pub fn emit_live_values(&mut self) {
let live_values = LiveValuesEvent {};
// Calculate puller speed from current motor steps
let steps_per_second = self.puller.get_speed();
let angular_velocity = self
.puller_speed_controller
.converter
.steps_to_angular_velocity(steps_per_second as f64);
let puller_speed = self
.puller_speed_controller
.angular_velocity_to_speed(angular_velocity);
// live values to be emittet
let live_values = LiveValuesEvent {
lift_position: self
.buffer_lift_controller
.get_current_position()
.map(|x| x.get::<millimeter>()),
puller_speed: puller_speed.get::<meter_per_minute>(),
};
let event = live_values.build();
self.namespace.emit(BufferV1Events::LiveValues(event));
@@ -86,53 +130,199 @@ impl BufferV1 {
})
.unwrap_or(false),
},
lift_state: LiftState {
limit_top: self
.buffer_lift_controller
.get_limit_top()
.get::<millimeter>(),
limit_bottom: self
.buffer_lift_controller
.get_limit_bottom()
.get::<millimeter>(),
position_in: self
.buffer_lift_controller
.get_limit_top()
.get::<millimeter>(),
position_out: self
.buffer_lift_controller
.get_limit_bottom()
.get::<millimeter>(),
is_going_up: self.buffer_lift_controller.is_going_up(),
is_going_down: self.buffer_lift_controller.is_going_down(),
is_homed: self.buffer_lift_controller.is_homed(),
is_going_home: self.buffer_lift_controller.is_going_home(),
is_buffering: self.buffer_lift_controller.is_buffering(),
step_size: self
.buffer_lift_controller
.get_step_size()
.get::<millimeter>(),
padding: self
.buffer_lift_controller
.get_padding()
.get::<millimeter>(),
can_go_top: self.can_go_up(),
can_go_bottom: self.can_go_down(),
can_go_home: self.can_go_home(),
},
puller_state: PullerState {
regulation: self.puller_speed_controller.regulation_mode.clone(),
target_speed: self
.puller_speed_controller
.target_speed
.get::<meter_per_minute>(),
target_diameter: self
.puller_speed_controller
.target_diameter
.get::<millimeter>(),
forward: self.puller_speed_controller.forward,
},
};
let event = state.build();
self.namespace.emit(BufferV1Events::State(event));
}
/// Can buffer capability check
pub fn can_buffer(&self) -> bool {
// Check if tension arm is zeroed and traverse is homed
self.buffer_lift_controller.is_homed() && !self.buffer_lift_controller.is_going_home()
}
/// Can go to upper limit capability check
pub fn can_go_up(&self) -> bool {
self.buffer_lift_controller.is_homed()
&& self.mode != BufferV1Mode::Standby
&& !self.buffer_lift_controller.is_going_up()
&& !self.buffer_lift_controller.is_going_home()
&& !self.buffer_lift_controller.is_buffering()
&& self.mode != BufferV1Mode::Filling
&& self.mode != BufferV1Mode::Emptying
}
/// Can go to lower limit capability check
pub fn can_go_down(&self) -> bool {
self.buffer_lift_controller.is_homed()
&& self.mode != BufferV1Mode::Standby
&& !self.buffer_lift_controller.is_going_down()
&& !self.buffer_lift_controller.is_going_home()
&& !self.buffer_lift_controller.is_buffering()
&& self.mode != BufferV1Mode::Filling
&& self.mode != BufferV1Mode::Emptying
}
/// Can go home capability check
pub fn can_go_home(&self) -> bool {
// Check if not in standby, not traversing
// Allow going home even when going in or out
self.mode != BufferV1Mode::Standby
&& !self.buffer_lift_controller.is_going_home()
&& !self.buffer_lift_controller.is_buffering()
&& self.mode != BufferV1Mode::Filling
&& self.mode != BufferV1Mode::Emptying
}
}
impl BufferV1 {
// To be implemented
fn fill_buffer(&mut self) {
todo!();
// stop the winder until the buffer is full
self.update_winder2_mode(Winder2Mode::Hold);
self.update_winder2_buffer_state(BufferState::Buffering);
self.get_winder(|winder| {
winder
.puller_speed_controller
.set_buffer_speed(Velocity::ZERO);
});
}
fn empty_buffer(&mut self) {
todo!();
// Set the winder2 to a mode where its faster than before to empty the buffer slowly
self.update_winder2_mode(Winder2Mode::Wind);
self.update_winder2_buffer_state(BufferState::Emptying);
self.get_winder(|winder| {
winder
.puller_speed_controller
.set_buffer_speed(self.puller_speed_controller.last_speed);
});
}
fn hold_buffer(&mut self) {
// Hold buffer in current state input speed = output speed
self.update_winder2_mode(Winder2Mode::Wind);
self.update_winder2_buffer_state(BufferState::Hold);
self.get_winder(|winder| {
winder
.puller_speed_controller
.set_buffer_speed(self.puller_speed_controller.last_speed);
});
}
fn update_winder2_mode(&mut self, mode: Winder2Mode) {
self.get_winder(|winder2| {
if winder2.mode != mode {
winder2.mode = mode;
}
});
}
fn update_winder2_buffer_state(&mut self, state: BufferState) {
self.get_winder(|winder2| {
if winder2.buffer_state != state {
winder2.set_buffer_state(state);
}
});
}
// Turn off motor and do nothing
fn switch_to_standby(&mut self) {
match self.mode {
BufferV1Mode::Standby => (),
BufferV1Mode::FillingBuffer => {}
BufferV1Mode::EmptyingBuffer => {}
BufferV1Mode::Hold => {}
BufferV1Mode::Filling => {}
BufferV1Mode::Emptying => {}
};
self.mode = BufferV1Mode::Standby;
self.buffer_tower_controller.set_enabled(false);
self.buffer_lift_controller.set_enabled(false);
let _ = self.buffer_lift_controller.stepper_driver.set_speed(0.0);
self.update_winder2_buffer_state(BufferState::Hold);
}
// hold motor
fn switch_to_hold(&mut self) {
match self.mode {
BufferV1Mode::Standby => self.hold_buffer(),
BufferV1Mode::Hold => (),
BufferV1Mode::Filling => {}
BufferV1Mode::Emptying => {}
};
self.mode = BufferV1Mode::Hold;
self.buffer_lift_controller.set_enabled(true);
let _ = self.buffer_lift_controller.stepper_driver.set_speed(0.0);
}
// Turn on motor and fill buffer
fn switch_to_filling(&mut self) {
match self.mode {
BufferV1Mode::Standby => self.fill_buffer(),
BufferV1Mode::FillingBuffer => (),
BufferV1Mode::EmptyingBuffer => {}
BufferV1Mode::Hold => {}
BufferV1Mode::Filling => (),
BufferV1Mode::Emptying => {}
};
self.mode = BufferV1Mode::FillingBuffer;
self.buffer_tower_controller.set_enabled(true);
self.mode = BufferV1Mode::Filling;
self.buffer_lift_controller.set_forward(true);
self.buffer_lift_controller.set_enabled(true);
}
// Turn on motor reverse and empty buffer
fn switch_to_emptying(&mut self) {
match self.mode {
BufferV1Mode::Standby => self.empty_buffer(),
BufferV1Mode::FillingBuffer => {}
BufferV1Mode::EmptyingBuffer => (),
BufferV1Mode::Hold => {}
BufferV1Mode::Filling => {}
BufferV1Mode::Emptying => (),
};
self.mode = BufferV1Mode::EmptyingBuffer;
self.mode = BufferV1Mode::Emptying;
self.buffer_lift_controller.set_forward(false);
self.buffer_lift_controller.set_enabled(true);
}
fn switch_mode(&mut self, mode: BufferV1Mode) {
@@ -142,20 +332,110 @@ impl BufferV1 {
match mode {
BufferV1Mode::Standby => self.switch_to_standby(),
BufferV1Mode::FillingBuffer => self.switch_to_filling(),
BufferV1Mode::EmptyingBuffer => self.switch_to_emptying(),
BufferV1Mode::Hold => self.switch_to_hold(),
BufferV1Mode::Filling => self.switch_to_filling(),
BufferV1Mode::Emptying => self.switch_to_emptying(),
}
}
}
impl BufferV1 {
fn set_mode_state(&mut self, mode: BufferV1Mode) {
pub fn set_mode_state(&mut self, mode: BufferV1Mode) {
self.switch_mode(mode);
self.emit_state();
}
}
/// Connecting/Disconnecting machine
// impl Winder
impl BufferV1 {
pub fn sync_winder_puller(&mut self) {
self.get_winder(|winder| {
winder
.puller_speed_controller
.set_buffer_speed(self.buffer_lift_controller.get_target_output_speed());
});
}
}
// Implement Puller
impl BufferV1 {
/// called by `act`
pub fn sync_puller_speed(&mut self, t: Instant) {
let angular_velocity = self.puller_speed_controller.calc_angular_velocity(t);
let steps_per_second = self
.puller_speed_controller
.converter
.angular_velocity_to_steps(angular_velocity);
let _ = self.puller.set_speed(steps_per_second);
// sync puller speed to lift input speed
let linear_velocity = self
.puller_speed_controller
.converter
.angular_velocity_to_velocity(angular_velocity);
self.buffer_lift_controller
.set_current_input_speed(linear_velocity);
}
pub fn puller_set_regulation(&mut self, puller_regulation_mode: PullerRegulationMode) {
self.puller_speed_controller
.set_regulation_mode(puller_regulation_mode);
self.emit_state();
}
/// Set target speed in m/min
pub fn puller_set_target_speed(&mut self, target_speed: f64) {
// Convert m/min to velocity
let target_speed = Velocity::new::<meter_per_minute>(target_speed);
self.puller_speed_controller.set_target_speed(target_speed);
self.emit_state();
}
/// Set target diameter in mm
pub fn puller_set_target_diameter(&mut self, target_diameter: f64) {
// Convert mm to length
let target_diameter = Length::new::<millimeter>(target_diameter);
self.puller_speed_controller
.set_target_diameter(target_diameter);
self.emit_state();
}
/// Set forward direction
pub fn puller_set_forward(&mut self, forward: bool) {
self.puller_speed_controller.set_forward(forward);
self.emit_state();
}
}
// Implement Lift
impl BufferV1 {
pub fn sync_lift_speed(&mut self, t: Instant) {
let linear_velocity =
self.buffer_lift_controller
.update_speed(&mut self.lift, &self.lift_end_stop, t);
if self.can_move() {
let steps_per_second = self.lift_step_converter.velocity_to_steps(linear_velocity);
let _ = self.lift.set_speed(steps_per_second);
} else {
let _ = self.lift.set_speed(0.0);
}
}
fn can_move(&mut self) -> bool {
match self.lift_end_stop.get_value() {
Ok(reached) => {
if reached {
false
} else {
true
}
}
Err(_) => false,
}
}
}
// Connecting/Disconnecting machine
impl BufferV1 {
/// set connected winder
pub fn set_connected_winder(
@@ -240,11 +520,34 @@ impl BufferV1 {
}
}
}
/// This helper function provides an easy way
/// to get the machine out of the Weak Reference
///
/// Usage:
///
/// self.get_winder(|winder2| {
/// winder2.do_something // Use the Winder here as usual
/// });
fn get_winder<F, R>(&self, func: F) -> Option<R>
where
F: FnOnce(&mut Winder2) -> R,
{
self.connected_winder
.as_ref()?
.machine
.upgrade()
.map(|winder_arc| {
let mut winder = block_on(winder_arc.lock());
func(&mut winder)
})
}
}
#[derive(Deserialize, Serialize, Debug, Clone, PartialEq, Eq)]
pub enum BufferV1Mode {
Standby,
FillingBuffer,
EmptyingBuffer,
Hold,
Filling,
Emptying,
}
+44 -23
View File
@@ -1,28 +1,33 @@
use std::time::Instant;
use anyhow::Error;
use control_core::converters::linear_step_converter::LinearStepConverter;
use control_core::machines::new::{
MachineNewHardware, MachineNewParams, MachineNewTrait, validate_no_role_dublicates,
validate_same_machine_identification_unique,
};
use ethercat_hal::{
coe::ConfigurableDevice,
devices::el7031_0030::coe::EL7031_0030Configuration,
devices::el7031_0030::pdo::EL7031_0030PredefinedPdoAssignment,
devices::el7041_0052::coe::EL7041_0052Configuration,
devices::{
EthercatDeviceUsed,
ek1100::{EK1100, EK1100_IDENTITY_A},
el7031_0030::{EL7031_0030, EL7031_0030_IDENTITY_A},
el7041_0052::{EL7041_0052, EL7041_0052_IDENTITY_A, EL7041_0052Port},
},
io::stepper_velocity_el70x1::StepperVelocityEL70x1,
shared_config,
shared_config::el70x1::{EL70x1OperationMode, StmMotorConfiguration},
use control_core::uom_extensions::velocity::meter_per_minute;
use ethercat_hal::coe::ConfigurableDevice;
use ethercat_hal::devices::EthercatDeviceUsed;
use ethercat_hal::devices::ek1100::EK1100;
use ethercat_hal::devices::ek1100::EK1100_IDENTITY_A;
use ethercat_hal::devices::el7031_0030::coe::EL7031_0030Configuration;
use ethercat_hal::devices::el7031_0030::pdo::EL7031_0030PredefinedPdoAssignment;
use ethercat_hal::devices::el7031_0030::{
EL7031_0030, EL7031_0030_IDENTITY_A, EL7031_0030StepperPort,
};
use ethercat_hal::devices::el7041_0052::coe::EL7041_0052Configuration;
use ethercat_hal::devices::el7041_0052::{EL7041_0052, EL7041_0052_IDENTITY_A, EL7041_0052Port};
use ethercat_hal::io::digital_input::DigitalInput;
use ethercat_hal::io::stepper_velocity_el70x1::StepperVelocityEL70x1;
use ethercat_hal::shared_config;
use ethercat_hal::shared_config::el70x1::{EL70x1OperationMode, StmMotorConfiguration};
use uom::si::f64::{Length, Velocity};
use uom::si::length::{centimeter, millimeter};
use crate::machines::buffer1::BufferV1Mode;
use crate::machines::buffer1::buffer_tower_controller::BufferTowerController;
use crate::machines::buffer1::buffer_lift_controller::BufferLiftController;
use crate::machines::buffer1::puller_speed_controller::PullerSpeedController;
use crate::machines::get_ethercat_device;
use super::{BufferV1, api::Buffer1Namespace};
@@ -76,7 +81,7 @@ impl MachineNewTrait for BufferV1 {
el7041
.write()
.await
.write_config(&subdevice, &el7041_config)
.write_config(subdevice, &el7041_config)
.await?;
{
let mut device_guard = el7041.write().await;
@@ -111,7 +116,7 @@ impl MachineNewTrait for BufferV1 {
el7031
.write()
.await
.write_config(&subdevice, &el7031_config)
.write_config(subdevice, &el7031_config)
.await?;
{
let mut device_guard = el7031.write().await;
@@ -119,11 +124,19 @@ impl MachineNewTrait for BufferV1 {
}
// Controller
let buffer_tower_controller = BufferTowerController::new(StepperVelocityEL70x1::new(
el7041.clone(),
EL7041_0052Port::STM1,
));
let buffer_lift_controller = BufferLiftController::new(
StepperVelocityEL70x1::new(el7041.clone(), EL7041_0052Port::STM1),
Length::new::<centimeter>(135.0),
Length::new::<centimeter>(0.0),
64,
);
let puller_speed_controller = PullerSpeedController::new(
Velocity::new::<meter_per_minute>(1.0),
Length::new::<millimeter>(1.75),
LinearStepConverter::from_diameter(200, Length::new::<centimeter>(8.0)),
);
// Get machine identification unique
let machine_id = params
.device_group
.first()
@@ -133,11 +146,19 @@ impl MachineNewTrait for BufferV1 {
.clone();
// create buffer instance
let mut buffer: Self = Self {
let mut buffer = Self {
lift: StepperVelocityEL70x1::new(el7041.clone(), EL7041_0052Port::STM1),
lift_end_stop: DigitalInput::new(el7041, EL7041_0052Port::DI1),
puller: StepperVelocityEL70x1::new(el7031.clone(), EL7031_0030StepperPort::STM1),
lift_step_converter: LinearStepConverter::from_diameter(
200,
Length::new::<millimeter>(32.22),
),
namespace: Buffer1Namespace::new(params.socket_queue_tx.clone()),
last_measurement_emit: Instant::now(),
mode: BufferV1Mode::Standby,
buffer_tower_controller,
buffer_lift_controller,
puller_speed_controller,
machine_manager: params.machine_manager.clone(),
machine_identification_unique: machine_id,
connected_winder: None,
@@ -0,0 +1,119 @@
use std::time::Instant;
use control_core::{
controllers::second_degree_motion::linear_jerk_speed_controller::LinearJerkSpeedController,
converters::linear_step_converter::LinearStepConverter,
uom_extensions::{
acceleration::meter_per_minute_per_second, jerk::meter_per_minute_per_second_squared,
velocity::meter_per_minute,
},
};
use serde::{Deserialize, Serialize};
use uom::{
ConstZero,
si::f64::{Acceleration, AngularVelocity, Jerk, Length, Velocity},
};
#[derive(Debug)]
pub struct PullerSpeedController {
enabled: bool,
pub target_speed: Velocity,
pub target_diameter: Length,
pub regulation_mode: PullerRegulationMode,
/// Forward rotation direction. If false, applies negative sign to speed
pub forward: bool,
/// Linear acceleration controller to dampen speed change
acceleration_controller: LinearJerkSpeedController,
/// Converter for linear to angular transformations
pub converter: LinearStepConverter,
pub last_speed: Velocity,
}
impl PullerSpeedController {
pub fn new(
target_speed: Velocity,
target_diameter: Length,
converter: LinearStepConverter,
) -> Self {
let acceleration = Acceleration::new::<meter_per_minute_per_second>(5.0);
let jerk = Jerk::new::<meter_per_minute_per_second_squared>(10.0);
let speed = Velocity::new::<meter_per_minute>(50.0);
Self {
enabled: false,
target_speed,
target_diameter,
regulation_mode: PullerRegulationMode::Speed,
forward: true,
acceleration_controller: LinearJerkSpeedController::new_simple(
Some(speed),
acceleration,
jerk,
),
converter,
last_speed: Velocity::ZERO,
}
}
pub fn set_enabled(&mut self, enabled: bool) {
self.enabled = enabled;
}
pub fn set_target_speed(&mut self, target: Velocity) {
self.target_speed = target;
}
pub fn set_target_diameter(&mut self, target: Length) {
self.target_diameter = target;
}
pub fn set_regulation_mode(&mut self, regulation: PullerRegulationMode) {
self.regulation_mode = regulation;
}
pub fn set_forward(&mut self, forward: bool) {
self.forward = forward;
}
fn update_speed(&mut self, t: Instant) -> Velocity {
let speed = match self.enabled {
true => match self.regulation_mode {
PullerRegulationMode::Speed => self.target_speed,
PullerRegulationMode::Diameter => unimplemented!(),
},
false => Velocity::ZERO,
};
let speed = if self.forward { speed } else { -speed };
let speed = self.acceleration_controller.update(speed, t);
self.last_speed = speed;
speed
}
pub fn speed_to_angular_velocity(&self, speed: Velocity) -> AngularVelocity {
// Use the converter to transform from linear velocity to angular velocity
self.converter.velocity_to_angular_velocity(speed)
}
pub fn angular_velocity_to_speed(&self, angular_speed: AngularVelocity) -> Velocity {
// Use the converter to transform from angular velocity to linear velocity
self.converter.angular_velocity_to_velocity(angular_speed)
}
pub fn calc_angular_velocity(&mut self, t: Instant) -> AngularVelocity {
let speed = self.update_speed(t);
self.speed_to_angular_velocity(speed)
}
pub fn get_target_speed(&self) -> Velocity {
self.target_speed
}
}
#[derive(Serialize, Deserialize, Debug, Clone)]
pub enum PullerRegulationMode {
Speed,
Diameter,
}
+9 -1
View File
@@ -1,6 +1,7 @@
use std::time::{Duration, Instant};
use super::Winder2;
use control_core::machines::new::MachineAct;
use std::time::{Duration, Instant};
impl MachineAct for Winder2 {
fn act(&mut self, now: Instant) {
@@ -20,10 +21,17 @@ impl MachineAct for Winder2 {
self.emit_state();
}
// sync the puller speed to the buffer output speed
self.sync_buffer_speed();
// more than 33ms have passed since last emit (30 "fps" target)
if now.duration_since(self.last_measurement_emit) > Duration::from_secs_f64(1.0 / 30.0) {
self.emit_live_values();
self.last_measurement_emit = now;
// check if traverse state changed
if self.traverse_controller.did_change_state() {
self.emit_state();
}
}
}
}
+7 -1
View File
@@ -103,6 +103,9 @@ enum Mutation {
// Disconnect Machine
DisconnectMachine(MachineIdentificationUnique),
// Connected Buffer
StartBuffering(),
}
#[derive(Serialize, Debug, Clone, Default)]
@@ -304,7 +307,9 @@ impl MachineApi for Winder2 {
Mutation::GotoTraverseLimitInner => self.traverse_goto_limit_inner(),
Mutation::GotoTraverseHome => self.traverse_goto_home(),
Mutation::SetPullerRegulationMode(regulation) => self.puller_set_regulation(regulation),
Mutation::SetPullerTargetSpeed(value) => self.puller_set_target_speed(value),
Mutation::SetPullerTargetSpeed(value) => {
self.puller_set_target_speed(value);
}
Mutation::SetPullerTargetDiameter(_) => todo!(),
Mutation::SetPullerForward(value) => self.puller_set_forward(value),
Mutation::SetSpoolRegulationMode(mode) => self.spool_set_regulation_mode(mode),
@@ -337,6 +342,7 @@ impl MachineApi for Winder2 {
Mutation::DisconnectMachine(machine_identification_unique) => {
self.disconnect_buffer(machine_identification_unique)
}
Mutation::StartBuffering() => self.start_buffering(),
}
Ok(())
}
+63 -2
View File
@@ -84,6 +84,8 @@ pub struct Winder2 {
// machine connection
pub machine_manager: Weak<RwLock<MachineManager>>,
pub machine_identification_unique: MachineIdentificationUnique,
// Buffer State
pub buffer_state: BufferState,
// connected machines
pub connected_buffer: Option<ConnectedMachine<Weak<Mutex<BufferV1>>>>,
@@ -391,7 +393,7 @@ impl Winder2 {}
/// Implement Mode
impl Winder2 {
fn set_mode(&mut self, mode: &Winder2Mode) {
pub fn set_mode(&mut self, mode: &Winder2Mode) {
let should_update = *mode != Winder2Mode::Wind || self.can_wind();
if should_update {
@@ -779,8 +781,37 @@ impl Winder2 {
}
}
/// implement machine connection
/// Buffer
impl Winder2 {
/// Sync Puller to Buffer
pub fn sync_buffer_speed(&mut self) {
// send puller speed to buffer, if connected
self.get_buffer(|buffer| {
buffer
.buffer_lift_controller
.set_target_output_speed(self.puller_speed_controller.get_target_speed());
});
}
/// Start Buffering Process
pub fn start_buffering(&mut self) {
// change winder's Buffer state to Buffering
self.set_buffer_state(BufferState::Buffering);
self.puller_speed_controller
.set_regulation_mode(PullerRegulationMode::Buffer);
// change Buffer state to Filling
self.get_buffer(|buffer| {
buffer.set_mode_state(super::buffer1::BufferV1Mode::Filling);
});
}
/// Set Buffer State
pub fn set_buffer_state(&mut self, state: BufferState) {
self.buffer_state = state;
}
/// Buffer Connection Logic
/// set connected buffer
pub fn set_connected_buffer(
&mut self,
@@ -863,6 +894,28 @@ impl Winder2 {
}
}
}
/// This helper function provides an easy way
/// to get the machine out of the Weak Reference
///
/// Usage:
///
/// self.get_buffer(|buffer| {
/// buffer.do_something // Use the Buffer here as usual
/// });
fn get_buffer<F, R>(&self, func: F) -> Option<R>
where
F: FnOnce(&mut BufferV1) -> R,
{
self.connected_buffer
.as_ref()?
.machine
.upgrade()
.map(|buffer_arc| {
let mut buffer = block_on(buffer_arc.lock());
func(&mut buffer)
})
}
}
#[derive(Debug, Clone, PartialEq, Eq)]
@@ -927,6 +980,14 @@ impl From<Winder2Mode> for PullerMode {
}
}
#[derive(Debug, Clone, PartialEq)]
pub enum BufferState {
Disconnected,
Hold,
Buffering,
Emptying,
}
impl std::fmt::Display for Winder2 {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
write!(f, "Winder2")
+2
View File
@@ -4,6 +4,7 @@ use super::api::Winder2Namespace;
use super::tension_arm::TensionArm;
use super::{Winder2, Winder2Mode};
use crate::machines::get_ethercat_device;
use crate::machines::winder2::BufferState;
use crate::machines::winder2::puller_speed_controller::PullerSpeedController;
use crate::machines::winder2::spool_speed_controller::SpoolSpeedController;
use crate::machines::winder2::traverse_controller::TraverseController;
@@ -228,6 +229,7 @@ impl MachineNewTrait for Winder2 {
machine_manager: params.machine_manager.clone(),
machine_identification_unique: machine_id,
connected_buffer: None,
buffer_state: BufferState::Disconnected,
};
// initalize events
@@ -19,11 +19,12 @@ pub struct PullerSpeedController {
enabled: bool,
pub target_speed: Velocity,
pub target_diameter: Length,
pub buffer_speed: Velocity,
pub regulation_mode: PullerRegulationMode,
/// Forward rotation direction. If false, applies negative sign to speed
pub forward: bool,
/// Linear acceleration controller to dampen speed change
acceleration_controller: LinearJerkSpeedController,
pub acceleration_controller: LinearJerkSpeedController,
/// Converter for linear to angular transformations
pub converter: LinearStepConverter,
pub last_speed: Velocity,
@@ -43,6 +44,7 @@ impl PullerSpeedController {
enabled: false,
target_speed,
target_diameter,
buffer_speed: Velocity::ZERO,
regulation_mode: PullerRegulationMode::Speed,
forward: true,
acceleration_controller: LinearJerkSpeedController::new_simple(
@@ -67,6 +69,10 @@ impl PullerSpeedController {
self.target_diameter = target;
}
pub fn set_buffer_speed(&mut self, speed: Velocity) {
self.buffer_speed = speed;
}
pub const fn set_regulation_mode(&mut self, regulation: PullerRegulationMode) {
self.regulation_mode = regulation;
}
@@ -80,6 +86,7 @@ impl PullerSpeedController {
true => match self.regulation_mode {
PullerRegulationMode::Speed => self.target_speed,
PullerRegulationMode::Diameter => unimplemented!(),
PullerRegulationMode::Buffer => self.buffer_speed,
},
false => Velocity::ZERO,
};
@@ -116,4 +123,5 @@ impl PullerSpeedController {
pub enum PullerRegulationMode {
Speed,
Diameter,
Buffer,
}