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 @@
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':
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();
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment