Add motor behavior for food production

the-orange-alliance · Sep 16, 2024 · 9102ef0 · 9102ef0
1 parent 3e1ade1
commit 9102ef0
Show file tree

Hide file tree

Showing 2 changed files with 37 additions and 3 deletions.
diff --git a/back-end/realtime/src/util/FeedingTheFutureCallbacks.ts b/back-end/realtime/src/util/FeedingTheFutureCallbacks.ts
@@ -1,11 +1,13 @@
 import {
   applyPatternToStrips,
+  applySetpointToMotors,
   applyStateToGoal,
   FeedingTheFuture,
   FieldControlUpdatePacket,
   FieldOptions,
   LedStrip,
-  Match
+  Match,
+  Motor
 } from '@toa-lib/models';
 import { NexusGoalState } from '@toa-lib/models/build/seasons/FeedingTheFuture.js';
 import { Socket } from 'socket.io';
@@ -21,83 +23,95 @@ export const matchUpdateCallback = (
     previousDetails.redNexusState.CW1,
     currentDetails.redNexusState.CW1,
     LedStrip.RED_SIDE_GOALS[0],
+    Motor.RED_SIDE_GOALS[0],
     'red.CW1',
     broadcast
   );
   handleGoalStateChange(
     previousDetails.redNexusState.CW2,
     currentDetails.redNexusState.CW2,
     LedStrip.RED_SIDE_GOALS[1],
+    Motor.RED_SIDE_GOALS[1],
     'red.CW2',
     broadcast
   );
   handleGoalStateChange(
     previousDetails.redNexusState.CW3,
     currentDetails.redNexusState.CW3,
     LedStrip.RED_SIDE_GOALS[2],
+    Motor.RED_SIDE_GOALS[2],
     'red.CW3',
     broadcast
   );
   handleGoalStateChange(
     previousDetails.redNexusState.CW4,
     currentDetails.redNexusState.CW4,
     LedStrip.RED_SIDE_GOALS[3],
+    Motor.RED_SIDE_GOALS[3],
     'red.CW4',
     broadcast
   );
   handleGoalStateChange(
     previousDetails.redNexusState.CW5,
     currentDetails.redNexusState.CW5,
     LedStrip.RED_SIDE_GOALS[4],
+    Motor.RED_SIDE_GOALS[4],
     'red.CW5',
     broadcast
   );
   handleGoalStateChange(
     previousDetails.redNexusState.CW6,
     currentDetails.redNexusState.CW6,
     LedStrip.RED_SIDE_GOALS[5],
+    Motor.RED_SIDE_GOALS[5],
     'red.CW6',
     broadcast
   );
   handleGoalStateChange(
     previousDetails.redNexusState.EC1,
     currentDetails.redNexusState.EC1,
     LedStrip.RED_CENTER_GOALS[0],
+    Motor.RED_CENTER_GOALS[0],
     'red.EC1',
     broadcast
   );
   handleGoalStateChange(
     previousDetails.redNexusState.EC2,
     currentDetails.redNexusState.EC2,
     LedStrip.RED_CENTER_GOALS[1],
+    Motor.RED_CENTER_GOALS[1],
     'red.EC2',
     broadcast
   );
   handleGoalStateChange(
     previousDetails.redNexusState.EC3,
     currentDetails.redNexusState.EC3,
     LedStrip.RED_CENTER_GOALS[2],
+    Motor.RED_CENTER_GOALS[2],
     'red.EC3',
     broadcast
   );
   handleGoalStateChange(
     previousDetails.redNexusState.EC4,
     currentDetails.redNexusState.EC4,
     LedStrip.RED_CENTER_GOALS[3],
+    Motor.RED_CENTER_GOALS[3],
     'red.EC4',
     broadcast
   );
   handleGoalStateChange(
     previousDetails.redNexusState.EC5,
     currentDetails.redNexusState.EC5,
     LedStrip.RED_CENTER_GOALS[4],
+    Motor.RED_CENTER_GOALS[4],
     'red.EC5',
     broadcast
   );
   handleGoalStateChange(
     previousDetails.redNexusState.EC6,
     currentDetails.redNexusState.EC6,
     LedStrip.RED_CENTER_GOALS[5],
+    Motor.RED_CENTER_GOALS[5],
     'red.EC6',
     broadcast
   );
@@ -106,83 +120,95 @@ export const matchUpdateCallback = (
     previousDetails.blueNexusState.CW1,
     currentDetails.blueNexusState.CW1,
     LedStrip.BLUE_SIDE_GOALS[0],
+    Motor.BLUE_SIDE_GOALS[0],
     'blue.CW1',
     broadcast
   );
   handleGoalStateChange(
     previousDetails.blueNexusState.CW2,
     currentDetails.blueNexusState.CW2,
     LedStrip.BLUE_SIDE_GOALS[1],
+    Motor.BLUE_SIDE_GOALS[1],
     'blue.CW2',
     broadcast
   );
   handleGoalStateChange(
     previousDetails.blueNexusState.CW3,
     currentDetails.blueNexusState.CW3,
     LedStrip.BLUE_SIDE_GOALS[2],
+    Motor.BLUE_SIDE_GOALS[2],
     'blue.CW3',
     broadcast
   );
   handleGoalStateChange(
     previousDetails.blueNexusState.CW4,
     currentDetails.blueNexusState.CW4,
     LedStrip.BLUE_SIDE_GOALS[3],
+    Motor.BLUE_SIDE_GOALS[3],
     'blue.CW4',
     broadcast
   );
   handleGoalStateChange(
     previousDetails.blueNexusState.CW5,
     currentDetails.blueNexusState.CW5,
     LedStrip.BLUE_SIDE_GOALS[4],
+    Motor.BLUE_SIDE_GOALS[4],
     'blue.CW5',
     broadcast
   );
   handleGoalStateChange(
     previousDetails.blueNexusState.CW6,
     currentDetails.blueNexusState.CW6,
     LedStrip.BLUE_SIDE_GOALS[5],
+    Motor.BLUE_SIDE_GOALS[5],
     'blue.CW6',
     broadcast
   );
   handleGoalStateChange(
     previousDetails.blueNexusState.EC1,
     currentDetails.blueNexusState.EC1,
     LedStrip.BLUE_CENTER_GOALS[0],
+    Motor.BLUE_CENTER_GOALS[0],
     'blue.EC1',
     broadcast
   );
   handleGoalStateChange(
     previousDetails.blueNexusState.EC2,
     currentDetails.blueNexusState.EC2,
     LedStrip.BLUE_CENTER_GOALS[1],
+    Motor.BLUE_CENTER_GOALS[1],
     'blue.EC2',
     broadcast
   );
   handleGoalStateChange(
     previousDetails.blueNexusState.EC3,
     currentDetails.blueNexusState.EC3,
     LedStrip.BLUE_CENTER_GOALS[2],
+    Motor.BLUE_CENTER_GOALS[2],
     'blue.EC3',
     broadcast
   );
   handleGoalStateChange(
     previousDetails.blueNexusState.EC4,
     currentDetails.blueNexusState.EC4,
     LedStrip.BLUE_CENTER_GOALS[3],
+    Motor.BLUE_CENTER_GOALS[3],
     'blue.EC4',
     broadcast
   );
   handleGoalStateChange(
     previousDetails.blueNexusState.EC5,
     currentDetails.blueNexusState.EC5,
     LedStrip.BLUE_CENTER_GOALS[4],
+    Motor.BLUE_CENTER_GOALS[4],
     'blue.EC5',
     broadcast
   );
   handleGoalStateChange(
     previousDetails.blueNexusState.EC6,
     currentDetails.blueNexusState.EC6,
     LedStrip.BLUE_CENTER_GOALS[5],
+    Motor.BLUE_CENTER_GOALS[5],
     'blue.EC6',
     broadcast
   );
@@ -198,6 +224,7 @@ const handleGoalStateChange = (
   previousState: NexusGoalState,
   currentState: NexusGoalState,
   strip: LedStrip,
+  motor: Motor,
   goal: string,
   broadcast: (update: FieldControlUpdatePacket) => void
 ) => {
@@ -230,8 +257,15 @@ const handleGoalStateChange = (
         // Set pattern
         const result: FieldControlUpdatePacket = { hubs: {}, wleds: {} };
         applyPatternToStrips('ffffff', [strip], result);
+        applySetpointToMotors(1.0, [motor], result);
         broadcast(result);
 
+        setTimeout(() => {
+          const result: FieldControlUpdatePacket = { hubs: {}, wleds: {} };
+          applySetpointToMotors(0, [motor], result);
+          broadcast(result);
+        }, 5000); // TODO(jan): Use field options
+
         // TODO(jan): Update match to mark food as dispensed
       }, 5000) // TODO(jan): Make this time configurable
     );

diff --git a/lib/models/src/fcs/Packets.ts b/lib/models/src/fcs/Packets.ts
@@ -57,7 +57,7 @@ export function applyPatternToStrips(
   });
 }
 
-function applySetpointToMotors(
+export function applySetpointToMotors(
   setpoint: number,
   motors: Motor[],
   packet: FieldControlUpdatePacket
@@ -153,7 +153,7 @@ export class LedStrip {
 
 type MotorPortType = 'on board' | 'spark mini';
 
-class Motor {
+export class Motor {
   public static readonly RED_SIDE_GOALS: Motor[] = [
     new Motor(RevHub.RED_CONTROL_HUB, 'on board', 0),
     new Motor(RevHub.RED_CONTROL_HUB, 'on board', 1),