886 lines
31 KiB
C++
886 lines
31 KiB
C++
#include <Arduino.h>
|
|
#include <Wire.h>
|
|
#include <Homie.h>
|
|
|
|
//Upload config: platformio run --target uploadfs
|
|
|
|
/*
|
|
TODO:
|
|
- implement failure detection (timeouts),
|
|
- has to stop when driving upwards (or in general) for too long (for example end marker missing)
|
|
- manual mover function should work better with iot commands (set position, no errors). implemented but untested
|
|
- implement find end mode when only using manual move buttons
|
|
*/
|
|
|
|
|
|
#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
|
|
|
|
|
|
#define BUTTON_DEBOUNCE 200
|
|
#define PIN_BUTTON1 D5
|
|
#define PIN_BUTTON2 D6
|
|
struct button{
|
|
uint8_t pin;
|
|
unsigned long last_time_read=0;
|
|
bool down=false;
|
|
bool changed=false;
|
|
bool manual_drive_direction=false;
|
|
};
|
|
button button1;
|
|
button button2;
|
|
|
|
unsigned long last_sensor_read=0;
|
|
#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
|
|
|
|
#define CALCULATEPOSITIONESTIMATE_INTERVAL 100
|
|
|
|
|
|
#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 MAX_ALLOWED_CLASSCHANGE_LENGTH 130 //[mm] . for error check. after with length traveled without classification change error will be triggered. should be at least max(length_opaque,length_clear)
|
|
|
|
#define PIN_SENSE A0
|
|
unsigned long last_print=0;
|
|
|
|
#define PIN_SENSE_LED1 D7
|
|
#define PIN_SENSE_LED2 D8
|
|
uint8_t sensorreadID=0;
|
|
|
|
//#define SENSOR_SWITCH_INTERVAL 20 //should be lower or equal than SENSOR_READ_INTERVAL
|
|
//unsigned long last_sensor_led_switch=0;
|
|
|
|
//define directions for motors
|
|
#define _UP _CCW
|
|
#define _DOWN _CW
|
|
|
|
#define SENSESTATUS_CLEAR 1
|
|
#define SENSESTATUS_OPAQUE 2
|
|
#define SENSESTATUS_END 3
|
|
#define SENSESTATUS_UNKNOWN 0
|
|
|
|
#define MODE_IDLE 0
|
|
#define MODE_FIND_END 1
|
|
#define MODE_MEASURE_SPEED 2
|
|
#define MODE_GO_TO_POS 3
|
|
#define MODE_MANUAL 4
|
|
#define MODE_ERROR 5
|
|
|
|
|
|
//model parameters/variables
|
|
struct blindmodel
|
|
{
|
|
unsigned long lastreadtime=0;
|
|
float position=0; //0 is furthest open. positive is down (closing). unit is mm. estimated position
|
|
|
|
int8_t last_n=0; //-1 means not defined/unknown
|
|
|
|
float length_clear; //length of clear part in position units
|
|
float length_opaque; //lengt of opaque part in position units
|
|
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 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
|
|
|
|
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
|
|
|
|
|
|
uint8_t sense_status=0;
|
|
uint8_t last_sense_status=0;
|
|
int sense_read[SENSE_FILTER_SIZE] = {0};
|
|
uint8_t sense_read_pos=0; //position of last element written to
|
|
|
|
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
|
|
|
|
float position_last_classchange=0; //last position sensor measured class was ok
|
|
|
|
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)
|
|
|
|
unsigned long last_calculate_position_estimate=0;
|
|
|
|
uint8_t error=0;
|
|
};
|
|
|
|
blindmodel blind1;
|
|
blindmodel blind2;
|
|
|
|
|
|
//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
|
|
|
|
unsigned long last_motor_send=0;
|
|
#define MOTOR_UPDATE_INTERVAL 100
|
|
|
|
#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
|
|
#define ERRORCODE_POSITIONDIFFTOOHIGH 1 //deviation too high on position correction
|
|
#define ERRORCODE_N_NOT_NEXT 2 //skipped one transition. position jumped too far?
|
|
#define ERRORCODE_UNDEFINED_POSITION 3
|
|
#define ERRORCODE_CLASSIFY_LENGTH 4
|
|
|
|
#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, HomieNode &node);
|
|
void manualMoveHandler(button &btn, blindmodel &blind);
|
|
void readSensor(blindmodel &blind, int value, HomieNode &node);
|
|
void estimatePosition(blindmodel &blind, HomieNode& node);
|
|
void errorCheck(blindmodel &blind, HomieNode &node);
|
|
void updateMotor(blindmodel &blind, Motor motor);
|
|
void setError(blindmodel &blind, uint8_t errorcode, HomieNode& node);
|
|
String modeNumToString(uint8_t modenum);
|
|
String sensestatusNumToString(uint8_t sensestatusnum);
|
|
String errorcodeNumToString(uint8_t errorcode);
|
|
|
|
|
|
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);
|
|
bool blind_cmdHandler(blindmodel &blind, HomieNode& node, const String& value); //referenced by blind_l and r _cmdHandler
|
|
|
|
void loopHandler();
|
|
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
Serial.println("Starting");
|
|
|
|
button1.pin=PIN_BUTTON1;
|
|
button2.pin=PIN_BUTTON2;
|
|
|
|
pinMode(button1.pin, INPUT_PULLUP);
|
|
pinMode(button2.pin, INPUT_PULLUP);
|
|
pinMode(PIN_SENSE, INPUT);
|
|
|
|
pinMode(PIN_SENSE_LED1,OUTPUT);
|
|
pinMode(PIN_SENSE_LED2,OUTPUT);
|
|
digitalWrite(PIN_SENSE_LED1, LOW);
|
|
digitalWrite(PIN_SENSE_LED2, LOW);
|
|
|
|
M1.setmotor(_STOP);
|
|
M2.setmotor(_STOP);
|
|
|
|
|
|
//settings for blind
|
|
blind1.pin_sensor_led=PIN_SENSE_LED1;
|
|
blind1.length_clear=50;
|
|
blind1.length_opaque=74;
|
|
blind1.sense_clear_lower=40;
|
|
blind1.sense_clear_upper=200;
|
|
blind1.sense_opaque_lower=400;
|
|
blind1.sense_opaque_upper=800;
|
|
blind1.sense_end_lower=850;
|
|
blind1.sense_end_upper=1024;
|
|
blind1.speedfactorLow=28.7;
|
|
blind1.speedfactorHigh=25.3;
|
|
blind1.start_first_clear=27;
|
|
blind1.simulated_acc_dec=-150;
|
|
blind1.simulated_acc_inc=200;
|
|
blind1.softlimit_min=0;
|
|
blind1.softlimit_max=2500;
|
|
setError(blind1,ERRORCODE_UNDEFINED_POSITION,blind1Node);
|
|
|
|
blind2.pin_sensor_led=PIN_SENSE_LED2;
|
|
blind2.length_clear=50;
|
|
blind2.length_opaque=74;
|
|
blind2.sense_clear_lower=40;
|
|
blind2.sense_clear_upper=200;
|
|
blind2.sense_opaque_lower=400;
|
|
blind2.sense_opaque_upper=800;
|
|
blind2.sense_end_lower=850;
|
|
blind2.sense_end_upper=1024;
|
|
blind2.speedfactorLow=27.6;
|
|
blind2.speedfactorHigh=23.5;
|
|
blind2.start_first_clear=27;
|
|
blind2.simulated_acc_dec=-150;
|
|
blind2.simulated_acc_inc=200;
|
|
blind2.softlimit_min=0;
|
|
blind2.softlimit_max=2500;
|
|
setError(blind2,ERRORCODE_UNDEFINED_POSITION,blind2Node);
|
|
|
|
//Test
|
|
//blind1.mode = MODE_FIND_END;
|
|
//blind1.mode_find_end_state=0; //reset mode find state
|
|
|
|
//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);
|
|
|
|
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);
|
|
|
|
|
|
Homie.setup();
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
void loop() {
|
|
Homie.loop();
|
|
}
|
|
void loopHandler() {
|
|
|
|
checkButton(button1);
|
|
checkButton(button2);
|
|
|
|
|
|
//Manual movement by button
|
|
manualMoveHandler(button1, blind1);
|
|
manualMoveHandler(button2, blind2);
|
|
|
|
|
|
|
|
//Read sensor/encoder
|
|
if (millis() > last_sensor_read + SENSOR_READ_INTERVAL/2) {
|
|
int rawsensorvalue=analogRead(PIN_SENSE);
|
|
|
|
switch (sensorreadID) {
|
|
case 0:
|
|
if (millis() > last_print + 500 && blind1.speedSimulated!=0) { //when readSensor would print its status
|
|
Serial.print("1: ");
|
|
}
|
|
readSensor(blind1,rawsensorvalue, blind1Node);
|
|
digitalWrite(blind1.pin_sensor_led,LOW); //turn self off
|
|
digitalWrite(blind2.pin_sensor_led,HIGH); //turn next on
|
|
break;
|
|
case 1:
|
|
if (millis() > last_print + 500 && blind2.speedSimulated!=0) { //when readSensor would print its status
|
|
Serial.print("2: ");
|
|
}
|
|
readSensor(blind2,rawsensorvalue, blind2Node);
|
|
digitalWrite(blind2.pin_sensor_led,LOW); //turn self off
|
|
digitalWrite(blind1.pin_sensor_led,HIGH); //turn next on
|
|
break;
|
|
default:
|
|
sensorreadID=0; //reset, failsafe
|
|
break;
|
|
}
|
|
last_sensor_read=millis();
|
|
sensorreadID++;
|
|
sensorreadID=sensorreadID%2;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
checkModes(blind1, blind1Node);
|
|
checkModes(blind2, blind2Node);
|
|
|
|
//Estimate blind position and correct
|
|
estimatePosition(blind1, blind1Node);
|
|
estimatePosition(blind2, blind2Node);
|
|
|
|
errorCheck(blind1, blind1Node);
|
|
errorCheck(blind2, blind2Node);
|
|
|
|
//Update Motor Driver
|
|
if (millis() > last_motor_send + MOTOR_UPDATE_INTERVAL) {
|
|
updateMotor(blind1, M1);
|
|
updateMotor(blind2, M2);
|
|
last_motor_send=millis();
|
|
}
|
|
}
|
|
|
|
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];
|
|
}
|
|
|
|
void classifySensorValue(blindmodel &blind) {
|
|
int filtered=getFitered(blind.sense_read, SENSE_FILTER_SIZE);
|
|
if (filtered>=blind.sense_clear_lower && filtered<=blind.sense_clear_upper) {
|
|
if (blind.sense_status==SENSESTATUS_OPAQUE) { //change from opaque to clear
|
|
blind.position_last_classchange=blind.position;
|
|
Serial.print("classchange to clear. last="); Serial.print(blind.position_last_classchange); Serial.print(", pos="); Serial.println(blind.position);
|
|
}
|
|
blind.sense_status=SENSESTATUS_CLEAR;
|
|
} else if (filtered>=blind.sense_opaque_lower && filtered<=blind.sense_opaque_upper) {
|
|
if (blind.sense_status==SENSESTATUS_CLEAR) { //change from clear to opaque
|
|
blind.position_last_classchange=blind.position;
|
|
Serial.print("classchange to opaque. last="); Serial.print(blind.position_last_classchange); Serial.print(", pos="); Serial.println(blind.position);
|
|
}
|
|
blind.sense_status=SENSESTATUS_OPAQUE;
|
|
} else if (filtered>=blind.sense_end_lower && filtered<=blind.sense_end_upper) {
|
|
blind.sense_status=SENSESTATUS_END;
|
|
} //if not in these boundaries, keep last class
|
|
|
|
}
|
|
|
|
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) {
|
|
if (btn.down) { //changed to pressed
|
|
blind.mode=MODE_MANUAL;
|
|
if (btn.manual_drive_direction) { //drive up
|
|
//M1.setmotor( _CW, 100);
|
|
blind.speed=-100;
|
|
//Serial.print("CW PWM: ");
|
|
}else{ //drive down
|
|
blind.speed=100;
|
|
//Serial.print("CCW PWM: ");
|
|
}
|
|
btn.manual_drive_direction=!btn.manual_drive_direction; //switch direction every new press
|
|
}else{ //changed to released
|
|
//Serial.println("Motor STOP");
|
|
blind.mode=MODE_IDLE;
|
|
blind.speed=0;
|
|
}
|
|
}
|
|
}
|
|
|
|
void readSensor(blindmodel &blind, int value, HomieNode &node)
|
|
{
|
|
blind.sense_read_pos=(blind.sense_read_pos+1)%SENSE_FILTER_SIZE; //next element
|
|
blind.sense_read[blind.sense_read_pos]=value;
|
|
|
|
classifySensorValue(blind); //writes to blindmodel.sense_status
|
|
|
|
|
|
if (millis() > last_print + 500 && blind.speedSimulated!=0) {
|
|
Serial.print("SenseStatus=");
|
|
Serial.print(sensestatusNumToString(blind.sense_status));
|
|
|
|
Serial.print("("); Serial.print(getFitered(blind.sense_read, SENSE_FILTER_SIZE)); Serial.print(")");
|
|
Serial.print(", mode=");
|
|
Serial.print(modeNumToString(blind.mode));
|
|
|
|
Serial.print(", speed=");
|
|
Serial.print(blind.speed);
|
|
Serial.print(", speedSim=");
|
|
Serial.print(blind.speedSimulated);
|
|
Serial.print(", pos=");
|
|
Serial.println(blind.position);
|
|
last_print=millis();
|
|
}
|
|
}
|
|
|
|
void errorCheck(blindmodel &blind, HomieNode &node) {
|
|
if (blind.sense_status==SENSESTATUS_END) {
|
|
if (blind.speed<0) { //stop driving up
|
|
blind.speed=0;
|
|
}
|
|
}
|
|
if (blind.position>=blind.softlimit_max) { //reached bottom
|
|
if (blind.speed>0) { //stop driving down
|
|
blind.speed=0;
|
|
}
|
|
}
|
|
|
|
if (abs(blind.position_last_classchange-blind.position) > MAX_ALLOWED_CLASSCHANGE_LENGTH ) { //sensor reading havent been classified for too far
|
|
if (blind.error==0) { //only first time
|
|
Serial.print("classchange error: last_classchange="); Serial.print(blind.position_last_classchange); Serial.print(", pos="); Serial.println(blind.position);
|
|
}
|
|
setError(blind, ERRORCODE_CLASSIFY_LENGTH, node);
|
|
}
|
|
|
|
//TODO: led self test. turn off ,should be high value
|
|
}
|
|
|
|
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() > blind.last_calculate_position_estimate + CALCULATEPOSITIONESTIMATE_INTERVAL) {
|
|
float _actualTime=(millis()-blind.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*positional_speedfactor * _actualTime;
|
|
blind.last_calculate_position_estimate=millis();
|
|
}
|
|
|
|
//if (blind.mode!= MODE_FIND_END) {
|
|
if (blind.sense_status == SENSESTATUS_END && blind.last_sense_status != SENSESTATUS_END) { //entered end marker
|
|
blind.position=0;
|
|
blind.position_last_classchange=blind.position; //also set lastchange position
|
|
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)
|
|
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
|
|
|
|
float _position_before=blind.position;//only for text output debugging
|
|
|
|
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
|
|
|
|
//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
|
|
|
|
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*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) && 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=");
|
|
|
|
|
|
}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
|
|
|
|
int _n = round(( blind.position - blind.start_first_clear)/(blind.length_clear+blind.length_opaque)); //find closest n
|
|
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*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=");
|
|
|
|
}
|
|
|
|
Serial.print(_position_before);
|
|
Serial.print(", after=");
|
|
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)); //positive value means estimation position was too low (estimated speed to low), when driving down (positive dir)
|
|
|
|
if (blind.mode != MODE_FIND_END && blind.mode != MODE_MEASURE_SPEED ) { //only check error if not in find_end mode or measure speed
|
|
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)
|
|
{
|
|
if (blind.error!=0) { //error appeared
|
|
blind.speed=0; //set speed to 0
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
}
|
|
|
|
void checkModes(blindmodel &blind, HomieNode &node) {
|
|
if (blind.error==0) //only if no errors
|
|
{
|
|
switch(blind.mode) {
|
|
case MODE_FIND_END:
|
|
switch(blind.mode_find_end_state) {
|
|
case 0: //drive up until end sensed
|
|
blind.speed=-100; //up
|
|
if (blind.sense_status==SENSESTATUS_END) {
|
|
blind.speed=0; //stop. for safety
|
|
blind.mode_find_end_state++;
|
|
}
|
|
break;
|
|
|
|
case 1: //drive down slowly until passed end maker
|
|
blind.speed=25; //down slow
|
|
if (blind.sense_status!=SENSESTATUS_END) {
|
|
blind.speed=0; //stop
|
|
blind.position=0;
|
|
blind.position_last_classchange=blind.position; //also set lastchange position
|
|
blind.mode=MODE_IDLE;
|
|
//blind.mode=MODE_MEASURE_SPEED; //next measure speed
|
|
blind.mode_find_end_state=0; //reset
|
|
blind.last_n=-1; //unknown
|
|
}
|
|
break;
|
|
}
|
|
|
|
break;
|
|
|
|
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 (blind.sense_status==SENSESTATUS_CLEAR) {
|
|
blind.timing_start=millis();
|
|
blind.mode_measure_speed_state++;
|
|
}
|
|
break;
|
|
case 1: //on clear section
|
|
blind.speed=100; //down
|
|
if (blind.sense_status==SENSESTATUS_OPAQUE) { //wait for opaque section
|
|
blind.mode_measure_speed_state++;
|
|
}
|
|
break;
|
|
case 2: //on opaque section
|
|
blind.speed=100; //down
|
|
if (blind.sense_status==SENSESTATUS_CLEAR) { //wait for clear section
|
|
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 (blind.sense_status==SENSESTATUS_CLEAR) {
|
|
blind.timing_start=millis();
|
|
blind.mode_measure_speed_state++;
|
|
}
|
|
break;
|
|
case 5: //on clear section
|
|
blind.speed=100; //down
|
|
if (blind.sense_status==SENSESTATUS_OPAQUE) { //wait for opaque section
|
|
blind.mode_measure_speed_state++;
|
|
}
|
|
break;
|
|
case 6: //on opaque section
|
|
blind.speed=100; //down
|
|
if (blind.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;
|
|
|
|
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;
|
|
|
|
case MODE_MANUAL:
|
|
blind.set_position=blind.position; //use current position as set position
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
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="+errorcodeNumToString(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;
|
|
}
|
|
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;
|
|
}
|
|
return "UNDEF";
|
|
}
|
|
|
|
String errorcodeNumToString(uint8_t errorcode) {
|
|
switch(errorcode){
|
|
case ERRORCODE_UNDEFINED_POSITION:
|
|
return "ERROR_UNDEFINED_POSITION";
|
|
break;
|
|
case ERRORCODE_POSITIONDIFFTOOHIGH:
|
|
return "ERROR_POSITIONDIFFTOOHIGH";
|
|
break;
|
|
case ERRORCODE_N_NOT_NEXT:
|
|
return "ERROR_N_NOT_NEXT";
|
|
break;
|
|
case ERRORCODE_CLASSIFY_LENGTH:
|
|
return "ERROR_CLASSIFY_LENGTH";
|
|
break;
|
|
}
|
|
return "no error";
|
|
}
|
|
|
|
|
|
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_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;
|
|
}
|
|
|
|
|
|
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;
|
|
|
|
return blind_cmdHandler(blind1,blind1Node, value);
|
|
|
|
}
|
|
|
|
|
|
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;
|
|
|
|
return blind_cmdHandler(blind2,blind2Node, value);
|
|
/*
|
|
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
|
|
blind1Node.setProperty("cmd").send("cmd measure speed");
|
|
}else{
|
|
Homie.getLogger() << "unknown command" << endl;
|
|
return false;
|
|
}
|
|
|
|
|
|
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
|
|
blind.position_last_classchange=blind.position; //otherwise error trips immediately
|
|
if (blind.error==ERRORCODE_UNDEFINED_POSITION) {
|
|
blind.error=0; //reset
|
|
}
|
|
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.position_last_classchange=blind.position;
|
|
blind.error=0;
|
|
node.setProperty("cmd").send("reset");
|
|
}else{
|
|
Homie.getLogger() << "unknown command" << endl;
|
|
return false;
|
|
}
|
|
|
|
|
|
return true;
|
|
}
|
|
|