Commit 5f0b6f53 authored by Xoaquin Castrelo's avatar Xoaquin Castrelo
Browse files

Added state controller.

parent 41d5eb4a
#include "servos.hpp"
#include "command.h"
#include "time.hpp"
#include <avr/interrupt.h>
enum State
{
STATE_DELAY, STATE_READY
};
static State state = STATE_READY;
static uint32_t delayEnd = 0;
void parseCommand(command *comm)
{
switch (comm->comm_ch)
{
case 's':
setServoAngle(comm->arg[0] - 1, comm->arg[1] * 60);
updateServos();
break;
case 'd':
delayEnd = getCurrentMillis() + comm->arg[0];
state = STATE_DELAY;
break;
}
}
void updateState()
{
switch (state)
{
case STATE_READY:
{
cli();
command *tmp = get_command();
command comm = *tmp;
sei();
if (tmp != nullptr)
parseCommand(&comm);
break;
}
case STATE_DELAY:
if (getCurrentMillis() > delayEnd)
state = STATE_READY;
break;
}
}
#ifndef STATE_CONTROLLER_H
#define STATE_CONTROLLER_H
#include "command.h"
/**
* Updates command the state.
* Should be called periodically.
*/
void updateState();
#endif /* STATE_CONTROLLER_H */
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