From 6d64383df17217f0c34c50111e284ed36d9a0ea6 Mon Sep 17 00:00:00 2001
From: Xoaquin Castrelo <xoaquin.cb@gmail.com>
Date: Sat, 13 Nov 2021 23:51:59 +0000
Subject: [PATCH] Added support for velocity control.
---
emb/state-controller.cpp | 48 ++++++++++++++++++++++++++++++++++++----
1 file changed, 44 insertions(+), 4 deletions(-)
diff --git a/emb/state-controller.cpp b/emb/state-controller.cpp
index 08c3d7d..428094b 100644
--- a/emb/state-controller.cpp
+++ b/emb/state-controller.cpp
@@ -6,19 +6,39 @@
enum State
{
- STATE_DELAY, STATE_READY
+ STATE_READY = 0, STATE_DELAY
};
static State state = STATE_READY;
static uint32_t delayEnd = 0;
+static int16_t servoVelocities[NUM_SERVOS];
+static uint32_t velocitiesEndMillis[NUM_SERVOS];
+static int16_t startAngles[NUM_SERVOS];
+static uint32_t startMillis[NUM_SERVOS];
+
void parseCommand(command *comm)
{
+ uint32_t currentMillis = getCurrentMillis();
+
switch (comm->comm_ch)
{
case 's':
- setServoAngle(comm->arg[0] - 1, comm->arg[1] * 60);
- updateServos();
+ if (0 <= comm->arg[0] - 1 && comm->arg[0] - 1 < NUM_SERVOS)
+ {
+ setServoAngle(comm->arg[0] - 1, comm->arg[1] * 60);
+ updateServos();
+ servoVelocities[comm->arg[0] - 1] = 0;
+ }
+ break;
+ case 'v':
+ if (0 <= comm->arg[0] - 1 && comm->arg[0] - 1 < NUM_SERVOS)
+ {
+ servoVelocities[comm->arg[0] - 1] = comm->arg[1];
+ velocitiesEndMillis[comm->arg[0] - 1] = currentMillis + comm->arg[2];
+ startAngles[comm->arg[0] - 1] = getServoAngle(comm->arg[0] - 1);
+ startMillis[comm->arg[0] - 1] = currentMillis;
+ }
break;
case 'd':
delayEnd = getCurrentMillis() + comm->arg[0];
@@ -29,6 +49,8 @@ void parseCommand(command *comm)
void updateState()
{
+ uint32_t currentMillis = getCurrentMillis();
+
switch (state)
{
case STATE_READY:
@@ -44,11 +66,29 @@ void updateState()
break;
}
case STATE_DELAY:
- if (getCurrentMillis() > delayEnd)
+ if (currentMillis > delayEnd)
state = STATE_READY;
break;
}
+
+ for (uint8_t i = 0; i < NUM_SERVOS; i++)
+ {
+ if (servoVelocities[i] != 0)
+ {
+ if (currentMillis < velocitiesEndMillis[i])
+ {
+ int16_t newAngle = startAngles[i] + (int32_t) servoVelocities[i] * 60 * (int32_t) (currentMillis - startMillis[i]) / 1000;
+ setServoAngle(i, newAngle);
+ }
+ else
+ {
+ setServoAngle(i, startAngles[i] + (int32_t) servoVelocities[i] * 60 * (int32_t) (velocitiesEndMillis[i] - startMillis[i]) / 1000);
+ servoVelocities[i] = 0;
+ }
+ }
+ }
+ updateServos();
}
--
GitLab