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

implement last_n error check

This commit is contained in:
interfisch 2021-02-02 21:59:15 +01:00
parent a00c2f0049
commit 7bb6d58c60
1 changed files with 12 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/main.cpp
View File

@ -4,7 +4,6 @@
/* /*
TODO: TODO:
- correct speedfactor when moving at pwm=100 for some time over opaque+clear OR implement speedfactor for up and lower state - correct speedfactor when moving at pwm=100 for some time over opaque+clear OR implement speedfactor for up and lower state
- implement second blind
- implement drive to position - implement drive to position
- implement failure detection - implement failure detection
- implement homie - implement homie
@ -66,6 +65,8 @@ struct blindmodel
unsigned long lastreadtime=0; unsigned long lastreadtime=0;
float position=0; //0 is furthest open. positive is down (closing). unit is mm. estimated position 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_clear; //length of clear part in position units
float length_opaque; //lengt of opaque 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 float start_first_clear; //distance from end marker to beginning of first clear section
@ -121,8 +122,9 @@ Motor M2(0x30, _MOTOR_B, 1000); //Motor B
unsigned long last_motor_send=0; unsigned long last_motor_send=0;
#define MOTOR_UPDATE_INTERVAL 100 #define MOTOR_UPDATE_INTERVAL 100
#define DIFF_ERROR_FACTOR 0.35 //between 0 and 1. 1=error when estimated position error is at maximum (between two possible encoder readings). 0=no deviation allowed #define DIFF_ERROR_FACTOR 0.4 //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 #define ERRORCODE_POSITIONDIFFTOOHIGH 1 //deviation too high on position correction
#define ERRORCODE_N_NOT_NEXT 2 //skipped one transition. position jumped too far?
int getFitered(int* values,uint8_t size); int getFitered(int* values,uint8_t size);
void classifySensorValue(blindmodel &blind); void classifySensorValue(blindmodel &blind);
@ -203,7 +205,6 @@ void loop() {
checkButton(button2); checkButton(button2);
//Manual movement by button //Manual movement by button
manualMoveHandler(button1, blind1); manualMoveHandler(button1, blind1);
manualMoveHandler(button2, blind2); manualMoveHandler(button2, blind2);
@ -413,8 +414,12 @@ void estimatePosition(blindmodel &blind) {
blind.position = blind.start_first_clear+blind.length_clear + _n*(blind.length_clear+blind.length_opaque); //calculate position from _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*blind.speedfactor *SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL/1000.0;
blind.position += _filterdelay_correction; //correct for filter delay blind.position += _filterdelay_correction; //correct for filter delay
Serial.print("n="); Serial.print("nearest n="); Serial.println(_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);
}
blind.last_n=_n; //for error check
Serial.print("posCorrection="); Serial.print(_filterdelay_correction); Serial.print("posCorrection="); Serial.print(_filterdelay_correction);
@ -491,7 +496,7 @@ void checkModes(blindmodel &blind) {
//blind.mode=MODE_IDLE; //blind.mode=MODE_IDLE;
blind.mode=MODE_MEASURE_SPEED; //next measure speed blind.mode=MODE_MEASURE_SPEED; //next measure speed
blind.mode_find_end_state=0; //reset blind.mode_find_end_state=0; //reset
blind.last_n=-1; //unknown
} }
break; break;
} }