state-controller.cpp 2.47 KB
Newer Older
Xoaquin Castrelo's avatar
Xoaquin Castrelo committed
1
2
3
4
5
6
7
8
#include "servos.hpp"
#include "command.h"
#include "time.hpp"

#include <avr/interrupt.h>

enum State
{
9
     STATE_READY = 0, STATE_DELAY
Xoaquin Castrelo's avatar
Xoaquin Castrelo committed
10
11
12
13
14
};

static State state = STATE_READY;
static uint32_t delayEnd = 0;

15
16
17
18
19
static int16_t servoVelocities[NUM_SERVOS];
static uint32_t velocitiesEndMillis[NUM_SERVOS];
static int16_t startAngles[NUM_SERVOS];
static uint32_t startMillis[NUM_SERVOS];

Xoaquin Castrelo's avatar
Xoaquin Castrelo committed
20
21
void parseCommand(command *comm)
{
22
23
    uint32_t currentMillis = getCurrentMillis();
    
Xoaquin Castrelo's avatar
Xoaquin Castrelo committed
24
25
26
    switch (comm->comm_ch)
    {
        case 's':
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
            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;
            }
Xoaquin Castrelo's avatar
Xoaquin Castrelo committed
42
43
44
45
46
47
48
49
50
51
            break;
        case 'd':
            delayEnd = getCurrentMillis() + comm->arg[0];
            state = STATE_DELAY;
            break;
    }
}

void updateState()
{
52
53
    uint32_t currentMillis = getCurrentMillis();
    
Xoaquin Castrelo's avatar
Xoaquin Castrelo committed
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
    switch (state)
    {
        case STATE_READY:
        {
            cli();
            command *tmp = get_command();
            command comm = *tmp;
            sei();
            
            if (tmp != nullptr)
                parseCommand(&comm);
            
            break;
        }
        case STATE_DELAY:
69
            if (currentMillis > delayEnd)
Xoaquin Castrelo's avatar
Xoaquin Castrelo committed
70
71
72
73
                state = STATE_READY;
            
            break;
    }
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
    
    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();
Xoaquin Castrelo's avatar
Xoaquin Castrelo committed
92
93
94
}