Skip to content
Snippets Groups Projects
Commit 6d64383d authored by Xoaquin Castrelo's avatar Xoaquin Castrelo
Browse files

Added support for velocity control.

parent 814c10e4
No related branches found
No related tags found
No related merge requests found
...@@ -6,19 +6,39 @@ ...@@ -6,19 +6,39 @@
enum State enum State
{ {
STATE_DELAY, STATE_READY STATE_READY = 0, STATE_DELAY
}; };
static State state = STATE_READY; static State state = STATE_READY;
static uint32_t delayEnd = 0; 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) void parseCommand(command *comm)
{ {
uint32_t currentMillis = getCurrentMillis();
switch (comm->comm_ch) switch (comm->comm_ch)
{ {
case 's': case 's':
if (0 <= comm->arg[0] - 1 && comm->arg[0] - 1 < NUM_SERVOS)
{
setServoAngle(comm->arg[0] - 1, comm->arg[1] * 60); setServoAngle(comm->arg[0] - 1, comm->arg[1] * 60);
updateServos(); 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; break;
case 'd': case 'd':
delayEnd = getCurrentMillis() + comm->arg[0]; delayEnd = getCurrentMillis() + comm->arg[0];
...@@ -29,6 +49,8 @@ void parseCommand(command *comm) ...@@ -29,6 +49,8 @@ void parseCommand(command *comm)
void updateState() void updateState()
{ {
uint32_t currentMillis = getCurrentMillis();
switch (state) switch (state)
{ {
case STATE_READY: case STATE_READY:
...@@ -44,11 +66,29 @@ void updateState() ...@@ -44,11 +66,29 @@ void updateState()
break; break;
} }
case STATE_DELAY: case STATE_DELAY:
if (getCurrentMillis() > delayEnd) if (currentMillis > delayEnd)
state = STATE_READY; state = STATE_READY;
break; 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();
} }
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment