实现伺服电机的状态机

Question

我为我的伺服创建了一个FSM。它有两个州。我正在使用开关/外壳结构，但电机正在“卡住＆＃39;在第一种情况下，我不知道为什么。

这是我的代码：

#include <Servo.h>

#define one 1
#define two 2


Servo myservo1; //projector platform servo

unsigned long Timer1; //define timer variable for state 1 if statement

void setup()
{
    myservo1.attach(9);
}

void loop(){

    static int state = one; // initial state is one.

    switch(state)
    {
    case one:
        myservo1.writeMicroseconds(1374); // servo is moving cw
        delay(5000);
        myservo1.writeMicroseconds(1474); // servo is stationary
        Timer1 = millis();
        if (millis() - Timer1 > 5000)
        {
            state = two;
        }
        break;

    case two:
        for(int speedv1 = 0; speedv1 <= 100; speedv1 += 2) // loop to ramp up speed of servos
        {
            myservo1.writeMicroseconds(1474 + speedv1); // speed increase by 2 each iteration (servo 1) until servo reaches fullspeed (ACW)
            delay(40); // delay between loop iterations
        }
        delay(5000);
        for(int speedv2 = 0; speedv2 <= 100; speedv2 += 2) // loop to ramp down servo speed
        {
            myservo1.writeMicroseconds(1574 - speedv2); // speed decrease by 2 each iteration (servo 1) until servo stops
            delay(40); //delay between loop iterations 
        }
        delay(2000);
        state = one;
        break;
    }
} 

电机似乎卡在了

myservo1.writeMicroseconds(1374);

在case 1的第一行。

卡住我的意思是电机只是以顺时针方式继续旋转，并且在延迟后不会进入下一个writeMicroseconds()语句。感谢帮助。

Answer 1

将这段代码放在第一个状态：

Timer1 = millis();
if (millis() - Timer1 > 5000)
{
    state = two;
}

令人难以置信定时器不太可能在第一行和第二行之间提前五秒。因此，因为你改变了Timer1每个人运行那段代码的时间，它总是会保持在第一状态。

虽然目前尚不清楚你究竟想要实现什么（我不确定writeMicroSeconds()调用的内容，也不确定你需要在状态中明确延迟），让我们现在假设你我想留在状态一秒五秒，但在那段时间做其他事情。这意味着您需要将开始时间初始化为移动到状态1的一部分，而不是在一个代码本身的状态内。

例如，下面的代码显示了一种方法，顺时针连续运行伺服五秒钟，然后逆时针运行四天（带一些神秘的调用来启动伺服运行）：

void loop() {
    static int state = zero;
    static unsigned long timer1;

    switch (state) {
        case zero: {
            timer1 = millis();
            state = one;
            servo.startClockwise();
            break;
        }
        case one: {
            if (millis() - timer1 >= 5000) {
                servo.stop();
                servo.startAntiClockwise();
                timer1 = millis();
                state = two;
            } else {
                doSomethingElse();
            }
            break;
        }
        case two: {
            if (millis() - timer1 >= 4000) {
                servo.stop();
                servo.startClockwise();
                timer1 = millis();
                state = one;
            } else {
                doSomethingElse();
            }
            break;
        }
    }
}