fix errors during code duplication
This commit is contained in:
parent
34c7134239
commit
e3c918edb8
210
src/main.cpp
210
src/main.cpp
|
|
@ -11,14 +11,12 @@
|
|||
- 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)
|
||||
- homie commands (reset error, etc)
|
||||
- command to activate sensro raw value print over topics
|
||||
- do not allow certain commands if error mode
|
||||
- manual mover function should work better with iot commands (set position, no errors)
|
||||
*/
|
||||
|
||||
|
||||
//BIG TODO: fix sensor reading right blind. left blind too
|
||||
//right blind kind off delayed to commands??
|
||||
//right blind kind off delayed to commands?? ////
|
||||
|
||||
#define FW_NAME "blindctrl"
|
||||
#define FW_VERSION "1.0.0"
|
||||
|
|
@ -47,11 +45,12 @@ button button1;
|
|||
button button2;
|
||||
|
||||
unsigned long last_sensor_read=0;
|
||||
#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 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
|
||||
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)
|
||||
|
||||
|
|
@ -62,8 +61,9 @@ unsigned long last_print=0;
|
|||
#define PIN_SENSE_LED1 D7
|
||||
#define PIN_SENSE_LED2 D8
|
||||
uint8_t sensorreadID=0;
|
||||
unsigned long last_sensor_led_switch=0;
|
||||
#define SENSOR_SWITCH_INTERVAL 20 //should be lower or equal than SENSOR_READ_INTERVAL
|
||||
|
||||
//#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
|
||||
|
|
@ -131,6 +131,8 @@ struct blindmodel
|
|||
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;
|
||||
};
|
||||
|
||||
|
|
@ -146,7 +148,7 @@ Motor M2(0x30, _MOTOR_B, 1000); //Motor B
|
|||
unsigned long last_motor_send=0;
|
||||
#define MOTOR_UPDATE_INTERVAL 100
|
||||
|
||||
#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 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?
|
||||
|
||||
|
|
@ -165,12 +167,14 @@ void errorCheck(blindmodel &blind);
|
|||
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);
|
||||
|
||||
|
||||
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();
|
||||
|
||||
|
|
@ -199,13 +203,13 @@ void setup() {
|
|||
blind1.length_clear=50;
|
||||
blind1.length_opaque=74;
|
||||
blind1.sense_clear_lower=40;
|
||||
blind1.sense_clear_upper=100;
|
||||
blind1.sense_opaque_lower=150;
|
||||
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=29;
|
||||
blind1.speedfactorHigh=24.5;
|
||||
blind1.speedfactorLow=28.7;
|
||||
blind1.speedfactorHigh=25.3;
|
||||
blind1.start_first_clear=27;
|
||||
blind1.simulated_acc_dec=-150;
|
||||
blind1.simulated_acc_inc=200;
|
||||
|
|
@ -216,13 +220,13 @@ void setup() {
|
|||
blind2.length_clear=50;
|
||||
blind2.length_opaque=74;
|
||||
blind2.sense_clear_lower=40;
|
||||
blind2.sense_clear_upper=100;
|
||||
blind2.sense_opaque_lower=150;
|
||||
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=28;
|
||||
blind2.speedfactorHigh=24.5;
|
||||
blind2.speedfactorLow=27.6;
|
||||
blind2.speedfactorHigh=23.5;
|
||||
blind2.start_first_clear=27;
|
||||
blind2.simulated_acc_dec=-150;
|
||||
blind2.simulated_acc_inc=200;
|
||||
|
|
@ -276,15 +280,22 @@ void loopHandler() {
|
|||
|
||||
|
||||
//Read sensor/encoder
|
||||
if (millis() > last_sensor_led_switch + SENSOR_SWITCH_INTERVAL) {
|
||||
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
|
||||
|
|
@ -293,7 +304,7 @@ void loopHandler() {
|
|||
sensorreadID=0; //reset, failsafe
|
||||
break;
|
||||
}
|
||||
last_sensor_led_switch=millis();
|
||||
last_sensor_read=millis();
|
||||
sensorreadID++;
|
||||
sensorreadID=sensorreadID%2;
|
||||
}
|
||||
|
|
@ -339,8 +350,11 @@ void loopHandler() {
|
|||
estimatePosition(blind2, blind2Node);
|
||||
|
||||
//Update Motor Driver
|
||||
updateMotor(blind1, M1);
|
||||
updateMotor(blind2, M2);
|
||||
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
|
||||
|
|
@ -415,32 +429,16 @@ void manualMoveHandler(button &btn, blindmodel &blind)
|
|||
|
||||
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
|
||||
blind.sense_read[blind.sense_read_pos]=value;
|
||||
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
|
||||
classifySensorValue(blind); //writes to blindmodel.sense_status
|
||||
|
||||
|
||||
last_sensor_read=millis();
|
||||
}
|
||||
|
||||
if (millis() > last_print + 500 && blind.speedSimulated!=0) {
|
||||
Serial.print("SenseStatus=");
|
||||
switch(blind.sense_status){
|
||||
case SENSESTATUS_UNKNOWN:
|
||||
Serial.print("UNK");
|
||||
break;
|
||||
case SENSESTATUS_CLEAR:
|
||||
Serial.print("CLE");
|
||||
break;
|
||||
case SENSESTATUS_OPAQUE:
|
||||
Serial.print("OPA");
|
||||
break;
|
||||
case SENSESTATUS_END:
|
||||
Serial.print("END");
|
||||
break;
|
||||
}
|
||||
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));
|
||||
|
|
@ -467,12 +465,12 @@ void errorCheck(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
|
||||
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;
|
||||
last_calculate_position_estimate=millis();
|
||||
blind.last_calculate_position_estimate=millis();
|
||||
}
|
||||
|
||||
//if (blind.mode!= MODE_FIND_END) {
|
||||
|
|
@ -535,9 +533,9 @@ void estimatePosition(blindmodel &blind, HomieNode& node) {
|
|||
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));
|
||||
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) { //only check error if not in find_end mode
|
||||
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);
|
||||
}
|
||||
|
|
@ -555,16 +553,15 @@ void updateMotor(blindmodel &blind, Motor motor)
|
|||
if (blind.error!=0) { //error appeared
|
||||
blind.speed=0; //set speed to 0
|
||||
}
|
||||
if (millis() > last_motor_send + MOTOR_UPDATE_INTERVAL) {
|
||||
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);
|
||||
}
|
||||
last_motor_send=millis();
|
||||
|
||||
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) {
|
||||
|
|
@ -580,7 +577,7 @@ void checkModes(blindmodel &blind, HomieNode &node) {
|
|||
break;
|
||||
|
||||
case 1: //drive down slowly until passed end maker
|
||||
blind.speed=10; //down slow
|
||||
blind.speed=25; //down slow
|
||||
if (blind.sense_status!=SENSESTATUS_END) {
|
||||
blind.speed=0; //stop
|
||||
blind.position=0;
|
||||
|
|
@ -708,6 +705,24 @@ String modeNumToString(uint8_t modenum){
|
|||
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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -729,28 +744,6 @@ bool blind_l_positionHandler(const HomieRange& range, const String& value) {
|
|||
|
||||
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) {
|
||||
|
|
@ -763,12 +756,27 @@ bool blind_r_positionHandler(const HomieRange& range, const String& value) {
|
|||
|
||||
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;
|
||||
|
|
@ -776,11 +784,49 @@ bool blind_r_cmdHandler(const HomieRange& range, const String& value) {
|
|||
}else if (value=="SPEEDFACTOR")
|
||||
{
|
||||
blind2.mode=MODE_MEASURE_SPEED; //measure speed
|
||||
blind1Node.setProperty("cmd").send("cmd measure speed");
|
||||
}else{
|
||||
Homie.getLogger() << "command not known" << endl;
|
||||
Homie.getLogger() << "unknown command" << endl;
|
||||
return false;
|
||||
}
|
||||
blind2Node.setProperty("cmd").send(value); //can be done in main loop
|
||||
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue