johannes.payr/pizzabox-teensy
0
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Interim commit (not working)

Browse source 
- Added state machine (work in progress)
- Added heartbeat blink (weird errors)
This commit is contained in:
jpunkt 2021-12-22 21:13:07 +01:00
parent 300050a80f
commit 3bfe65f1f7
13 changed files with 1030 additions and 77 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

126
lib/StateMachine-1.0.13b/README.md Normal file
View file

@ -0,0 +1,126 @@
# StateMachine
This library implements a basic State Machine. The state logic and its transition's conditions are implemented as functions in your sketch for flexibility. The machine is intended to be deterministic, meaning that you can only be in one state at the time and transitions should occur only when your transition condition functions return true. Please note that if multiple transitions are defined for a state the first one to evaluate to true is the one that modifies the current state.
This implementation relies on LinkedList (https://github.com/ivanseidel/LinkedList) to implement the collection of states and the collection of transitions for a state.
## STATES
There are two ways of declaring a state logic:
1. Through a lambda function (an anonymous function) declared in the addState() method.
2. Defining the function normally and passing the address to the addState() method.
States contain the machine logic of the program. The machine only evaluates the current state until a transition occurs that points to another state.
To evaluate a piece of code only once while the machine is in a particular state, you can use the machine.evaluateOnce attribute. It is true each time the machine enters a new state until the first transition is evaluated.
## TRANSITIONS
Each state has transitions defined in setup(). Transitions require two parameters,
1. The transition test function that returns a boolean value indicating whether or not the transition occured,
2. The number of the target state. The target state can also be specified passing the state pointer. This could point to the same state it is in, if you want to dynamically set the transition target. To do so, use state->setTransition(). You must pass the index of the transition you want to modify and the number of the target state.
Transitions are evaluated by the state machine after the state logic has executed. If none of the transitions evaluate to true, then the machine stays in the current state.
## EXAMPLE
![state diagram](STATEMACHINE.png)
```c++
#include <StateMachine.h>
const int STATE_DELAY = 1000;
int randomState = 0;
const int LED = 13;
StateMachine machine = StateMachine();
State* S0 = machine.addState(&state0);
State* S1 = machine.addState(&state1);
State* S2 = machine.addState(&state2);
State* S3 = machine.addState(&state3);
State* S4 = machine.addState(&state4);
State* S5 = machine.addState(&state5);
void setup() {
Serial.begin(115200);
pinMode(LED,OUTPUT);
randomSeed(A0);
S0->addTransition(&transitionS0,S0); // Transition to itself (see transition logic for details)
S1->addTransition(&transitionS1S2,S2); // S1 transition to S2
S2->addTransition(&transitionS2S3,S3); // S2 transition to S3
S3->addTransition(&transitionS3S4,S4); // S3 transition to S4
S4->addTransition(&transitionS4S5,S5); // S4 transition to S5
S5->addTransition(&transitionS5S0,S0); // S5 transition to S0
S5->addTransition(&transitionS5S2,S2); // S5 transition to S2
}
void loop() {
machine.run();
delay(STATE_DELAY);
}
```
### States
The state logic is defined in a function that is passed as a parameter to the ``` machine.addState() ``` method.
```c++
State* S0 = machine.addState(&state0);
```
Here, **state0** is the name of the function that defines S0 logic. The function must be defined in the sketch, for example:
```c++
void state0(){
Serial.println("State 0");
if(machine.executeOnce){
Serial.println("Execute Once");
digitalWrite(LED,!digitalRead(LED));
}
}
```
### State Execute Once
If you need to execute a portion of code within a state just once, you can use the machine attribute ```executeOnce```. This attribute is true the first time a state logic is evaluated and then becomes false until a transition to a new state occurs. In the example above, the LED is toggled once every time the machine enters state S0.
### Transitions
Transitions are added to the states in the setup() function. When specifying a transition of a state you pass the name of the function that evaluates the transition and the state object you want the machine to transition to when it evaluates to true.
```c++
S1->addTransition(&transitionS1S2,S2); // S1 transition to S2
```
The transitions are implemented by evaluating a function that returns a bool value of true or false. If the function returns **true** then the machine will transition from the current state to the state specified when the transition was added in setup().
```c++
bool transitionS1S2(){
if(digitalRead(5) == HIGH){
return true;
}
return false;
}
```
Each state can have multiple transitions, and when the state is active (current state of the machine) all of its transitions are evaluated to determine the next active state. When a state has multiple transitions, the transitions are evaluated in the order they were added to the state. The first transition to return true will determine the next active state.
### Transition to any state
You can also force a transition to any state by calling the method ```transitionTo(State* s)```. This method accepts either a pointer to a specific state or an int that represents the state index. The easiest way is to use the pointer. For example,
```c++
machine.transitionTo(S4);
```
When this line is evaluated the machine will unconditionally transition to S4.
### Modifying a transition target
You can also dynamically modify the transition target of a defined transition by using the ```setTransition(int index, int stateNo)``` method.
```c++
randomState = random(0,6);
Serial.print("Transitioning to random state ");
Serial.println(randomState);
S0->setTransition(0,randomState);
```
In this example, the first transition (0) added to S0 is assigned a random target from 0 to 5. Assuming the machine has 6 or more states, the first transition in S0 now points to a different state each time this code is executed.

BIN
lib/StateMachine-1.0.13b/STATEMACHINE.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 8.2 KiB

171
lib/StateMachine-1.0.13b/examples/arduino_state_machine/arduino_state_machine.ino Normal file
View file

@ -0,0 +1,171 @@
/****************************************************************
*
* STATE MACHINE EXAMPLE SKETCH
*
*
* This sketch is an example to learn how to use the state
* machine. In this example we define a state machine with
* 6 states (S0-S5).
*
* STATES
* There are two ways of declaring a state logic:
* 1. Through a lambda function (an anonymous function) declared
* in the addState() method.
* 2. Defining the function normally and passing the address
* to the addState() method.
*
* States contain the machine logic of the program. The machine
* only evaluates the current state until a transition occurs
* that points to another state.
*
* To evaluate a piece of code only once while the machine
* is in a particular state, you can use the machine.evaluateOnce
* attribute. It is true each time the machine enters a new state
* until the first transition is evaluated.
*
* TRANSITIONS
* Each state has transitions defined in setup(). Transitions
* require two parameters,
* 1. The transition test function that
* returns a boolean value indicating whether or not the
* transition occured,
* 2. The number of the target state. The target state can also
* be specified passing the state pointer. This could point to
* the same state it is in, if you want to dynamically set the
* transition target. To do so, use state->setTransition(). You
* must pass the index of the transition you want to modify and
* the number of the target state.
*
* Transitions are evaluated by the state machine after the state
* logic has executed. If none of the transitions evaluate to
* true, then the machine stays in the current state.
*
*
* Author: Jose Rullan
* Date: 10/December/17
* Project's page: https://github.com/jrullan/StateMachine
* License: MIT
****************************************************************/
#include <StateMachine.h>
const int STATE_DELAY = 1000;
int randomState = 0;
const int LED = 13;
StateMachine machine = StateMachine();
/*
* Example of using a lambda (or anonymous function) callback
* instead of providing the address of an existing function.
* Also example of using the attribute executeOnce to execute
* some part of the code only once per state.
*/
State* S0 = machine.addState([](){
Serial.println("State 0, anonymous function");
if(machine.executeOnce){
Serial.println("Execute Once");
digitalWrite(LED,!digitalRead(LED));
}
});;
/*
* The other way to define states.
* (Looks cleaner)
* Functions must be defined in the sketch
*/
State* S1 = machine.addState(&state1);
State* S2 = machine.addState(&state2);
State* S3 = machine.addState(&state3);
State* S4 = machine.addState(&state4);
State* S5 = machine.addState(&state5);
void setup() {
Serial.begin(115200);
pinMode(LED,OUTPUT);
randomSeed(A0);
/*
* Example of a transition that uses a lambda
* function, and sets the transition (first one)
* to a random state.
*
* Add only one transition(index=0)
* that points to randomly selected states
* Initially points to itself.
*/
S0->addTransition([](){
randomState = random(0,6);
Serial.print("Transitioning to random state ");
Serial.println(randomState);
S0->setTransition(0,randomState);
return true;
},S0);
/*
* The other way to define transitions.
* (Looks cleaner)
* Functions must be defined in the sketch
*/
S1->addTransition(&transitionS1S2,S2);
S2->addTransition(&transitionS2S3,S3);
S3->addTransition(&transitionS3S4,S4);
S4->addTransition(&transitionS4S5,S5);
S5->addTransition(&transitionS5S0,S0);
S5->addTransition(&transitionS5S2,S2);
}
void loop() {
machine.run();
delay(STATE_DELAY);
}
//=======================================
void state1(){
Serial.println("State 1");
}
bool transitionS1S2(){
return true;
}
//-------------------------
void state2(){
Serial.println("State 2");
}
bool transitionS2S3(){
return true;
}
//------------------------
void state3(){
Serial.println("State 3");
}
bool transitionS3S4(){
return true;
}
//-------------------------
void state4(){
Serial.println("State 4");
}
bool transitionS4S5(){
return true;
}
//-------------------------
void state5(){
Serial.println("State 5");
}
bool transitionS5S0(){
return random(0,2);
}
bool transitionS5S2(){
return true;
}

86
lib/StateMachine-1.0.13b/examples/states_timers/states_timers.ino Normal file
View file

@ -0,0 +1,86 @@
/******************************************
*
* This is a test combining the StateMachine
* and neotimer in dynamically generated states
* and timer objects.
*
* Because these are dynamically generated
* the compiler can't really determine the
* correct memory usage and the microcontroller
* will crash after a given quantity of states.
*
* Currently more than 25 states crashes the micro
*
******************************************/
#include <neotimer.h>
#include <StateMachine.h>
#define STATES 25
StateMachine machine = StateMachine();
State* states[STATES];
Neotimer timers[STATES];
void setup() {
Serial.begin(9600);
randomSeed(analogRead(0));
for(int i=0; i<STATES; i++){
int randomDuration = random(500,3000);
Serial.print("Duration ");
Serial.print(i);
Serial.print(" ");
Serial.println(randomDuration);
timers[i]= Neotimer(randomDuration);
// Add a state to the machine with code to display currentState
states[i] = machine.addState(&stateLogic);
/* LAMBDA STATE
states[i] = machine.addState([](){
if(machine.executeOnce){
Serial.print("State ");
Serial.print(machine.currentState);
Serial.println(" Execute Once");
timers[machine.currentState].stop();
timers[machine.currentState].start();
}
});*/
// Add a transition to the state, for the next state
states[i]->addTransition(&stateTransition, (i==(STATES-1)) ? 0 : i+1);
/* LAMBDA TRANSITION
states[i]->addTransition([](){
return (timers[machine.currentState].done())?true:false;
},(i==(STATES-1)) ? 0 : i+1);
*/
}
}
void loop(){
machine.run();
}
/******************************************
* Functions to be used as State logic
* and transitions logic
* if not using lambda functions.
******************************************/
void stateLogic(){
if(machine.executeOnce){
Serial.print("State ");
Serial.print(machine.currentState);
Serial.println(" Execute Once");
timers[machine.currentState].stop();
timers[machine.currentState].start();
}
}
bool stateTransition(){
return (timers[machine.currentState].done())?true:false;
}

16
lib/StateMachine-1.0.13b/keywords.txt Normal file
View file

@ -0,0 +1,16 @@
StateMachine KEYWORD1
State KEYWORD1
Transition KEYWORD1
execute KEYWORD2
evalTransitions KEYWORD2
addTransition KEYWORD2
stateLogic KEYWORD2
transitions KEYWORD2
init KEYWORD2
run KEYWORD2
addState KEYWORD2
currentState KEYWORD2
stateList KEYWORD2
index KEYWORD2
setTransition KEYWORD2
executeOnce KEYWORD2

11
lib/StateMachine-1.0.13b/library.properties Normal file
View file

@ -0,0 +1,11 @@
name=StateMachine
category=Device Control
version=1.0.13
author=Jose Rullan <jerullan@yahoo.com>
maintainer=Jose Rullan <jerullan@yahoo.com>
sentence="A simple state machine implementation."
url=http://github.com/jrullan/StateMachine
paragraph=A state machine is implemented by defining state logic as a function in your sketch. Transitions are implemented as functions returning a boolean value and a next state number. Requires LinkedList library https://github.com/ivanseidel/LinkedList.
architectures=*
includes=StateMachine.h
depends=LinkedList

21
lib/StateMachine-1.0.13b/license.txt Normal file
View file

@ -0,0 +1,21 @@
MIT License
Copyright (c) 2017 jrullan
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

71
lib/StateMachine-1.0.13b/src/State.cpp Normal file
View file

@ -0,0 +1,71 @@
#include "State.h"
State::State(){
transitions = new LinkedList<struct Transition*>();
};
State::~State(){};
/*
* Adds a transition structure to the list of transitions
* for this state.
* Params:
* conditionFunction is the address of a function that will be evaluated
* to determine if the transition occurs
* state is the state to transition to
*/
void State::addTransition(bool (*conditionFunction)(), State* s){
struct Transition* t = new Transition{conditionFunction,s->index};
transitions->add(t);
}
/*
* Adds a transition structure to the list of transitions
* for this state.
* Params:
* conditionFunction is the address of a function that will be evaluated
* to determine if the transition occurs
* stateNumber is the number of the state to transition to
*/
void State::addTransition(bool (*conditionFunction)(), int stateNumber){
struct Transition* t = new Transition{conditionFunction,stateNumber};
transitions->add(t);
}
/*
* Evals all transitions sequentially until one of them is true.
* Returns:
* The stateNumber of the transition that evaluates to true
* -1 if none evaluate to true ===> Returning index now instead to avoid confusion between first run and no transitions
*/
int State::evalTransitions(){
if(transitions->size() == 0) return index;
bool result = false;
for(int i=0;i<transitions->size();i++){
result = transitions->get(i)->conditionFunction();
if(result == true){
return transitions->get(i)->stateNumber;
}
}
return index;
}
/*
* Execute runs the stateLogic and then evaluates
* all available transitions. The transition that
* returns true is returned.
*/
int State::execute(){
stateLogic();
return evalTransitions();
}
/*
* Method to dynamically set a transition
*/
int State::setTransition(int index, int stateNo){
if(transitions->size() == 0) return -1;
transitions->get(index)->stateNumber = stateNo;
return stateNo;
}

43
lib/StateMachine-1.0.13b/src/State.h Normal file
View file

@ -0,0 +1,43 @@
#include <LinkedList.h>
#ifndef _STATE_H
#define _STATE_H
/*
* Transition is a structure that holds the address of
* a function that evaluates whether or not not transition
* from the current state and the number of the state to transition to
*/
struct Transition{
bool (*conditionFunction)();
int stateNumber;
};
/*
* State represents a state in the statemachine.
* It consists mainly of the address of the function
* that contains the state logic and a collection of transitions
* to other states.
*/
class State{
public:
State();
~State();
void addTransition(bool (*c)(), State* s);
void addTransition(bool (*c)(), int stateNumber);
int evalTransitions();
int execute();
int setTransition(int index, int stateNumber); //Can now dynamically set the transition
// stateLogic is the pointer to the function
// that represents the state logic
void (*stateLogic)();
LinkedList<struct Transition*> *transitions;
int index;
};
#endif

71
lib/StateMachine-1.0.13b/src/StateMachine.cpp Normal file
View file

@ -0,0 +1,71 @@
#include "StateMachine.h"
StateMachine::StateMachine(){
stateList = new LinkedList<State*>();
};
StateMachine::~StateMachine(){};
/*
* Main execution of the machine occurs here in run
* The current state is executed and it's transitions are evaluated
* to determine the next state.
*
* By design, only one state is executed in one loop() cycle.
*/
void StateMachine::run(){
//Serial.println("StateMachine::run()");
// Early exit, no states are defined
if(stateList->size() == 0) return;
// Initial condition
if(currentState == -1){
currentState = 0;
}
// Execute state logic and return transitioned
// to state number. Remember the current state then check
// if it wasnt't changed in state logic. If it was, we
// should ignore predefined transitions.
int initialState = currentState;
int next = stateList->get(currentState)->execute();
if(initialState == currentState){
executeOnce = (currentState == next)?false:true;
currentState = next;
}
}
/*
* Adds a state to the machine
* It adds the state in sequential order.
*/
State* StateMachine::addState(void(*functionPointer)()){
State* s = new State();
s->stateLogic = functionPointer;
stateList->add(s);
s->index = stateList->size()-1;
return s;
}
/*
* Jump to a state
* given by a pointer to that state.
*/
State* StateMachine::transitionTo(State* s){
this->currentState = s->index;
this->executeOnce = true;
return s;
}
/*
* Jump to a state
* given by a state index number.
*/
int StateMachine::transitionTo(int i){
if(i < stateList->size()){
this->currentState = i;
this->executeOnce = true;
return i;
}
return currentState;
}

31
lib/StateMachine-1.0.13b/src/StateMachine.h Normal file
View file

@ -0,0 +1,31 @@
#include <LinkedList.h>
#include "State.h"
#ifndef _STATEMACHINE_H
#define _STATEMACHINE_H
class StateMachine
{
public:
// Methods
StateMachine();
~StateMachine();
void init();
void run();
// When a stated is added we pass the function that represents
// that state logic
State* addState(void (*functionPointer)());
State* transitionTo(State* s);
int transitionTo(int i);
// Attributes
LinkedList<State*> *stateList;
bool executeOnce = true; //Indicates that a transition to a different state has occurred
int currentState = -1; //Indicates the current state number
};
#endif

1
platformio.ini
View file

@ -16,4 +16,5 @@ upload_protocol = teensy-cli
lib_deps = lib_deps =
adafruit/Adafruit NeoPixel@^1.10.1 adafruit/Adafruit NeoPixel@^1.10.1
gitlab-simple-serial-protocol/SimpleSerialProtocol@^2.4.0 gitlab-simple-serial-protocol/SimpleSerialProtocol@^2.4.0
ivanseidel/LinkedList@0.0.0-alpha+sha.dac3874d28
src_filter = +<*> -<.git/> -<.svn/> -<example/> -<examples/> -<test/> -<tests/> -<scratch*> src_filter = +<*> -<.git/> -<.svn/> -<example/> -<examples/> -<test/> -<tests/> -<scratch*>

459
src/main.cpp
View file

@ -1,10 +1,13 @@
#include <Arduino.h> #include <Arduino.h>
#include <Adafruit_NeoPixel.h> #include <Adafruit_NeoPixel.h>
#include <SimpleSerialProtocol.h> #include <SimpleSerialProtocol.h>
#include <StateMachine.h>
#include "Commands.h" #include "Commands.h"
#include "Motor.h" #include "Motor.h"
// #include "Serial_Comm.h" // #include "Serial_Comm.h"
/*-------- Pin definitions --------*/
// Vertical motor top // Vertical motor top
#define VERT_UP_PWM 3 #define VERT_UP_PWM 3
#define VERT_UP_AIN2 4 #define VERT_UP_AIN2 4
@ -44,30 +47,67 @@
#define LED_BACK 14 #define LED_BACK 14
#define LED_COUNT_BACK 72 #define LED_COUNT_BACK 72
void on_serial_error(uint8_t errorNum); /*-------- State Definitions --------*/
volatile int32_t hor_pos; // States - implementations below loop()
volatile int32_t vert_pos; void state_post();
void state_zero();
void state_init_callbacks();
void state_wait_serial();
void state_serial_com();
void state_error();
int32_t hor_aim; // Transitions - implementations below loop()
int32_t vert_aim; bool transition_post_zero();
bool transition_zero_init();
bool transition_init_wait();
bool transition_wait_sercom();
bool up; // Statemachine setup
StateMachine sm = StateMachine();
State* S00 = sm.addState(&state_post);
State* S10 = sm.addState(&state_zero);
State* S20 = sm.addState(&state_init_callbacks);
State* S30 = sm.addState(&state_wait_serial);
State* S40 = sm.addState(&state_serial_com);
State* SER = sm.addState(&state_error);
// Heartbeat blink interval constants
#define WAIT_ON_MS 200 // Blink when waiting for Serial
#define WAIT_OFF_MS 1800
#define ERROR_ON_MS 1000 // Blink when in error state
#define ERROR_OFF_MS 500
/*-------- Variables --------*/
// Heartbeat blinker times
uint32_t on_time;
uint32_t off_time;
// Statemachine booleans
bool handshake_complete;
// Position counters
volatile int16_t hor_pos;
volatile int16_t vert_pos;
/*-------- Objects --------*/
// Motors
Motor vert_up(VERT_UP_PWM, VERT_UP_AIN1, VERT_UP_AIN2); Motor vert_up(VERT_UP_PWM, VERT_UP_AIN1, VERT_UP_AIN2);
Motor vert_down(VERT_DOWN_PWM, VERT_DOWN_AIN1, VERT_DOWN_AIN2); Motor vert_down(VERT_DOWN_PWM, VERT_DOWN_AIN1, VERT_DOWN_AIN2);
Motor horz_left(HORZ_LEFT_PWM, HORZ_LEFT_AIN1, HORZ_LEFT_AIN2); Motor horz_left(HORZ_LEFT_PWM, HORZ_LEFT_AIN1, HORZ_LEFT_AIN2);
Motor horz_right(HORZ_RIGHT_PWM, HORZ_RIGHT_AIN1, HORZ_RIGHT_AIN2); Motor horz_right(HORZ_RIGHT_PWM, HORZ_RIGHT_AIN1, HORZ_RIGHT_AIN2);
// LEDs
Adafruit_NeoPixel led_front(LED_COUNT_FRONT, LED_FRONT, NEO_GBRW + NEO_KHZ800); Adafruit_NeoPixel led_front(LED_COUNT_FRONT, LED_FRONT, NEO_GBRW + NEO_KHZ800);
Adafruit_NeoPixel led_back(LED_COUNT_BACK, LED_BACK, NEO_GBRW + NEO_KHZ800); Adafruit_NeoPixel led_back(LED_COUNT_BACK, LED_BACK, NEO_GBRW + NEO_KHZ800);
bool back; /*-------- Serial Communication --------*/
bool led_on; // Error handler
int led_n; void serial_on_error(uint8_t errorNum);
u_int8_t brightness;
u_int8_t color;
// serial handshake was performed
bool serial_connected; bool serial_connected;
// inintialize hardware constants // inintialize hardware constants
@ -75,46 +115,146 @@ const long BAUDRATE = 115200; // speed of serial connection
const long CHARACTER_TIMEOUT = 500; // wait max 500 ms between single chars to be received const long CHARACTER_TIMEOUT = 500; // wait max 500 ms between single chars to be received
// Create instance. Pass Serial instance. Define command-id-range within Simple Serial Protocol is listening (here: a - z) // Create instance. Pass Serial instance. Define command-id-range within Simple Serial Protocol is listening (here: a - z)
SimpleSerialProtocol ssp(Serial1, BAUDRATE, CHARACTER_TIMEOUT, on_serial_error, 0, 'Z'); // ASCII: 'a' - 'z' (26 byes of RAM is reserved) SimpleSerialProtocol ssp(Serial1, BAUDRATE, CHARACTER_TIMEOUT, serial_on_error, 0, 'Z'); // ASCII: 'a' - 'z' (26 byes of RAM is reserved)
/**
* @brief Generic encoder logic for callbacks
*
* @param pinA
* @param pinB
* @return int32_t
*/
int32_t count(int pinA, int pinB) { int32_t count(int pinA, int pinB) {
if (digitalRead(pinA)) return digitalRead(pinB) ? -1 : 1; if (digitalRead(pinA)) return digitalRead(pinB) ? -1 : 1;
else return digitalRead(pinB) ? 1 : -1; else return digitalRead(pinB) ? 1 : -1;
} }
/**
* @brief Callback for horizontal counting
*
*/
void hor_count() { void hor_count() {
hor_pos += count(HORZ_CNT_INNER, HORZ_CNT_OUTER); // TODO enable: if (!digitalRead(HORZ_END_OUTER))
} hor_pos -= count(HORZ_CNT_INNER, HORZ_CNT_OUTER);
void vert_count() {
vert_pos += count(VERT_CNT_INNER, VERT_CNT_OUTER);
} }
/** /**
* @brief Generic motor control (full speed). Call every 10us for good results. * @brief Callback for vertical counting
* *
*/ */
void mot_control(Motor &mot1, Motor &mot2, int32_t pos, int32_t aim) { void vert_count() {
if (pos < aim) { // TODO enable: if (!digitalRead(VERT_END_OUTER))
mot1.run(255, false); vert_pos -= count(VERT_CNT_INNER, VERT_CNT_OUTER);
mot2.run(127, false); }
} else if (vert_pos > vert_aim) {
mot2.run(255, true); /**
mot1.run(127, true); * @brief Blink the internal LED with defined on- and off- times. Call in loop to blink.
*
* @param on_interval time LED stays on in millis
* @param off_interval time LED is off in millis
*/
void blink_builtin(int on_interval, int off_interval) {
uint32_t cur_time = millis();
if (digitalRead(LED_BUILTIN)) {
if (off_time < cur_time) {
digitalWrite(LED_BUILTIN, LOW);
on_time = cur_time + off_interval;
}
} else { } else {
mot1.stop(false); if (on_time < cur_time) {
mot2.stop(false); digitalWrite(LED_BUILTIN, HIGH);
// vert_aim = (vert_aim == 50) ? 0 : 50; off_time = cur_time + on_interval;
}
} }
} }
uint8_t color_value(uint8_t col_from, uint8_t col_to, uint32_t cur_time, uint32_t duration) { /**
float_t perc = (float) cur_time / (float) duration; * @brief Generic scroll zeroing code
float_t col = (float) (col_to - col_from) * perc; *
* @param mot1 Motor in positive direction
* @param mot2 Motor in negative direction
* @param zero_pin Sensor pin where LOW enables count
* @param end_pin Sensor pin attached to emergency stop (end-stop)
*/
void zero_motor(Motor &mot1, Motor &mot2, int zero_pin, int end_pin) {
bool is_zero = digitalRead(end_pin) & !digitalRead(zero_pin);
uint32_t end_time = millis() + 10000;
while (!is_zero & (millis() < end_time))
{
mot2.run(255, false);
mot1.run(127, false);
is_zero = digitalRead(zero_pin);
}
delayMicroseconds(20);
end_time = millis() + 200;
while (digitalRead(zero_pin) & (millis() < end_time))
{
mot1.run(200, true);
mot2.stop(true);
}
mot1.stop(false);
mot2.stop(false);
}
/**
* @brief Serial communication error handler
*
* @param errno
*/
void serial_on_error(uint8_t errno) {
Serial.printf("SSP error %i \n", errno);
ssp.writeCommand(ERROR);
ssp.writeInt8(errno);
ssp.writeEot();
sm.transitionTo(SER);
}
/**
* @brief Send RECEIVED+EOT bytes over Serial
*
*/
void serial_received() {
ssp.writeCommand(RECEIVED);
ssp.writeEot();
}
/**
* @brief Helper function to calculate transition between two colors
*
* @param col_from 8bit color value start
* @param col_to 8bit color value end
* @param perc percentage in float
* @return uint8_t 8bit color value at percentage of transition
*/
uint8_t color_value(uint8_t col_from, uint8_t col_to, float_t perc) {
float_t col;
if (col_from < col_to) {
col = col_from + (float) (col_to - col_from) * perc;
} else {
col = col_from - (float) (col_from - col_to) * perc;
}
return (uint8_t) (col + 0.5); return (uint8_t) (col + 0.5);
} }
void led_fade(Adafruit_NeoPixel &led, int8_t to_R, int8_t to_G, int8_t to_B, int8_t to_W, uint32_t time_ms) { /**
* @brief Generic fade LEDs from one color to another
*
* @param led
*/
void serial_led_fade(Adafruit_NeoPixel &led) {
uint8_t to_R = ssp.readUnsignedInt8();
uint8_t to_G = ssp.readUnsignedInt8();
uint8_t to_B = ssp.readUnsignedInt8();
uint8_t to_W = ssp.readUnsignedInt8();
uint32_t time_ms = ssp.readUnsignedInt32();
ssp.readEot();
Serial.printf("Received BACKLIGHT (%i, %i, %i, %i, %i) \n", to_R, to_G, to_B, to_W, time_ms);
uint32_t startcol = led.getPixelColor(0); uint32_t startcol = led.getPixelColor(0);
Serial.printf("col = %i \n", startcol); Serial.printf("col = %i \n", startcol);
uint8_t from_W = (startcol & 0xff000000) >> 24; uint8_t from_W = (startcol & 0xff000000) >> 24;
@ -128,29 +268,63 @@ void led_fade(Adafruit_NeoPixel &led, int8_t to_R, int8_t to_G, int8_t to_B, int
uint32_t end_time = start_time + time_ms; uint32_t end_time = start_time + time_ms;
while (millis() < end_time) { while (millis() < end_time) {
u_int32_t cur_time = millis() - start_time; float_t perc = (float) (millis() - start_time) / (float) time_ms;
uint32_t color = led.Color(color_value(from_R, to_R, cur_time, time_ms), uint32_t color = led.Color(color_value(from_R, to_R, perc),
color_value(from_G, to_G, cur_time, time_ms), color_value(from_G, to_G, perc),
color_value(from_B, to_B, cur_time, time_ms), color_value(from_B, to_B, perc),
color_value(from_W, to_W, cur_time, time_ms)); color_value(from_W, to_W, perc));
led.fill(color); led.fill(color);
led.show(); led.show();
// Serial.printf("t = %i, c = %i \n", cur_time, color); }
led.fill(led.Color(to_R, to_G, to_B, to_W));
led.show();
serial_received();
}
/**
* @brief Generic motor control (full speed). Call every 10us for good results.
*
*/
bool mot_control(Motor &mot1, Motor &mot2, volatile int16_t &pos, int16_t &aim) {
if (pos < aim) {
mot1.run(255, true);
mot2.run(127, true);
return false;
} else if (pos > aim) {
mot2.run(255, false);
mot1.run(127, false);
return false;
} else {
mot1.stop(false);
mot2.stop(false);
return true;
} }
} }
void on_serial_error(uint8_t errno) { /**
Serial.printf("SSP error %i \n", errno); * @brief Generic serial command handler to drive scroll to position
ssp.writeCommand(ERROR); *
ssp.writeInt8(errno); * @param mot1 Motor in positive direction
ssp.writeEot(); * @param mot2 Motor in negative direction
} * @param pos position variable
*/
void serial_received() { void serial_motor(Motor &mot1, Motor &mot2, volatile int16_t &pos) {
ssp.writeCommand(RECEIVED); int16_t inc = ssp.readInt16();
ssp.writeEot(); ssp.readEot();
int16_t aim = pos + inc;
while (!mot_control(mot1, mot2, pos, aim)) {
Serial.printf("aim = %i, pos = %i \n", aim, pos);
delayMicroseconds(10);
}
} }
/**
* @brief Serial command handler for handshake (responds to HELLO and ALREADY_CONNECTED)
*
*/
void serial_hello() { void serial_hello() {
ssp.readEot(); ssp.readEot();
@ -161,54 +335,75 @@ void serial_hello() {
} }
else { else {
ssp.writeCommand(ALREADY_CONNECTED); ssp.writeCommand(ALREADY_CONNECTED);
handshake_complete = true;
Serial.println("Handshake complete."); Serial.println("Handshake complete.");
} }
ssp.writeEot(); ssp.writeEot();
} }
/**
* @brief Serial command handler for BACKLIGHT
*
*/
void serial_backlight() { void serial_backlight() {
uint8_t r = ssp.readUnsignedInt8(); Serial.println("Received BACKLIGHT");
uint8_t g = ssp.readUnsignedInt8(); serial_led_fade(led_back);
uint8_t b = ssp.readUnsignedInt8();
uint8_t w = ssp.readUnsignedInt8();
uint32_t t = ssp.readUnsignedInt32();
ssp.readEot();
Serial.printf("Received BACKLIGHT (%i, %i, %i, %i, %i) \n", r, g, b, w, t);
led_fade(led_back, r, g, b, w, t);
serial_received();
} }
/**
* @brief Serial command handler for FRONTLIGHT
*
*/
void serial_frontlight() { void serial_frontlight() {
ssp.readEot();
Serial.println("Received FRONTLIGHT"); Serial.println("Received FRONTLIGHT");
serial_received(); serial_led_fade(led_front);
} }
/**
* @brief Serial command handler for MOTOR_V
*
*/
void serial_motor_v() { void serial_motor_v() {
ssp.readEot();
Serial.println("Received MOTOR_V"); Serial.println("Received MOTOR_V");
serial_motor(vert_up, vert_down, vert_pos);
serial_received(); serial_received();
} }
/**
* @brief Serial command handler for MOTOR_H
*
*/
void serial_motor_h() { void serial_motor_h() {
ssp.readEot(); Serial.println("Received MOTOR_V");
Serial.println("Received MOTOR_H"); serial_motor(horz_left, horz_right, hor_pos);
serial_received(); serial_received();
} }
/**
* @brief Serial command handler for RECORD
*
*/
void serial_record() { void serial_record() {
ssp.readEot(); ssp.readEot();
Serial.println("Received RECORD"); Serial.println("Received RECORD");
serial_received(); serial_received();
} }
/**
* @brief Serial command handler for REWIND
*
*/
void serial_rewind() { void serial_rewind() {
ssp.readEot(); ssp.readEot();
Serial.println("Received REWIND"); Serial.println("Received REWIND");
serial_received(); serial_received();
} }
/**
* @brief Serial command handler for USER_INTERACT
*
*/
void serial_userinteract() { void serial_userinteract() {
ssp.readEot(); ssp.readEot();
Serial.println("Received USER_INTERACT"); Serial.println("Received USER_INTERACT");
@ -220,12 +415,13 @@ void setup() {
Serial1.begin(115200); Serial1.begin(115200);
pinMode(LED_BUILTIN, OUTPUT); pinMode(LED_BUILTIN, OUTPUT);
digitalWrite(LED_BUILTIN, LOW);
hor_pos = 0; hor_pos = 0;
vert_pos = 0; vert_pos = 0;
hor_aim = 0; on_time = 0;
vert_aim = -20; off_time = 0;
vert_up.setup(); vert_up.setup();
vert_down.setup(); vert_down.setup();
@ -247,25 +443,12 @@ void setup() {
digitalWrite(VERT_CNT_INNER, LOW); digitalWrite(VERT_CNT_INNER, LOW);
digitalWrite(VERT_CNT_OUTER, LOW); digitalWrite(VERT_CNT_OUTER, LOW);
attachInterrupt(digitalPinToInterrupt(HORZ_CNT_INNER), hor_count, CHANGE);
// attachInterrupt(digitalPinToInterrupt(HORZ_CNT_OUTER), hor_count, CHANGE);
attachInterrupt(digitalPinToInterrupt(VERT_CNT_INNER), vert_count, CHANGE);
// attachInterrupt(digitalPinToInterrupt(VERT_CNT_OUTER), vert_count, CHANGE);
back = true;
led_on = true;
brightness = 0;
color = 0;
led_back.begin(); led_back.begin();
led_back.show(); led_back.show();
led_front.begin(); led_front.begin();
led_front.show(); led_front.show();
serial_connected = false;
ssp.init(); ssp.init();
ssp.registerCommand(HELLO, serial_hello); ssp.registerCommand(HELLO, serial_hello);
ssp.registerCommand(ALREADY_CONNECTED, serial_hello); ssp.registerCommand(ALREADY_CONNECTED, serial_hello);
@ -276,8 +459,130 @@ void setup() {
ssp.registerCommand(RECORD, serial_record); ssp.registerCommand(RECORD, serial_record);
ssp.registerCommand(REWIND, serial_rewind); ssp.registerCommand(REWIND, serial_rewind);
ssp.registerCommand(USER_INTERACT, serial_userinteract); ssp.registerCommand(USER_INTERACT, serial_userinteract);
S00->addTransition(transition_post_zero, S10);
S10->addTransition(transition_zero_init, S20);
S20->addTransition(transition_init_wait, S30);
S30->addTransition(transition_wait_sercom, S40);
} }
void loop() { void loop() {
// Just run the state machine
sm.run();
// blink_builtin(WAIT_ON_MS, WAIT_OFF_MS);
}
/**
* @brief State Power-On-Self-Test
*
*/
void state_post() {
if (sm.executeOnce) {
digitalWrite(LED_BUILTIN, HIGH);
}
Serial.println("State POST.");
}
/**
* @brief State Zeroing motors
*
*/
void state_zero() {
Serial.println("State Zeroing.");
zero_motor(vert_up, vert_down, VERT_END_INNER, VERT_END_OUTER);
zero_motor(horz_left, horz_right, HORZ_END_OUTER, HORZ_END_INNER); // TODO check this
}
/**
* @brief State Initialize callbacks (for counting)
*
*/
void state_init_callbacks() {
Serial.println("State Initialize Callbacks.");
attachInterrupt(digitalPinToInterrupt(HORZ_CNT_INNER), hor_count, CHANGE);
// attachInterrupt(digitalPinToInterrupt(HORZ_CNT_OUTER), hor_count, CHANGE);
attachInterrupt(digitalPinToInterrupt(VERT_CNT_INNER), vert_count, CHANGE);
// attachInterrupt(digitalPinToInterrupt(VERT_CNT_OUTER), vert_count, CHANGE);
}
/**
* @brief State wait for serial handshake
*
*/
void state_wait_serial() {
if (sm.executeOnce) {
serial_connected = false;
handshake_complete = false;
Serial.println("State Waiting for Serial Handshake.");
// digitalWrite(LED_BUILTIN, LOW);
// off_time = millis() + WAIT_ON_MS;
// on_time = millis() + WAIT_OFF_MS;
}
blink_builtin(WAIT_ON_MS, WAIT_OFF_MS);
// ssp.loop();
}
/**
* @brief State accept serial communications
*
*/
void state_serial_com() {
if (sm.executeOnce) {
Serial.println("State Serial Communication.");
}
ssp.loop(); ssp.loop();
} }
/**
* @brief State an error occurred
*
*/
void state_error() {
if (sm.executeOnce) {
Serial.println("State Error.");
}
blink_builtin(ERROR_ON_MS, ERROR_OFF_MS);
}
/**
* @brief Transition POST to zeroing. Always true.
*
* @return true
*/
bool transition_post_zero() {
return true;
}
/**
* @brief Transition zeroing to callback initialisation. Always true.
*
* @return true
*/
bool transition_zero_init() {
return true;
}
/**
* @brief Transition callback initialisation to wait for serial handshake. Always true.
*
* @return true
*/
bool transition_init_wait() {
return true;
}
/**
* @brief Transition serial handshake to serial communication. True when handshake complete.
*
* @return true
* @return false
*/
bool transition_wait_sercom() {
digitalWrite(LED_BUILTIN, LOW);
return handshake_complete;
}