-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathPOOL_PLC.ino
149 lines (128 loc) · 4.24 KB
/
POOL_PLC.ino
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
/**
* @file POOL_PLC.ino
* @author Ryan Johnson (ryan@johnsonweb.us)
* @brief Main program for the Pool PLC
* @version 0.1
* @date 2020-06-13
*
* @copyright Copyright (c) 2020
*
*/
#include "ACEINNAInclinometer.h"
#include "FaultHandling.h"
#include "InclinometerModel.h"
#include "InclinometerModule.h"
#include "PersistentStorage.h"
#include "MotionController.h"
#include "PinMappings.h"
#include <stlport.h>
#include <Eigen30.h>
#include <Adafruit_EEPROM_I2C.h>
#include <Adafruit_FRAM_I2C.h>
#include <Controllino.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <SPI.h>
Fault::Handler *faultHandler;
PersistentStorage::Manager storageManager;
Inclinometer::ACEINNAInclinometer aceinna(Serial2, 0.5);
Inclinometer::Module inclinometer1(&aceinna, 0.0);
Motion::MotionController motionController(inclinometer1);
bool raising = false;
bool lowering = false;
void setup()
{
faultHandler = Fault::Handler::instance();
// put your setup code here, to run once:
pinMode(STATUS_PIN, OUTPUT);
pinMode(FAULT_PIN, OUTPUT);
pinMode(ZERO_BUTTON, INPUT);
pinMode(REFLASH_ACEINNA, INPUT);
pinMode(RAISE_BUTTON, INPUT);
pinMode(LOWER_BUTTON, INPUT);
digitalWrite(STATUS_PIN, LOW);
SPI.begin();
Serial.begin(9600);
Serial.println("Starting up");
if (!inclinometer1.begin()) {
faultHandler->setFaultCode(Fault::INCLINOMETER_INIT);
}
Serial.println("Inclinometer began");
if (!storageManager.begin()) {
faultHandler->setFaultCode(Fault::FRAM_INIT);
}
Serial.println("Storage began");
storageManager.readMap();
inclinometer1.importZero(storageManager.getMap()->zeroFrame1);
motionController.Initialize();
motionController.Step(); // Step the motion controller once to show any active faults
Serial.println("Initialized");
}
/**
* @brief This function is the main periodic loop, which runs forever (until the
* controller powers off or resets)
*/
void loop()
{
// Determine if raising or lowering
bool wasRaising = raising;
bool wasLowering = lowering;
raising = digitalRead(RAISE_BUTTON);
lowering = digitalRead(LOWER_BUTTON);
raising = raising && !lowering;
lowering = lowering && !raising;
if (!wasRaising && raising) {
motionController.RequestRaise();
}
if (!wasLowering && lowering) {
motionController.RequestLower();
}
if (!lowering && !raising && (wasLowering || wasRaising)) {
motionController.RequestOff();
}
// Step the motion controller
motionController.Step();
status_flash(motionController.GetState());
digitalWrite(FAULT_PIN, Fault::Handler::instance()->hasFault());
// Check if the user wanted to zero the inclinometer
if (digitalRead(ZERO_BUTTON) && !(raising || lowering)) {
storageManager.getMap()->zeroFrame1 = inclinometer1.zero();
storageManager.writeMap();
motionController.PopMessage("RESET LEVEL SENSOR");
delay(500);
}
// Check if the user wanted to reflash the aceinna module
if (digitalRead(REFLASH_ACEINNA) && !(raising || lowering)) {
aceinna.ProvisionACEINNAInclinometer();
motionController.PopMessage("FLASHED SENSE EEPROM");
delay(500);
}
// avoid thrashing the sensor too much
delay(10);
}
bool flash = false;
unsigned long last_pulse_at = 0;
void status_flash(Motion::MotionStateMachine::STATE state)
{
switch (state) {
case Motion::MotionStateMachine::STATE_MOVING:
digitalWrite(STATUS_PIN, HIGH);
break;
case Motion::MotionStateMachine::STATE_MOVEMENT_REQUESTED:
if (millis() - last_pulse_at > 500) {
flash = !flash;
last_pulse_at = millis();
digitalWrite(STATUS_PIN, flash);
}
break;
case Motion::MotionStateMachine::STATE_FAULTED:
if (millis() - last_pulse_at > 50) {
flash = !flash;
last_pulse_at = millis();
digitalWrite(STATUS_PIN, flash);
}
break;
default:
digitalWrite(STATUS_PIN, LOW);
}
}