make manual drive better comply with iot

This commit is contained in:
interfisch 2021-02-14 12:39:19 +01:00
parent 2ef0cf5b69
commit a6e668f263
1 changed files with 19 additions and 8 deletions

View File

@ -7,7 +7,6 @@
/* /*
TODO: TODO:
- implement failure detection (timeouts), - implement failure detection (timeouts),
- 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) - 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) - manual mover function should work better with iot commands (set position, no errors)
*/ */
@ -74,7 +73,9 @@ uint8_t sensorreadID=0;
#define MODE_FIND_END 1 #define MODE_FIND_END 1
#define MODE_MEASURE_SPEED 2 #define MODE_MEASURE_SPEED 2
#define MODE_GO_TO_POS 3 #define MODE_GO_TO_POS 3
#define MODE_ERROR 4 #define MODE_MANUAL 4
#define MODE_ERROR 5
//model parameters/variables //model parameters/variables
struct blindmodel struct blindmodel
@ -341,14 +342,14 @@ void loopHandler() {
checkModes(blind1, blind1Node); checkModes(blind1, blind1Node);
checkModes(blind2, blind2Node); checkModes(blind2, blind2Node);
errorCheck(blind1, blind1Node);
errorCheck(blind2, blind2Node);
//Estimate blind position and correct //Estimate blind position and correct
estimatePosition(blind1, blind1Node); estimatePosition(blind1, blind1Node);
estimatePosition(blind2, blind2Node); estimatePosition(blind2, blind2Node);
errorCheck(blind1, blind1Node);
errorCheck(blind2, blind2Node);
//Update Motor Driver //Update Motor Driver
if (millis() > last_motor_send + MOTOR_UPDATE_INTERVAL) { if (millis() > last_motor_send + MOTOR_UPDATE_INTERVAL) {
updateMotor(blind1, M1); updateMotor(blind1, M1);
@ -408,19 +409,20 @@ void checkButton(button &btn) {
void manualMoveHandler(button &btn, blindmodel &blind) void manualMoveHandler(button &btn, blindmodel &blind)
{ {
if (btn.changed) { if (btn.changed) {
blind.mode=MODE_IDLE;
if (btn.down) { //changed to pressed if (btn.down) { //changed to pressed
if (btn.manual_drive_direction) { blind.mode=MODE_MANUAL;
if (btn.manual_drive_direction) { //drive up
//M1.setmotor( _CW, 100); //M1.setmotor( _CW, 100);
blind.speed=-100; blind.speed=-100;
//Serial.print("CW PWM: "); //Serial.print("CW PWM: ");
}else{ }else{ //drive down
blind.speed=100; blind.speed=100;
//Serial.print("CCW PWM: "); //Serial.print("CCW PWM: ");
} }
btn.manual_drive_direction=!btn.manual_drive_direction; //switch direction every new press btn.manual_drive_direction=!btn.manual_drive_direction; //switch direction every new press
}else{ //changed to released }else{ //changed to released
//Serial.println("Motor STOP"); //Serial.println("Motor STOP");
blind.mode=MODE_IDLE;
blind.speed=0; blind.speed=0;
} }
} }
@ -458,6 +460,11 @@ void errorCheck(blindmodel &blind, HomieNode &node) {
blind.speed=0; 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_classified_ok-blind.position) > MAX_ALLOWED_CLASSIFY_LENGTH ) { //sensor reading havent been classified for too far if (abs(blind.position_last_classified_ok-blind.position) > MAX_ALLOWED_CLASSIFY_LENGTH ) { //sensor reading havent been classified for too far
setError(blind, ERRORCODE_CLASSIFY_LENGTH, node); setError(blind, ERRORCODE_CLASSIFY_LENGTH, node);
@ -679,6 +686,10 @@ void checkModes(blindmodel &blind, HomieNode &node) {
} }
} }
break; break;
case MODE_MANUAL:
blind.set_position=blind.position; //use current position as set position
break;
} }
} }
} }