Commit 6d64383d authored by Xoaquin Castrelo's avatar Xoaquin Castrelo
Browse files

Added support for velocity control.

parent 814c10e4
......@@ -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();
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment