interfisch
/
blinds_motorcontrol
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

implement homie

This commit is contained in:
interfisch 2021-02-07 23:34:54 +01:00
parent 7bb6d58c60
commit 1a0c9a5b39
2 changed files with 243 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
platformio.ini
View File

@ -13,7 +13,8 @@ platform = espressif8266
board = d1_mini
framework = arduino
monitor_speed = 57600
monitor_speed = 115200
lib_deps =
https://github.com/thomasfredericks/wemos_motor_shield
https://github.com/thomasfredericks/wemos_motor_shield
Homie@3.0.0

296
src/main.cpp
View File

@ -1,14 +1,25 @@
#include <Arduino.h>
#include <Wire.h>
#include <Homie.h>
//Upload config: platformio run --target uploadfs
/*
TODO:
- correct speedfactor when moving at pwm=100 for some time over opaque+clear OR implement speedfactor for up and lower state
- implement drive to position
- implement failure detection
- implement homie
- implement failure detection (timeouts)
- homie commands (find zero, test speed, etc)
*/
#define FW_NAME "blindctrl"
#define FW_VERSION "1.0.0"
HomieNode blind1Node("blindl", "Blind Left", "blind"); //paramters: topic, $name, $type
HomieNode blind2Node("blindr", "Blind Right", "blind"); //paramters: topic, $name, $type
#include "WEMOS_Motor.h"
//follow firmware flash guide for new wemos motor shield v1.0 https://github.com/thomasfredericks/wemos_motor_shield
@ -28,12 +39,13 @@ button button1;
button button2;
unsigned long last_sensor_read=0;
#define SENSOR_READ_INTERVAL 20 //in ms
#define SENSOR_READ_INTERVAL 5 //in ms
#define SENSE_FILTER_SIZE 20 //value will have a delay of td = SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL
#define CALCULATEPOSITIONESTIMATE_INTERVAL 100
unsigned long last_calculate_position_estimate=0;
#define POSITION_SPEEDMEASURE_HIGH 1000.0 //at which position (in mm) speedfactorHigh should be measured (should be at least two encoder segments beyond that left)
#define PIN_SENSE A0
@ -58,6 +70,7 @@ unsigned long last_sensor_led_switch=0;
#define MODE_FIND_END 1
#define MODE_MEASURE_SPEED 2
#define MODE_GO_TO_POS 3
#define MODE_ERROR 4
//model parameters/variables
struct blindmodel
@ -72,7 +85,8 @@ struct blindmodel
float start_first_clear; //distance from end marker to beginning of first clear section
//end marker should be on the opaque section. So that a full clear section follows
float speedfactor=1; //how much position units (mm) per second at pwm=100
float speedfactorLow=1; //how much position units (mm) per second at pwm=100. speedfactorLow for position at 0
float speedfactorHigh=1; //speedfactorHigh for position at 1000 mm. gets extrapolated above
int sense_clear_lower; //adc value lower limit for clear part. clear is around 70
int sense_clear_upper; //adc value upper limit for clear part
@ -92,6 +106,8 @@ struct blindmodel
uint8_t pin_sensor_led; //pin to activate sensor. high activates sensor
float set_position=0;
float softlimit_min=0;
float softlimit_max=1000; //doubled value for folding bilds
unsigned long last_sense_ok=0; //last time sensor measured class ok
//TODO: implement timeout if last_sense_ok gets too high.
@ -126,16 +142,29 @@ unsigned long last_motor_send=0;
#define ERRORCODE_POSITIONDIFFTOOHIGH 1 //deviation too high on position correction
#define ERRORCODE_N_NOT_NEXT 2 //skipped one transition. position jumped too far?
#define THRESHOLD_GO_TO_POS 20 //how close blind has to be to have reached position (in mm)
#define MINTIMEINTERVAL_POSITION 500 //how fast new position should be send via mqtt when moving
int getFitered(int* values,uint8_t size);
void classifySensorValue(blindmodel &blind);
void checkButton(button &btn);
void checkModes(blindmodel &blind);
void checkModes(blindmodel &blind, HomieNode &node);
void manualMoveHandler(button &btn, blindmodel &blind);
void readSensor(blindmodel &blind, int value);
void estimatePosition(blindmodel &blind);
void readSensor(blindmodel &blind, int value, HomieNode &node);
void estimatePosition(blindmodel &blind, HomieNode& node);
void errorCheck(blindmodel &blind);
void updateMotor(blindmodel &blind, Motor motor);
void setError(blindmodel &blind, uint8_t errorcode);
void setError(blindmodel &blind, uint8_t errorcode, HomieNode& node);
String modeNumToString(uint8_t modenum);
bool blind_l_positionHandler(const HomieRange& range, const String& value);
bool blind_r_positionHandler(const HomieRange& range, const String& value);
bool blind_l_cmdHandler(const HomieRange& range, const String& value);
bool blind_r_cmdHandler(const HomieRange& range, const String& value);
void loopHandler();
void setup() {
Serial.begin(115200);
@ -167,10 +196,13 @@ void setup() {
blind1.sense_opaque_upper=850;
blind1.sense_end_lower=850;
blind1.sense_end_upper=1024;
blind1.speedfactor=29;
blind1.speedfactorLow=28;
blind1.speedfactorHigh=24.5;
blind1.start_first_clear=27;
blind1.simulated_acc_dec=-150;
blind1.simulated_acc_inc=200;
blind1.softlimit_min=0;
blind1.softlimit_max=2500;
blind2.pin_sensor_led=PIN_SENSE_LED2;
blind2.length_clear=50;
@ -181,10 +213,13 @@ void setup() {
blind2.sense_opaque_upper=850;
blind2.sense_end_lower=850;
blind2.sense_end_upper=1024;
blind2.speedfactor=29;
blind1.speedfactorLow=28;
blind1.speedfactorHigh=24.5;
blind2.start_first_clear=27;
blind2.simulated_acc_dec=-150;
blind2.simulated_acc_inc=200;
blind2.softlimit_min=0;
blind2.softlimit_max=2500;
//Test
blind1.mode = MODE_FIND_END;
@ -193,6 +228,18 @@ void setup() {
//blind2.mode = MODE_FIND_END;
//blind2.mode_find_end_state=0; //reset mode find state
Homie_setFirmware(FW_NAME, FW_VERSION);
Homie_setBrand(FW_NAME);
Homie.setLoopFunction(loopHandler);
blind1Node.advertise("position").settable(blind_l_positionHandler); //function inputHandler gets called on new message on topic/input/set
blind1Node.advertise("debug");
blind1Node.advertise("estimationerror");
blind1Node.advertise("mode");
blind1Node.advertise("cmd").settable(blind_l_cmdHandler);
//TODO: same for blind2
Homie.setup();
@ -200,6 +247,9 @@ void setup() {
void loop() {
Homie.loop();
}
void loopHandler() {
checkButton(button1);
checkButton(button2);
@ -216,13 +266,13 @@ void loop() {
int rawsensorvalue=analogRead(PIN_SENSE);
switch (sensorreadID) {
case 0:
readSensor(blind1,rawsensorvalue);
readSensor(blind1,rawsensorvalue, blind1Node);
sensorreadID++;
digitalWrite(blind1.pin_sensor_led,LOW); //turn self off
digitalWrite(blind2.pin_sensor_led,HIGH); //turn next on
break;
case 1:
readSensor(blind2,rawsensorvalue);
readSensor(blind2,rawsensorvalue, blind2Node);
sensorreadID++;
digitalWrite(blind2.pin_sensor_led,LOW); //turn self off
digitalWrite(blind1.pin_sensor_led,HIGH); //turn next on
@ -234,16 +284,31 @@ void loop() {
last_sensor_led_switch=millis();
}
static float last_blind1_position=0;
static unsigned long lastsend_blind1_position=0; //time
if (last_blind1_position != blind1.position && millis()-lastsend_blind1_position>MINTIMEINTERVAL_POSITION) {
blind1Node.setProperty("position").send((String)blind1.position);
last_blind1_position=blind1.position;
lastsend_blind1_position=millis();
}
//TODO: same for blind2
checkModes(blind1);
checkModes(blind2);
static uint8_t last_blind1_mode=100;
if (last_blind1_mode!=blind1.mode)
{
blind1Node.setProperty("mode").send(modeNumToString(blind1.mode));
last_blind1_mode=blind1.mode;
}
checkModes(blind1, blind1Node);
checkModes(blind2, blind2Node);
errorCheck(blind1);
errorCheck(blind2);
//Estimate blind position and correct
estimatePosition(blind1);
estimatePosition(blind2);
estimatePosition(blind1, blind1Node);
estimatePosition(blind2, blind2Node);
//Update Motor Driver
updateMotor(blind1, M1);
@ -319,7 +384,7 @@ void manualMoveHandler(button &btn, blindmodel &blind)
}
}
void readSensor(blindmodel &blind, int value)
void readSensor(blindmodel &blind, int value, HomieNode &node)
{
if (millis() > last_sensor_read + SENSOR_READ_INTERVAL) {
blind.sense_read_pos=(blind.sense_read_pos+1)%SENSE_FILTER_SIZE; //next element
@ -349,20 +414,8 @@ void readSensor(blindmodel &blind, int value)
}
Serial.print("("); Serial.print(getFitered(blind.sense_read, SENSE_FILTER_SIZE)); Serial.print(")");
Serial.print(", mode=");
switch(blind.mode){
case MODE_IDLE:
Serial.print("IDL");
break;
case MODE_FIND_END:
Serial.print("FIN");
break;
case MODE_GO_TO_POS:
Serial.print("POS");
break;
case MODE_MEASURE_SPEED:
Serial.print("MSP");
break;
}
Serial.print(modeNumToString(blind.mode));
Serial.print(", speed=");
Serial.print(blind.speed);
Serial.print(", speedSim=");
@ -383,21 +436,23 @@ void errorCheck(blindmodel &blind) {
//TODO: led self test. turn off ,should be high value
}
void estimatePosition(blindmodel &blind) {
void estimatePosition(blindmodel &blind, HomieNode& node) {
float positional_speedfactor = blind.speedfactorLow + constrain(blind.position, blind.softlimit_min, blind.softlimit_max)*(blind.speedfactorHigh-blind.speedfactorLow)/POSITION_SPEEDMEASURE_HIGH; //interpolate/extrapolate speedfactor (speed is different when rolled up)
if (millis() > last_calculate_position_estimate + CALCULATEPOSITIONESTIMATE_INTERVAL) {
float _actualTime=(millis()-last_calculate_position_estimate)/1000.0; //in seconds
blind.speedSimulated+= constrain(blind.speed - blind.speedSimulated, blind.simulated_acc_dec*_actualTime, blind.simulated_acc_inc*_actualTime);
blind.position+= blind.speedSimulated/100.0*blind.speedfactor * _actualTime;
blind.position+= blind.speedSimulated/100.0*positional_speedfactor * _actualTime;
last_calculate_position_estimate=millis();
}
if (blind.mode!= MODE_FIND_END) {
//if (blind.mode!= MODE_FIND_END) {
if (blind.sense_status == SENSESTATUS_END && blind.last_sense_status != SENSESTATUS_END) { //entered end marker
blind.position=0;
float _filterdelay_correction=blind.speedSimulated/100.0*blind.speedfactor *SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL/1000.0;
float _filterdelay_correction=blind.speedSimulated/100.0*positional_speedfactor *SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL/1000.0;
blind.position += _filterdelay_correction; //correct for filter delay
Serial.print("Reached End marker. set Position="); Serial.println(blind.position);
node.setProperty("debug").send("Reached End marker. set Position="+(String)blind.position);
}
if (blind.sense_status == SENSESTATUS_CLEAR || blind.sense_status == SENSESTATUS_OPAQUE || blind.sense_status == SENSESTATUS_END) { //either opaque or clear (or end, only for last update used)
@ -412,18 +467,20 @@ void estimatePosition(blindmodel &blind) {
int _n = round((blind.position -blind.start_first_clear - blind.length_clear)/(blind.length_clear+blind.length_opaque)); //find closest n
blind.position = blind.start_first_clear+blind.length_clear + _n*(blind.length_clear+blind.length_opaque); //calculate position from _n
float _filterdelay_correction=blind.speedSimulated/100.0*blind.speedfactor *SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL/1000.0;
float _filterdelay_correction=blind.speedSimulated/100.0*positional_speedfactor *SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL/1000.0;
blind.position += _filterdelay_correction; //correct for filter delay
Serial.print("nearest n="); Serial.println(_n);
if (blind.last_n !=-1 && abs(blind.last_n-_n>1)) { //if last_n is known and if n is not the next (error)
setError(blind, ERRORCODE_N_NOT_NEXT);
if (blind.last_n !=-1 && abs(blind.last_n-_n>1) && blind.mode!= MODE_FIND_END) { //if last_n is known and if n is not the next (error)
setError(blind, ERRORCODE_N_NOT_NEXT, node);
}
blind.last_n=_n; //for error check
Serial.print("posCorrection="); Serial.print(_filterdelay_correction);
Serial.print(", DOWN&CLE->OPA or UP&OPA->CLE, before=");
Serial.print(", DOWN&CLE->OPA or UP&OPA->CLE, before=");
}else if((blind.speedSimulated>0 && blind.sense_status==SENSESTATUS_CLEAR) || (blind.speedSimulated<0 && blind.sense_status==SENSESTATUS_OPAQUE)) { //the other transition
@ -433,10 +490,11 @@ void estimatePosition(blindmodel &blind) {
Serial.print("n=");
Serial.println(_n);
blind.position = blind.start_first_clear + _n*(blind.length_clear+blind.length_opaque); //calculate position from _n
float _filterdelay_correction=blind.speedSimulated/100.0*blind.speedfactor*SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL/1000.0;
float _filterdelay_correction=blind.speedSimulated/100.0*positional_speedfactor*SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL/1000.0;
blind.position += _filterdelay_correction; //correct for filter delay
Serial.print("posCorrection="); Serial.print(_filterdelay_correction);
Serial.print(", UP&CLE->OPA or DOWN&OPA->CLE, before=");
@ -447,16 +505,20 @@ void estimatePosition(blindmodel &blind) {
Serial.print(blind.position);
Serial.print(", diff=");
Serial.println(blind.position-_position_before);
//node.setProperty("debug").send("diff="+(String)(blind.position-_position_before));
node.setProperty("estimationerror").send((String)(blind.position-_position_before));
if (abs(blind.position-_position_before)>=(blind.length_clear+blind.length_opaque)/2.0*DIFF_ERROR_FACTOR) {
setError(blind, ERRORCODE_POSITIONDIFFTOOHIGH);
if (blind.mode!= MODE_FIND_END) { //only check error if not in find_end mode
if (abs(blind.position-_position_before)>=(blind.length_clear+blind.length_opaque)/2.0*DIFF_ERROR_FACTOR) {
setError(blind, ERRORCODE_POSITIONDIFFTOOHIGH, node);
}
}
}
blind.last_sense_status = blind.sense_status; //update only if new one is opaque or clear
}
}
}
//}
}
void updateMotor(blindmodel &blind, Motor motor)
@ -476,7 +538,7 @@ void updateMotor(blindmodel &blind, Motor motor)
}
}
void checkModes(blindmodel &blind) {
void checkModes(blindmodel &blind, HomieNode &node) {
switch(blind.mode) {
case MODE_FIND_END:
switch(blind.mode_find_end_state) {
@ -493,8 +555,8 @@ void checkModes(blindmodel &blind) {
if (blind1.sense_status!=SENSESTATUS_END) {
blind.speed=0; //stop
blind.position=0;
//blind.mode=MODE_IDLE;
blind.mode=MODE_MEASURE_SPEED; //next measure speed
blind.mode=MODE_IDLE;
//blind.mode=MODE_MEASURE_SPEED; //next measure speed
blind.mode_find_end_state=0; //reset
blind.last_n=-1; //unknown
}
@ -506,6 +568,13 @@ void checkModes(blindmodel &blind) {
case MODE_MEASURE_SPEED:
switch(blind.mode_measure_speed_state) {
case 0: //drive down, start timing at clear
if (blind.position>100) { //cancel
node.setProperty("cmd").send("canceled. Position>100: pos="+(String)blind.position);
blind.speed=0; //stop
blind.mode_measure_speed_state=0; // reset
blind.mode=MODE_IDLE;
break;
}
blind.speed=100; //down
if (blind1.sense_status==SENSESTATUS_CLEAR) {
blind.timing_start=millis();
@ -521,32 +590,147 @@ void checkModes(blindmodel &blind) {
case 2: //on opaque section
blind.speed=100; //down
if (blind1.sense_status==SENSESTATUS_CLEAR) { //wait for clear section
blind.speedfactor=(blind.length_clear+blind.length_opaque)/ ((millis()-blind.timing_start)/1000.0); //calculate length per second
Serial.print("speedfactor=");
Serial.print(blind.speedfactor);
blind.speedfactorLow=(blind.length_clear+blind.length_opaque)/ ((millis()-blind.timing_start)/1000.0); //calculate length per second
blind.speedfactorHigh=blind.speedfactorLow; //use speedfactorLow for a first rough estimate (needed to know when position for speedfactorHigh measure reached)
Serial.print("speedfactorLow=");
Serial.print(blind.speedfactorLow);
Serial.println(" mm/s");
node.setProperty("cmd").send("speedfactorLow="+(String)blind.speedfactorLow);
blind.position = blind.length_opaque+blind.length_clear+blind.start_first_clear; //set position
blind.mode_measure_speed_state++;
}
break;
case 3: // past first measurement. drive to lower position for speedfactorHigh
if (blind.position>=POSITION_SPEEDMEASURE_HIGH) { //position reached
blind.mode_measure_speed_state++;
}
break;
case 4: //drive further down, start timing at clear
blind.speed=100; //down
if (blind1.sense_status==SENSESTATUS_CLEAR) {
blind.timing_start=millis();
blind.mode_measure_speed_state++;
}
break;
case 5: //on clear section
blind.speed=100; //down
if (blind1.sense_status==SENSESTATUS_OPAQUE) { //wait for opaque section
blind.mode_measure_speed_state++;
}
break;
case 6: //on opaque section
blind.speed=100; //down
if (blind1.sense_status==SENSESTATUS_CLEAR) { //wait for clear section
blind.speedfactorHigh=(blind.length_clear+blind.length_opaque)/ ((millis()-blind.timing_start)/1000.0); //calculate length per second
Serial.print("speedfactorHigh=");
Serial.print(blind.speedfactorHigh);
Serial.println(" mm/s");
node.setProperty("cmd").send("speedfactorHigh="+(String)blind.speedfactorHigh);
blind.speed=0; //stop
blind.mode_measure_speed_state=0; // reset
blind.mode=MODE_IDLE;
}
break;
break;
}
break;
case MODE_GO_TO_POS:
if (abs(blind.position-blind.set_position)<=THRESHOLD_GO_TO_POS) {
blind.speed=0; //stop
blind.mode=MODE_IDLE;
}else{
if (blind.set_position<blind.position) {
blind.speed=-100; //drive up
}else if (blind.set_position>blind.position) {
blind.speed=100; //drive down
}
}
break;
}
}
void setError(blindmodel &blind, uint8_t errorcode){
void setError(blindmodel &blind, uint8_t errorcode, HomieNode& node){
if (blind.error==0) { //only set error if no error appeared before
blind.error=errorcode;
}
Serial.print("ERROR CODE="); Serial.println(errorcode);
}
node.setProperty("debug").send("Error="+(String)errorcode);
blind.mode=MODE_ERROR;
}
String modeNumToString(uint8_t modenum){
switch(modenum){
case MODE_IDLE:
return "MODE_IDLE";
break;
case MODE_FIND_END:
return "MODE_FIND_END";
break;
case MODE_GO_TO_POS:
return "MODE_GO_TO_POS";
break;
case MODE_MEASURE_SPEED:
return "MODE_MEASURE_SPEED";
break;
case MODE_ERROR:
return "MODE_ERROR";
break;
}
}
bool blind_l_positionHandler(const HomieRange& range, const String& value) {
if (range.isRange) {
return false; //if range is given but index is not in allowed range
}
Homie.getLogger() << "blind_l_positionHandler" << ": " << value << endl;
//lightNode.setProperty("enable").send(value); //can be done in main loop
//if (value.toFloat() >= 0 && value.toFloat() <= 1.0) {
//something with value.toFloat()
blind1.set_position=constrain(value.toFloat(), blind1.softlimit_min, blind1.softlimit_max);
blind1.mode=MODE_GO_TO_POS;
/*}else{
Homie.getLogger() << "Value outside range" << endl;
return false;
}*/
return true;
}
bool blind_l_cmdHandler(const HomieRange& range, const String& value) {
if (range.isRange) {
return false; //if range is given but index is not in allowed range
}
Homie.getLogger() << "blind_l_cmdHandler" << ": " << value << endl;
if (value=="END")
{
blind1.mode = MODE_FIND_END;
blind1.mode_find_end_state=0; //reset mode find state
}else if (value=="SPEEDFACTOR")
{
blind1.mode=MODE_MEASURE_SPEED; //measure speed
}else{
Homie.getLogger() << "command not known" << endl;
return false;
}
blind1Node.setProperty("cmd").send(value); //can be done in main loop
return true;
}
bool blind_r_positionHandler(const HomieRange& range, const String& value) {
if (range.isRange) {
return false; //if range is given but index is not in allowed range
}
Homie.getLogger() << "blind_r_positionHandler" << ": " << value << endl;
blind2.set_position=constrain(value.toFloat(), blind2.softlimit_min, blind2.softlimit_max);
blind2.mode=MODE_GO_TO_POS;
return true;
}