@ -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 ;
}