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

implement homie

master
interfisch 10 months ago
parent
7bb6d58c60
commit
1a0c9a5b39
2 changed files with 243 additions and 58 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 5
      platformio.ini
  2. 296
      src/main.cpp

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
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);
checkModes(blind2);
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;
}

Write Preview
Loading…
Cancel
Save