2021-01-31 19:41:53 +00:00
# include <Arduino.h>
2021-01-31 12:08:48 +00:00
# include <Wire.h>
2021-02-07 22:34:54 +00:00
# include <Homie.h>
//Upload config: platformio run --target uploadfs
2021-01-31 12:08:48 +00:00
2021-01-31 19:41:53 +00:00
/*
TODO :
- correct speedfactor when moving at pwm = 100 for some time over opaque + clear OR implement speedfactor for up and lower state
2021-02-12 21:16:09 +00:00
- implement failure detection ( timeouts ) ,
- has to stop when no sensor change detected for some time
- in classifySensorValue ( ) only allow a certain time of unclassified reading
- has to stop when driving upwards ( or in general ) for too long ( for example end marker missing )
- do not allow certain commands if error mode
2021-02-13 18:12:52 +00:00
- manual mover function should work better with iot commands ( set position , no errors )
2021-01-31 19:41:53 +00:00
*/
2021-01-31 12:08:48 +00:00
2021-02-07 22:34:54 +00:00
2021-02-13 18:12:52 +00:00
//right blind kind off delayed to commands?? ////
2021-02-12 21:16:09 +00:00
2021-02-07 22:34:54 +00:00
# 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
2021-01-31 12:08:48 +00:00
# include "WEMOS_Motor.h"
//follow firmware flash guide for new wemos motor shield v1.0 https://github.com/thomasfredericks/wemos_motor_shield
2021-01-31 16:59:57 +00:00
2021-01-31 12:45:28 +00:00
# define BUTTON_DEBOUNCE 200
2021-01-31 16:59:57 +00:00
# define PIN_BUTTON1 D5
2021-02-02 19:54:35 +00:00
# define PIN_BUTTON2 D6
2021-01-31 16:59:57 +00:00
struct button {
uint8_t pin ;
unsigned long last_time_read = 0 ;
bool down = false ;
bool changed = false ;
2021-02-02 19:54:35 +00:00
bool manual_drive_direction = false ;
2021-01-31 16:59:57 +00:00
} ;
button button1 ;
2021-02-02 19:54:35 +00:00
button button2 ;
2021-01-31 16:59:57 +00:00
unsigned long last_sensor_read = 0 ;
2021-02-13 18:12:52 +00:00
# define SENSOR_READ_INTERVAL 50 //in ms
# define SENSE_FILTER_SIZE 10 //value will have a delay of td = SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL
2021-01-31 12:45:28 +00:00
2021-01-31 19:41:53 +00:00
# define CALCULATEPOSITIONESTIMATE_INTERVAL 100
2021-02-13 18:12:52 +00:00
2021-01-31 19:41:53 +00:00
2021-02-07 22:34:54 +00:00
# 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)
2021-02-02 19:54:35 +00:00
2021-01-31 12:45:28 +00:00
# define PIN_SENSE A0
unsigned long last_print = 0 ;
2021-02-02 20:43:02 +00:00
# define PIN_SENSE_LED1 D7
# define PIN_SENSE_LED2 D8
uint8_t sensorreadID = 0 ;
2021-02-13 18:12:52 +00:00
//#define SENSOR_SWITCH_INTERVAL 20 //should be lower or equal than SENSOR_READ_INTERVAL
//unsigned long last_sensor_led_switch=0;
2021-02-02 20:43:02 +00:00
2021-01-31 12:45:28 +00:00
//define directions for motors
2021-01-31 19:41:53 +00:00
# define _UP _CCW
# define _DOWN _CW
2021-01-31 16:59:57 +00:00
2021-01-31 19:41:53 +00:00
# define SENSESTATUS_CLEAR 1
# define SENSESTATUS_OPAQUE 2
# define SENSESTATUS_END 3
# define SENSESTATUS_UNKNOWN 0
2021-01-31 16:59:57 +00:00
2021-01-31 19:41:53 +00:00
# define MODE_IDLE 0
# define MODE_FIND_END 1
# define MODE_MEASURE_SPEED 2
# define MODE_GO_TO_POS 3
2021-02-07 22:34:54 +00:00
# define MODE_ERROR 4
2021-01-31 16:59:57 +00:00
//model parameters/variables
struct blindmodel
{
unsigned long lastreadtime = 0 ;
2021-01-31 19:41:53 +00:00
float position = 0 ; //0 is furthest open. positive is down (closing). unit is mm. estimated position
2021-01-31 16:59:57 +00:00
2021-02-02 20:59:15 +00:00
int8_t last_n = 0 ; //-1 means not defined/unknown
2021-01-31 16:59:57 +00:00
float length_clear ; //length of clear part in position units
float length_opaque ; //lengt of opaque part in position units
2021-01-31 19:41:53 +00:00
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
2021-01-31 16:59:57 +00:00
2021-02-07 22:34:54 +00:00
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
2021-01-31 16:59:57 +00:00
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
2021-01-31 12:45:28 +00:00
2021-01-31 16:59:57 +00:00
int sense_opaque_lower ; //adc value lower limit for opaque part. opaque is around 675
int sense_opaque_upper ; //adc value upper limit for opaque part
int sense_end_lower ; //adc value lower limit for end marker
int sense_end_upper ; //adc value upper limit for end marker
2021-01-31 19:41:53 +00:00
2021-01-31 16:59:57 +00:00
uint8_t sense_status = 0 ;
2021-01-31 19:41:53 +00:00
uint8_t last_sense_status = 0 ;
2021-01-31 16:59:57 +00:00
int sense_read [ SENSE_FILTER_SIZE ] = { 0 } ;
uint8_t sense_read_pos = 0 ; //position of last element written to
2021-02-02 20:43:02 +00:00
uint8_t pin_sensor_led ; //pin to activate sensor. high activates sensor
2021-01-31 16:59:57 +00:00
float set_position = 0 ;
2021-02-07 22:34:54 +00:00
float softlimit_min = 0 ;
float softlimit_max = 1000 ; //doubled value for folding bilds
2021-01-31 16:59:57 +00:00
unsigned long last_sense_ok = 0 ; //last time sensor measured class ok
//TODO: implement timeout if last_sense_ok gets too high.
2021-01-31 19:41:53 +00:00
uint8_t mode = MODE_IDLE ;
uint8_t mode_find_end_state = 0 ;
uint8_t mode_measure_speed_state = 0 ;
unsigned long timing_start ;
int speed = 0 ; //-100 to 100. commanded speed to motor. negative means upwards
int speedSimulated = 0 ; //software simulated motor speed up and slow down
float simulated_acc_dec = - 100 ; //pwm/sec^2, speed getting more negative (accelerating up). this value should be negative. better choose too small absolute values
float simulated_acc_inc = 100 ; //pwm/sec^2, speed getting more positive (accelerating down)
2021-02-13 18:12:52 +00:00
unsigned long last_calculate_position_estimate = 0 ;
2021-02-02 20:43:02 +00:00
uint8_t error = 0 ;
2021-01-31 16:59:57 +00:00
} ;
blindmodel blind1 ;
2021-02-02 19:54:35 +00:00
blindmodel blind2 ;
2021-01-31 12:45:28 +00:00
2021-01-31 12:08:48 +00:00
//Motor shield default I2C Address: 0x30
//PWM frequency: 1000Hz(1kHz)
Motor M1 ( 0x30 , _MOTOR_A , 1000 ) ; //Motor A
Motor M2 ( 0x30 , _MOTOR_B , 1000 ) ; //Motor B
2021-01-31 19:41:53 +00:00
unsigned long last_motor_send = 0 ;
# define MOTOR_UPDATE_INTERVAL 100
2021-01-31 12:08:48 +00:00
2021-02-13 18:12:52 +00:00
# define DIFF_ERROR_FACTOR 0.6 //between 0 and 1. 1=error when estimated position error is at maximum (between two possible encoder readings). 0=no deviation allowed
2021-02-02 20:59:15 +00:00
# define ERRORCODE_POSITIONDIFFTOOHIGH 1 //deviation too high on position correction
# define ERRORCODE_N_NOT_NEXT 2 //skipped one transition. position jumped too far?
2021-01-31 16:59:57 +00:00
2021-02-07 22:34:54 +00:00
# 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
2021-01-31 16:59:57 +00:00
int getFitered ( int * values , uint8_t size ) ;
2021-02-02 20:43:02 +00:00
void classifySensorValue ( blindmodel & blind ) ;
2021-02-02 19:54:35 +00:00
void checkButton ( button & btn ) ;
2021-02-07 22:34:54 +00:00
void checkModes ( blindmodel & blind , HomieNode & node ) ;
2021-02-02 19:54:35 +00:00
void manualMoveHandler ( button & btn , blindmodel & blind ) ;
2021-02-07 22:34:54 +00:00
void readSensor ( blindmodel & blind , int value , HomieNode & node ) ;
void estimatePosition ( blindmodel & blind , HomieNode & node ) ;
2021-02-02 19:54:35 +00:00
void errorCheck ( blindmodel & blind ) ;
void updateMotor ( blindmodel & blind , Motor motor ) ;
2021-02-07 22:34:54 +00:00
void setError ( blindmodel & blind , uint8_t errorcode , HomieNode & node ) ;
String modeNumToString ( uint8_t modenum ) ;
2021-02-13 18:12:52 +00:00
String sensestatusNumToString ( uint8_t sensestatusnum ) ;
2021-02-07 22:34:54 +00:00
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 ) ;
2021-02-13 18:12:52 +00:00
bool blind_cmdHandler ( blindmodel & blind , HomieNode & node , const String & value ) ; //referenced by blind_l and r _cmdHandler
2021-02-07 22:34:54 +00:00
void loopHandler ( ) ;
2021-01-31 16:59:57 +00:00
2021-01-31 12:08:48 +00:00
void setup ( ) {
2021-01-31 19:41:53 +00:00
Serial . begin ( 115200 ) ;
2021-01-31 16:59:57 +00:00
Serial . println ( " Starting " ) ;
button1 . pin = PIN_BUTTON1 ;
2021-02-02 19:54:35 +00:00
button2 . pin = PIN_BUTTON2 ;
2021-01-31 12:08:48 +00:00
2021-01-31 16:59:57 +00:00
pinMode ( button1 . pin , INPUT_PULLUP ) ;
2021-02-02 19:54:35 +00:00
pinMode ( button2 . pin , INPUT_PULLUP ) ;
2021-01-31 12:45:28 +00:00
pinMode ( PIN_SENSE , INPUT ) ;
2021-02-02 20:43:02 +00:00
pinMode ( PIN_SENSE_LED1 , OUTPUT ) ;
pinMode ( PIN_SENSE_LED2 , OUTPUT ) ;
digitalWrite ( PIN_SENSE_LED1 , LOW ) ;
digitalWrite ( PIN_SENSE_LED2 , LOW ) ;
2021-01-31 12:45:28 +00:00
M1 . setmotor ( _STOP ) ;
M2 . setmotor ( _STOP ) ;
2021-01-31 16:59:57 +00:00
//settings for blind
2021-02-02 20:43:02 +00:00
blind1 . pin_sensor_led = PIN_SENSE_LED1 ;
2021-01-31 16:59:57 +00:00
blind1 . length_clear = 50 ;
blind1 . length_opaque = 74 ;
blind1 . sense_clear_lower = 40 ;
2021-02-13 18:12:52 +00:00
blind1 . sense_clear_upper = 200 ;
blind1 . sense_opaque_lower = 400 ;
2021-02-12 20:31:31 +00:00
blind1 . sense_opaque_upper = 800 ;
2021-01-31 16:59:57 +00:00
blind1 . sense_end_lower = 850 ;
blind1 . sense_end_upper = 1024 ;
2021-02-13 18:12:52 +00:00
blind1 . speedfactorLow = 28.7 ;
blind1 . speedfactorHigh = 25.3 ;
2021-01-31 19:41:53 +00:00
blind1 . start_first_clear = 27 ;
blind1 . simulated_acc_dec = - 150 ;
blind1 . simulated_acc_inc = 200 ;
2021-02-07 22:34:54 +00:00
blind1 . softlimit_min = 0 ;
blind1 . softlimit_max = 2500 ;
2021-01-31 19:41:53 +00:00
2021-02-02 20:43:02 +00:00
blind2 . pin_sensor_led = PIN_SENSE_LED2 ;
2021-02-02 19:54:35 +00:00
blind2 . length_clear = 50 ;
blind2 . length_opaque = 74 ;
blind2 . sense_clear_lower = 40 ;
2021-02-13 18:12:52 +00:00
blind2 . sense_clear_upper = 200 ;
blind2 . sense_opaque_lower = 400 ;
2021-02-12 20:31:31 +00:00
blind2 . sense_opaque_upper = 800 ;
2021-02-02 19:54:35 +00:00
blind2 . sense_end_lower = 850 ;
blind2 . sense_end_upper = 1024 ;
2021-02-13 18:12:52 +00:00
blind2 . speedfactorLow = 27.6 ;
blind2 . speedfactorHigh = 23.5 ;
2021-02-02 19:54:35 +00:00
blind2 . start_first_clear = 27 ;
blind2 . simulated_acc_dec = - 150 ;
blind2 . simulated_acc_inc = 200 ;
2021-02-07 22:34:54 +00:00
blind2 . softlimit_min = 0 ;
blind2 . softlimit_max = 2500 ;
2021-02-02 19:54:35 +00:00
2021-01-31 19:41:53 +00:00
//Test
2021-02-12 20:31:31 +00:00
//blind1.mode = MODE_FIND_END;
//blind1.mode_find_end_state=0; //reset mode find state
2021-02-02 19:54:35 +00:00
2021-02-02 20:43:02 +00:00
//blind2.mode = MODE_FIND_END;
//blind2.mode_find_end_state=0; //reset mode find state
2021-02-07 22:34:54 +00:00
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 ) ;
2021-02-12 21:16:09 +00:00
blind2Node . advertise ( " position " ) . settable ( blind_r_positionHandler ) ; //function inputHandler gets called on new message on topic/input/set
blind2Node . advertise ( " debug " ) ;
blind2Node . advertise ( " estimationerror " ) ;
blind2Node . advertise ( " mode " ) ;
blind2Node . advertise ( " cmd " ) . settable ( blind_r_cmdHandler ) ;
2021-02-07 22:34:54 +00:00
Homie . setup ( ) ;
2021-02-02 20:43:02 +00:00
2021-01-31 12:08:48 +00:00
}
2021-01-31 12:45:28 +00:00
void loop ( ) {
2021-02-07 22:34:54 +00:00
Homie . loop ( ) ;
}
void loopHandler ( ) {
2021-01-31 12:45:28 +00:00
2021-02-02 19:54:35 +00:00
checkButton ( button1 ) ;
2021-02-02 19:55:41 +00:00
checkButton ( button2 ) ;
2021-02-02 19:54:35 +00:00
2021-01-31 12:45:28 +00:00
2021-01-31 16:59:57 +00:00
//Manual movement by button
2021-02-02 19:54:35 +00:00
manualMoveHandler ( button1 , blind1 ) ;
2021-02-02 19:55:41 +00:00
manualMoveHandler ( button2 , blind2 ) ;
2021-02-02 19:54:35 +00:00
//Read sensor/encoder
2021-02-13 18:12:52 +00:00
if ( millis ( ) > last_sensor_read + SENSOR_READ_INTERVAL / 2 ) {
2021-02-02 20:43:02 +00:00
int rawsensorvalue = analogRead ( PIN_SENSE ) ;
2021-02-13 18:12:52 +00:00
2021-02-02 20:43:02 +00:00
switch ( sensorreadID ) {
case 0 :
2021-02-13 18:12:52 +00:00
if ( millis ( ) > last_print + 500 & & blind1 . speedSimulated ! = 0 ) { //when readSensor would print its status
Serial . print ( " 1: " ) ;
}
2021-02-07 22:34:54 +00:00
readSensor ( blind1 , rawsensorvalue , blind1Node ) ;
2021-02-02 20:43:02 +00:00
digitalWrite ( blind1 . pin_sensor_led , LOW ) ; //turn self off
digitalWrite ( blind2 . pin_sensor_led , HIGH ) ; //turn next on
break ;
case 1 :
2021-02-13 18:12:52 +00:00
if ( millis ( ) > last_print + 500 & & blind2 . speedSimulated ! = 0 ) { //when readSensor would print its status
Serial . print ( " 2: " ) ;
}
2021-02-07 22:34:54 +00:00
readSensor ( blind2 , rawsensorvalue , blind2Node ) ;
2021-02-02 20:43:02 +00:00
digitalWrite ( blind2 . pin_sensor_led , LOW ) ; //turn self off
digitalWrite ( blind1 . pin_sensor_led , HIGH ) ; //turn next on
break ;
default :
2021-02-12 21:16:09 +00:00
sensorreadID = 0 ; //reset, failsafe
2021-02-02 20:43:02 +00:00
break ;
}
2021-02-13 18:12:52 +00:00
last_sensor_read = millis ( ) ;
2021-02-12 21:16:09 +00:00
sensorreadID + + ;
sensorreadID = sensorreadID % 2 ;
2021-02-02 20:43:02 +00:00
}
2021-02-02 19:54:35 +00:00
2021-02-07 22:34:54 +00:00
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 ( ) ;
}
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 ;
}
2021-02-02 19:54:35 +00:00
2021-02-12 21:16:09 +00:00
static float last_blind2_position = 0 ;
static unsigned long lastsend_blind2_position = 0 ; //time
if ( last_blind2_position ! = blind2 . position & & millis ( ) - lastsend_blind2_position > MINTIMEINTERVAL_POSITION ) {
blind2Node . setProperty ( " position " ) . send ( ( String ) blind2 . position ) ;
last_blind2_position = blind2 . position ;
lastsend_blind2_position = millis ( ) ;
}
static uint8_t last_blind2_mode = 100 ;
if ( last_blind2_mode ! = blind2 . mode )
{
blind2Node . setProperty ( " mode " ) . send ( modeNumToString ( blind2 . mode ) ) ;
last_blind2_mode = blind2 . mode ;
}
2021-02-07 22:34:54 +00:00
checkModes ( blind1 , blind1Node ) ;
checkModes ( blind2 , blind2Node ) ;
2021-02-02 19:54:35 +00:00
errorCheck ( blind1 ) ;
2021-02-02 19:55:41 +00:00
errorCheck ( blind2 ) ;
2021-02-02 19:54:35 +00:00
//Estimate blind position and correct
2021-02-07 22:34:54 +00:00
estimatePosition ( blind1 , blind1Node ) ;
estimatePosition ( blind2 , blind2Node ) ;
2021-02-02 19:54:35 +00:00
//Update Motor Driver
2021-02-13 18:12:52 +00:00
if ( millis ( ) > last_motor_send + MOTOR_UPDATE_INTERVAL ) {
updateMotor ( blind1 , M1 ) ;
updateMotor ( blind2 , M2 ) ;
last_motor_send = millis ( ) ;
}
2021-02-02 19:54:35 +00:00
}
int sort_desc ( const void * cmp1 , const void * cmp2 ) //compare function for qsort
{
// Need to cast the void * to int *
int a = * ( ( int * ) cmp1 ) ;
int b = * ( ( int * ) cmp2 ) ;
// The comparison
return a > b ? - 1 : ( a < b ? 1 : 0 ) ;
// A simpler, probably faster way:
//return b - a;
}
int getFitered ( int * values , uint8_t size ) {
int copied_values [ size ] ;
for ( int i = 0 ; i < size ; i + + ) {
copied_values [ i ] = values [ i ] ; //TODO: maybe some value filtering/selection here
}
int copied_values_length = sizeof ( copied_values ) / sizeof ( copied_values [ 0 ] ) ;
qsort ( copied_values , copied_values_length , sizeof ( copied_values [ 0 ] ) , sort_desc ) ;
return copied_values [ size / 2 ] ;
}
2021-02-02 20:43:02 +00:00
void classifySensorValue ( blindmodel & blind ) {
2021-02-02 19:54:35 +00:00
int filtered = getFitered ( blind . sense_read , SENSE_FILTER_SIZE ) ;
if ( filtered > = blind . sense_clear_lower & & filtered < = blind . sense_clear_upper ) {
blind . sense_status = SENSESTATUS_CLEAR ;
blind . last_sense_ok = millis ( ) ;
} else if ( filtered > = blind . sense_opaque_lower & & filtered < = blind . sense_opaque_upper ) {
blind . sense_status = SENSESTATUS_OPAQUE ;
blind . last_sense_ok = millis ( ) ;
} else if ( filtered > = blind . sense_end_lower & & filtered < = blind . sense_end_upper ) {
blind . sense_status = SENSESTATUS_END ;
blind . last_sense_ok = millis ( ) ;
2021-02-12 21:16:09 +00:00
} //if not in these boundaries, keep last class
2021-02-02 19:54:35 +00:00
}
void checkButton ( button & btn ) {
btn . changed = false ;
if ( millis ( ) > btn . last_time_read + BUTTON_DEBOUNCE ) {
bool new_pin_button_down = ! digitalRead ( btn . pin ) ;
if ( btn . down ! = new_pin_button_down ) { //changed
btn . down = new_pin_button_down ; //update
btn . changed = true ;
btn . last_time_read = millis ( ) ; //delay next check
}
}
}
void manualMoveHandler ( button & btn , blindmodel & blind )
{
if ( btn . changed ) {
blind . mode = MODE_IDLE ;
if ( btn . down ) { //changed to pressed
if ( btn . manual_drive_direction ) {
2021-01-31 19:41:53 +00:00
//M1.setmotor( _CW, 100);
2021-02-02 19:54:35 +00:00
blind . speed = - 100 ;
2021-01-31 19:41:53 +00:00
//Serial.print("CW PWM: ");
2021-01-31 12:45:28 +00:00
} else {
2021-02-02 19:54:35 +00:00
blind . speed = 100 ;
2021-01-31 19:41:53 +00:00
//Serial.print("CCW PWM: ");
2021-01-31 12:45:28 +00:00
}
2021-02-02 19:54:35 +00:00
btn . manual_drive_direction = ! btn . manual_drive_direction ; //switch direction every new press
2021-01-31 16:59:57 +00:00
} else { //changed to released
2021-01-31 19:41:53 +00:00
//Serial.println("Motor STOP");
2021-02-02 19:54:35 +00:00
blind . speed = 0 ;
2021-01-31 12:45:28 +00:00
}
}
2021-02-02 19:54:35 +00:00
}
2021-01-31 16:59:57 +00:00
2021-02-07 22:34:54 +00:00
void readSensor ( blindmodel & blind , int value , HomieNode & node )
2021-02-02 19:54:35 +00:00
{
2021-02-13 18:12:52 +00:00
blind . sense_read_pos = ( blind . sense_read_pos + 1 ) % SENSE_FILTER_SIZE ; //next element
blind . sense_read [ blind . sense_read_pos ] = value ;
2021-01-31 16:59:57 +00:00
2021-02-13 18:12:52 +00:00
classifySensorValue ( blind ) ; //writes to blindmodel.sense_status
2021-01-31 16:59:57 +00:00
2021-01-31 12:45:28 +00:00
2021-02-02 20:43:02 +00:00
if ( millis ( ) > last_print + 500 & & blind . speedSimulated ! = 0 ) {
2021-01-31 19:41:53 +00:00
Serial . print ( " SenseStatus= " ) ;
2021-02-13 18:12:52 +00:00
Serial . print ( sensestatusNumToString ( blind . sense_status ) ) ;
2021-02-02 19:54:35 +00:00
Serial . print ( " ( " ) ; Serial . print ( getFitered ( blind . sense_read , SENSE_FILTER_SIZE ) ) ; Serial . print ( " ) " ) ;
2021-01-31 19:41:53 +00:00
Serial . print ( " , mode= " ) ;
2021-02-07 22:34:54 +00:00
Serial . print ( modeNumToString ( blind . mode ) ) ;
2021-01-31 19:41:53 +00:00
Serial . print ( " , speed= " ) ;
2021-02-02 19:54:35 +00:00
Serial . print ( blind . speed ) ;
2021-01-31 19:41:53 +00:00
Serial . print ( " , speedSim= " ) ;
2021-02-02 19:54:35 +00:00
Serial . print ( blind . speedSimulated ) ;
2021-01-31 19:41:53 +00:00
Serial . print ( " , pos= " ) ;
2021-02-02 19:54:35 +00:00
Serial . println ( blind . position ) ;
2021-01-31 12:45:28 +00:00
last_print = millis ( ) ;
}
2021-02-02 19:54:35 +00:00
}
2021-01-31 19:41:53 +00:00
2021-02-02 19:54:35 +00:00
void errorCheck ( blindmodel & blind ) {
if ( blind . sense_status = = SENSESTATUS_END ) {
if ( blind . speed < 0 ) { //stop driving up
blind . speed = 0 ;
2021-01-31 19:41:53 +00:00
}
}
2021-02-02 20:43:02 +00:00
//TODO: led self test. turn off ,should be high value
2021-02-02 19:54:35 +00:00
}
2021-02-07 22:34:54 +00:00
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)
2021-02-13 18:12:52 +00:00
if ( millis ( ) > blind . last_calculate_position_estimate + CALCULATEPOSITIONESTIMATE_INTERVAL ) {
float _actualTime = ( millis ( ) - blind . last_calculate_position_estimate ) / 1000.0 ; //in seconds
2021-02-02 19:54:35 +00:00
blind . speedSimulated + = constrain ( blind . speed - blind . speedSimulated , blind . simulated_acc_dec * _actualTime , blind . simulated_acc_inc * _actualTime ) ;
2021-01-31 19:41:53 +00:00
2021-02-07 22:34:54 +00:00
blind . position + = blind . speedSimulated / 100.0 * positional_speedfactor * _actualTime ;
2021-02-13 18:12:52 +00:00
blind . last_calculate_position_estimate = millis ( ) ;
2021-01-31 19:41:53 +00:00
}
2021-02-07 22:34:54 +00:00
//if (blind.mode!= MODE_FIND_END) {
2021-02-02 19:54:35 +00:00
if ( blind . sense_status = = SENSESTATUS_END & & blind . last_sense_status ! = SENSESTATUS_END ) { //entered end marker
blind . position = 0 ;
2021-02-07 22:34:54 +00:00
float _filterdelay_correction = blind . speedSimulated / 100.0 * positional_speedfactor * SENSE_FILTER_SIZE / 2 * SENSOR_READ_INTERVAL / 1000.0 ;
2021-02-02 19:54:35 +00:00
blind . position + = _filterdelay_correction ; //correct for filter delay
Serial . print ( " Reached End marker. set Position= " ) ; Serial . println ( blind . position ) ;
2021-02-07 22:34:54 +00:00
node . setProperty ( " debug " ) . send ( " Reached End marker. set Position= " + ( String ) blind . position ) ;
2021-01-31 19:41:53 +00:00
}
2021-02-02 19:54:35 +00:00
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)
if ( blind . sense_status ! = blind . last_sense_status ) { //status changed
if ( blind . last_sense_status = = SENSESTATUS_CLEAR | | blind . last_sense_status = = SENSESTATUS_OPAQUE ) { //only changes from clear to opaque or opaque to clear
2021-01-31 19:41:53 +00:00
2021-02-02 19:54:35 +00:00
float _position_before = blind . position ; //only for text output debugging
2021-01-31 19:41:53 +00:00
2021-02-02 19:54:35 +00:00
if ( ( blind . speedSimulated > 0 & & blind . sense_status = = SENSESTATUS_OPAQUE ) | | ( blind . speedSimulated < 0 & & blind . sense_status = = SENSESTATUS_CLEAR ) ) { //moving down from and clear to opaque OR moving up and from opaque to clear
2021-01-31 19:41:53 +00:00
2021-02-02 19:54:35 +00:00
//estimated_position_difference = -blind.position + blind.start_first_clear+blind.length_clear + _n*(blind.length_clear+blind.length_opaque); //possible occurances. let estimated_position=0 , solve for n
2021-01-31 19:41:53 +00:00
2021-02-02 19:54:35 +00:00
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
2021-02-07 22:34:54 +00:00
float _filterdelay_correction = blind . speedSimulated / 100.0 * positional_speedfactor * SENSE_FILTER_SIZE / 2 * SENSOR_READ_INTERVAL / 1000.0 ;
2021-02-02 19:54:35 +00:00
blind . position + = _filterdelay_correction ; //correct for filter delay
2021-02-02 20:59:15 +00:00
Serial . print ( " nearest n= " ) ; Serial . println ( _n ) ;
2021-02-07 22:34:54 +00:00
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 ) ;
2021-02-02 20:59:15 +00:00
}
blind . last_n = _n ; //for error check
2021-01-31 19:41:53 +00:00
Serial . print ( " posCorrection= " ) ; Serial . print ( _filterdelay_correction ) ;
2021-02-07 22:34:54 +00:00
2021-01-31 19:41:53 +00:00
2021-02-07 22:34:54 +00:00
Serial . print ( " , DOWN&CLE->OPA or UP&OPA->CLE, before= " ) ;
2021-01-31 19:41:53 +00:00
2021-02-02 19:54:35 +00:00
} else if ( ( blind . speedSimulated > 0 & & blind . sense_status = = SENSESTATUS_CLEAR ) | | ( blind . speedSimulated < 0 & & blind . sense_status = = SENSESTATUS_OPAQUE ) ) { //the other transition
//estimated_position_difference = -blind.position + blind.start_first_clear + _n*(blind.length_clear+blind.length_opaque); //possible occurances. let estimated_position=0 , solve for n
2021-01-31 19:41:53 +00:00
2021-02-02 19:54:35 +00:00
int _n = round ( ( blind . position - blind . start_first_clear ) / ( blind . length_clear + blind . length_opaque ) ) ; //find closest n
2021-01-31 19:41:53 +00:00
Serial . print ( " n= " ) ;
Serial . println ( _n ) ;
2021-02-02 19:54:35 +00:00
blind . position = blind . start_first_clear + _n * ( blind . length_clear + blind . length_opaque ) ; //calculate position from _n
2021-02-07 22:34:54 +00:00
float _filterdelay_correction = blind . speedSimulated / 100.0 * positional_speedfactor * SENSE_FILTER_SIZE / 2 * SENSOR_READ_INTERVAL / 1000.0 ;
2021-02-02 19:54:35 +00:00
blind . position + = _filterdelay_correction ; //correct for filter delay
2021-01-31 19:41:53 +00:00
2021-02-02 20:43:02 +00:00
Serial . print ( " posCorrection= " ) ; Serial . print ( _filterdelay_correction ) ;
2021-02-07 22:34:54 +00:00
2021-01-31 19:41:53 +00:00
Serial . print ( " , UP&CLE->OPA or DOWN&OPA->CLE, before= " ) ;
}
Serial . print ( _position_before ) ;
Serial . print ( " , after= " ) ;
2021-02-02 19:54:35 +00:00
Serial . print ( blind . position ) ;
2021-01-31 19:41:53 +00:00
Serial . print ( " , diff= " ) ;
2021-02-02 19:54:35 +00:00
Serial . println ( blind . position - _position_before ) ;
2021-02-07 22:34:54 +00:00
//node.setProperty("debug").send("diff="+(String)(blind.position-_position_before));
2021-02-13 18:12:52 +00:00
node . setProperty ( " estimationerror " ) . send ( ( String ) ( blind . position - _position_before ) ) ; //positive value means estimation position was too low (estimated speed to low), when driving down (positive dir)
2021-02-02 20:43:02 +00:00
2021-02-13 18:12:52 +00:00
if ( blind . mode ! = MODE_FIND_END & & blind . mode ! = MODE_MEASURE_SPEED ) { //only check error if not in find_end mode or measure speed
2021-02-07 22:34:54 +00:00
if ( abs ( blind . position - _position_before ) > = ( blind . length_clear + blind . length_opaque ) / 2.0 * DIFF_ERROR_FACTOR ) {
setError ( blind , ERRORCODE_POSITIONDIFFTOOHIGH , node ) ;
}
2021-02-02 20:43:02 +00:00
}
2021-01-31 19:41:53 +00:00
}
2021-02-02 19:54:35 +00:00
blind . last_sense_status = blind . sense_status ; //update only if new one is opaque or clear
2021-01-31 19:41:53 +00:00
}
}
2021-02-07 22:34:54 +00:00
//}
2021-02-02 19:54:35 +00:00
}
2021-01-31 19:41:53 +00:00
2021-02-02 19:54:35 +00:00
void updateMotor ( blindmodel & blind , Motor motor )
{
2021-02-02 20:43:02 +00:00
if ( blind . error ! = 0 ) { //error appeared
blind . speed = 0 ; //set speed to 0
}
2021-02-13 18:12:52 +00:00
if ( blind . speed < 0 ) {
motor . setmotor ( _UP , abs ( blind . speed ) ) ;
} else if ( blind . speed > 0 ) {
motor . setmotor ( _DOWN , abs ( blind . speed ) ) ;
} else {
motor . setmotor ( _STOP ) ;
2021-01-31 19:41:53 +00:00
}
2021-02-13 18:12:52 +00:00
2021-01-31 12:45:28 +00:00
}
2021-02-07 22:34:54 +00:00
void checkModes ( blindmodel & blind , HomieNode & node ) {
2021-01-31 19:41:53 +00:00
switch ( blind . mode ) {
case MODE_FIND_END :
switch ( blind . mode_find_end_state ) {
case 0 : //drive up until end sensed
blind . speed = - 100 ; //up
2021-02-02 19:54:35 +00:00
if ( blind . sense_status = = SENSESTATUS_END ) {
2021-01-31 19:41:53 +00:00
blind . speed = 0 ; //stop. for safety
blind . mode_find_end_state + + ;
}
break ;
case 1 : //drive down slowly until passed end maker
2021-02-13 18:12:52 +00:00
blind . speed = 25 ; //down slow
2021-02-12 21:16:09 +00:00
if ( blind . sense_status ! = SENSESTATUS_END ) {
2021-01-31 19:41:53 +00:00
blind . speed = 0 ; //stop
blind . position = 0 ;
2021-02-07 22:34:54 +00:00
blind . mode = MODE_IDLE ;
//blind.mode=MODE_MEASURE_SPEED; //next measure speed
2021-01-31 19:41:53 +00:00
blind . mode_find_end_state = 0 ; //reset
2021-02-02 20:59:15 +00:00
blind . last_n = - 1 ; //unknown
2021-01-31 19:41:53 +00:00
}
break ;
}
break ;
case MODE_MEASURE_SPEED :
switch ( blind . mode_measure_speed_state ) {
case 0 : //drive down, start timing at clear
2021-02-07 22:34:54 +00:00
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 ;
}
2021-01-31 19:41:53 +00:00
blind . speed = 100 ; //down
2021-02-12 21:16:09 +00:00
if ( blind . sense_status = = SENSESTATUS_CLEAR ) {
2021-01-31 19:41:53 +00:00
blind . timing_start = millis ( ) ;
blind . mode_measure_speed_state + + ;
}
break ;
case 1 : //on clear section
blind . speed = 100 ; //down
2021-02-12 21:16:09 +00:00
if ( blind . sense_status = = SENSESTATUS_OPAQUE ) { //wait for opaque section
2021-01-31 19:41:53 +00:00
blind . mode_measure_speed_state + + ;
}
break ;
case 2 : //on opaque section
blind . speed = 100 ; //down
2021-02-12 21:16:09 +00:00
if ( blind . sense_status = = SENSESTATUS_CLEAR ) { //wait for clear section
2021-02-07 22:34:54 +00:00
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 ) ;
2021-01-31 19:41:53 +00:00
Serial . println ( " mm/s " ) ;
2021-02-07 22:34:54 +00:00
node . setProperty ( " cmd " ) . send ( " speedfactorLow= " + ( String ) blind . speedfactorLow ) ;
2021-01-31 19:41:53 +00:00
blind . position = blind . length_opaque + blind . length_clear + blind . start_first_clear ; //set position
2021-02-07 22:34:54 +00:00
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
2021-02-12 21:16:09 +00:00
if ( blind . sense_status = = SENSESTATUS_CLEAR ) {
2021-02-07 22:34:54 +00:00
blind . timing_start = millis ( ) ;
blind . mode_measure_speed_state + + ;
}
break ;
case 5 : //on clear section
blind . speed = 100 ; //down
2021-02-12 21:16:09 +00:00
if ( blind . sense_status = = SENSESTATUS_OPAQUE ) { //wait for opaque section
2021-02-07 22:34:54 +00:00
blind . mode_measure_speed_state + + ;
}
break ;
case 6 : //on opaque section
blind . speed = 100 ; //down
2021-02-12 21:16:09 +00:00
if ( blind . sense_status = = SENSESTATUS_CLEAR ) { //wait for clear section
2021-02-07 22:34:54 +00:00
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 ) ;
2021-01-31 19:41:53 +00:00
blind . speed = 0 ; //stop
blind . mode_measure_speed_state = 0 ; // reset
blind . mode = MODE_IDLE ;
}
2021-02-07 22:34:54 +00:00
break ;
2021-01-31 19:41:53 +00:00
}
break ;
case MODE_GO_TO_POS :
2021-02-07 22:34:54 +00:00
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
}
}
2021-01-31 19:41:53 +00:00
break ;
}
}
2021-02-02 20:43:02 +00:00
2021-02-07 22:34:54 +00:00
void setError ( blindmodel & blind , uint8_t errorcode , HomieNode & node ) {
2021-02-02 20:43:02 +00:00
if ( blind . error = = 0 ) { //only set error if no error appeared before
blind . error = errorcode ;
}
Serial . print ( " ERROR CODE= " ) ; Serial . println ( errorcode ) ;
2021-02-07 22:34:54 +00:00
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 ;
}
2021-02-13 18:12:52 +00:00
return " UNDEF " ;
}
String sensestatusNumToString ( uint8_t sensestatusnum ) {
switch ( sensestatusnum ) {
case SENSESTATUS_UNKNOWN :
return " UNK " ;
break ;
case SENSESTATUS_CLEAR :
return " CLE " ;
break ;
case SENSESTATUS_OPAQUE :
return " OPA " ;
break ;
case SENSESTATUS_END :
return " END " ;
break ;
}
2021-02-07 22:34:54 +00:00
}
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 ;
}
2021-02-13 18:12:52 +00:00
bool blind_r_positionHandler ( const HomieRange & range , const String & value ) {
2021-02-07 22:34:54 +00:00
if ( range . isRange ) {
return false ; //if range is given but index is not in allowed range
}
2021-02-13 18:12:52 +00:00
Homie . getLogger ( ) < < " blind_r_positionHandler " < < " : " < < value < < endl ;
2021-02-07 22:34:54 +00:00
2021-02-13 18:12:52 +00:00
blind2 . set_position = constrain ( value . toFloat ( ) , blind2 . softlimit_min , blind2 . softlimit_max ) ;
blind2 . mode = MODE_GO_TO_POS ;
2021-02-07 22:34:54 +00:00
return true ;
}
2021-02-13 18:12:52 +00:00
bool blind_l_cmdHandler ( const HomieRange & range , const String & value ) {
2021-02-07 22:34:54 +00:00
if ( range . isRange ) {
return false ; //if range is given but index is not in allowed range
}
2021-02-13 18:12:52 +00:00
Homie . getLogger ( ) < < " blind_l_cmdHandler " < < " : " < < value < < endl ;
return blind_cmdHandler ( blind1 , blind1Node , value ) ;
2021-02-07 22:34:54 +00:00
}
2021-02-13 18:12:52 +00:00
2021-02-12 21:16:09 +00:00
bool blind_r_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_r_cmdHandler " < < " : " < < value < < endl ;
2021-02-13 18:12:52 +00:00
return blind_cmdHandler ( blind2 , blind2Node , value ) ;
/*
2021-02-12 21:16:09 +00:00
if ( value = = " END " )
{
blind2 . mode = MODE_FIND_END ;
blind2 . mode_find_end_state = 0 ; //reset mode find state
} else if ( value = = " SPEEDFACTOR " )
{
blind2 . mode = MODE_MEASURE_SPEED ; //measure speed
2021-02-13 18:12:52 +00:00
blind1Node . setProperty ( " cmd " ) . send ( " cmd measure speed " ) ;
2021-02-12 21:16:09 +00:00
} else {
2021-02-13 18:12:52 +00:00
Homie . getLogger ( ) < < " unknown command " < < endl ;
2021-02-12 21:16:09 +00:00
return false ;
}
2021-02-13 18:12:52 +00:00
return true ; */
}
bool blind_cmdHandler ( blindmodel & blind , HomieNode & node , const String & value ) {
if ( value = = " END " )
{
blind . mode = MODE_FIND_END ;
blind . mode_find_end_state = 0 ; //reset mode find state
node . setProperty ( " cmd " ) . send ( " cmd end " ) ;
} else if ( value = = " SPEEDFACTOR " )
{
blind . mode = MODE_MEASURE_SPEED ; //measure speed
node . setProperty ( " cmd " ) . send ( " cmd measure speed " ) ;
} else if ( value = = " SENSOR " ) //return adc values
{
Serial . print ( " ( " ) ; Serial . print ( getFitered ( blind . sense_read , SENSE_FILTER_SIZE ) ) ; Serial . print ( " ) " ) ;
Serial . print ( " , mode= " ) ;
Serial . print ( modeNumToString ( blind . mode ) ) ;
node . setProperty ( " cmd " ) . send ( " " + String ( getFitered ( blind . sense_read , SENSE_FILTER_SIZE ) ) + " " + sensestatusNumToString ( blind . sense_status ) ) ;
} else if ( value = = " STOP " ) //stop. go back to idle mode and stop driving
{
blind . speed = 0 ; //stop
blind . mode = MODE_IDLE ;
blind . set_position = blind . position ; //use current position as set pos to stay there
node . setProperty ( " cmd " ) . send ( " stop " ) ;
} else if ( value = = " RESET " ) //reset from error
{
blind . error = 0 ;
node . setProperty ( " cmd " ) . send ( " reset " ) ;
} else {
Homie . getLogger ( ) < < " unknown command " < < endl ;
return false ;
}
2021-02-12 21:16:09 +00:00
return true ;
}
2021-02-13 18:12:52 +00:00