implement position estimation and correction
This commit is contained in:
parent
ea71f7dd2c
commit
fd43be8326
287
src/main.cpp
287
src/main.cpp
|
|
@ -1,6 +1,14 @@
|
|||
#include<Arduino.h>
|
||||
#include <Arduino.h>
|
||||
#include <Wire.h>
|
||||
|
||||
/*
|
||||
TODO:
|
||||
- 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 failure detection
|
||||
- implement homie
|
||||
*/
|
||||
|
||||
#include "WEMOS_Motor.h"
|
||||
|
||||
|
|
@ -21,43 +29,40 @@ unsigned long last_sensor_read=0;
|
|||
#define SENSOR_READ_INTERVAL 20 //in ms
|
||||
#define SENSE_FILTER_SIZE 20 //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;
|
||||
|
||||
bool manual_drive_direction=false;
|
||||
|
||||
#define PIN_SENSE A0
|
||||
unsigned long last_print=0;
|
||||
#define SENSE_CLEAR_LOWER 40 //adc value lower limit for clear part. clear is around 70
|
||||
#define SENSE_CLEAR_UPPER 100 //adc value upper limit for clear part
|
||||
|
||||
#define SENSE_OPAQUE_LOWER 600 //adc value lower limit for opaque part. opaque is around 675
|
||||
#define SENSE_OPAQUE_UPPER 750 //adc value upper limit for opaque part
|
||||
|
||||
#define SENSE_END_LOWER 850 //adc value lower limit for end marker
|
||||
#define SENSE_END_UPPER 1024 //adc value upper limit for end marker
|
||||
|
||||
//define directions for motors
|
||||
#define _M1_UP _CCW;
|
||||
#define _M1_DOWN _CW;
|
||||
|
||||
#define _M2_UP _CCW;
|
||||
#define _M2_DOWN _CW;
|
||||
|
||||
#define SENSESTATUS_CLEAR 1;
|
||||
#define SENSESTATUS_OPAQUE 2;
|
||||
#define SENSESTATUS_END 3;
|
||||
#define SENSESTATUS_UNKNOWN 0;
|
||||
#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
|
||||
|
||||
//model parameters/variables
|
||||
struct blindmodel
|
||||
{
|
||||
unsigned long lastreadtime=0;
|
||||
float position=0; //0 is furthest open. positive is down (closing). unit is mm
|
||||
float position=0; //0 is furthest open. positive is down (closing). unit is mm. estimated position
|
||||
|
||||
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 speed_estimate=1; //how much position units (mm) per second at pwm=100
|
||||
float speedfactor=1; //how much position units (mm) per second at pwm=100
|
||||
|
||||
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
|
||||
|
|
@ -68,7 +73,9 @@ struct blindmodel
|
|||
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
|
||||
|
||||
|
|
@ -76,6 +83,18 @@ struct blindmodel
|
|||
|
||||
unsigned long last_sense_ok=0; //last time sensor measured class ok
|
||||
//TODO: implement timeout if last_sense_ok gets too high.
|
||||
|
||||
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)
|
||||
|
||||
};
|
||||
|
||||
blindmodel blind1;
|
||||
|
|
@ -86,15 +105,18 @@ blindmodel blind1;
|
|||
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
|
||||
|
||||
|
||||
int getFitered(int* values,uint8_t size);
|
||||
uint8_t classifySensorValue(blindmodel &blind);
|
||||
void checkModes(blindmodel &blind);
|
||||
|
||||
void setup() {
|
||||
|
||||
|
||||
Serial.begin(57600);
|
||||
Serial.begin(115200);
|
||||
Serial.println("Starting");
|
||||
|
||||
button1.pin=PIN_BUTTON1;
|
||||
|
|
@ -111,11 +133,18 @@ void setup() {
|
|||
blind1.length_opaque=74;
|
||||
blind1.sense_clear_lower=40;
|
||||
blind1.sense_clear_upper=100;
|
||||
blind1.sense_opaque_lower=600;
|
||||
blind1.sense_opaque_lower=500;
|
||||
blind1.sense_opaque_upper=750;
|
||||
blind1.sense_end_lower=850;
|
||||
blind1.sense_end_upper=1024;
|
||||
blind1.speed_estimate=20;
|
||||
blind1.speedfactor=29;
|
||||
blind1.start_first_clear=27;
|
||||
blind1.simulated_acc_dec=-150;
|
||||
blind1.simulated_acc_inc=200;
|
||||
|
||||
//Test
|
||||
blind1.mode = MODE_FIND_END;
|
||||
blind1.mode_find_end_state=0; //reset mode find state
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -135,19 +164,20 @@ void loop() {
|
|||
|
||||
//Manual movement by button
|
||||
if (button1.changed) {
|
||||
blind1.mode=MODE_IDLE;
|
||||
if (button1.down) { //changed to pressed
|
||||
if (manual_drive_direction) {
|
||||
M1.setmotor( _CW, 100);
|
||||
Serial.print("CW PWM: ");
|
||||
//M1.setmotor( _CW, 100);
|
||||
blind1.speed=-100;
|
||||
//Serial.print("CW PWM: ");
|
||||
}else{
|
||||
M1.setmotor( _CCW, 100);
|
||||
Serial.print("CCW PWM: ");
|
||||
blind1.speed=100;
|
||||
//Serial.print("CCW PWM: ");
|
||||
}
|
||||
Serial.println(100);
|
||||
manual_drive_direction=!manual_drive_direction; //switch direction every new press
|
||||
}else{ //changed to released
|
||||
Serial.println("Motor STOP");
|
||||
M1.setmotor(_STOP);
|
||||
//Serial.println("Motor STOP");
|
||||
blind1.speed=0;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -164,10 +194,134 @@ void loop() {
|
|||
last_sensor_read=millis();
|
||||
}
|
||||
|
||||
if (millis() > last_print + 100) {
|
||||
Serial.println(blind1.sense_status);
|
||||
if (millis() > last_print + 500) {
|
||||
Serial.print("SenseStatus=");
|
||||
switch(blind1.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("("); Serial.print(getFitered(blind1.sense_read, SENSE_FILTER_SIZE)); Serial.print(")");
|
||||
Serial.print(", mode=");
|
||||
switch(blind1.mode){
|
||||
case MODE_IDLE:
|
||||
Serial.print("IDL");
|
||||
break;
|
||||
case MODE_FIND_END:
|
||||
Serial.print("FIN");
|
||||
break;
|
||||
case MODE_GO_TO_POS:
|
||||
Serial.print("POS");
|
||||
break;
|
||||
case MODE_MEASURE_SPEED:
|
||||
Serial.print("MSP");
|
||||
break;
|
||||
}
|
||||
Serial.print(", speed=");
|
||||
Serial.print(blind1.speed);
|
||||
Serial.print(", speedSim=");
|
||||
Serial.print(blind1.speedSimulated);
|
||||
Serial.print(", pos=");
|
||||
Serial.println(blind1.position);
|
||||
last_print=millis();
|
||||
}
|
||||
|
||||
|
||||
checkModes(blind1);
|
||||
|
||||
|
||||
if (blind1.sense_status==SENSESTATUS_END) {
|
||||
if (blind1.speed<0) { //stop driving up
|
||||
blind1.speed=0;
|
||||
}
|
||||
}
|
||||
//Estimate blind position and correct
|
||||
if (millis() > last_calculate_position_estimate + CALCULATEPOSITIONESTIMATE_INTERVAL) {
|
||||
float _actualTime=(millis()-last_calculate_position_estimate)/1000.0; //in seconds
|
||||
blind1.speedSimulated+= constrain(blind1.speed - blind1.speedSimulated, blind1.simulated_acc_dec*_actualTime, blind1.simulated_acc_inc*_actualTime);
|
||||
|
||||
blind1.position+= blind1.speedSimulated/100.0*blind1.speedfactor * _actualTime;
|
||||
last_calculate_position_estimate=millis();
|
||||
}
|
||||
|
||||
if (blind1.mode!= MODE_FIND_END) {
|
||||
if (blind1.sense_status == SENSESTATUS_END && blind1.last_sense_status != SENSESTATUS_END) { //entered end marker
|
||||
blind1.position=0;
|
||||
float _filterdelay_correction=blind1.speedSimulated/100.0*blind1.speedfactor *SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL/1000.0;
|
||||
blind1.position += _filterdelay_correction; //correct for filter delay
|
||||
Serial.print("Reached End marker. set Position="); Serial.println(blind1.position);
|
||||
}
|
||||
|
||||
if (blind1.sense_status == SENSESTATUS_CLEAR || blind1.sense_status == SENSESTATUS_OPAQUE || blind1.sense_status == SENSESTATUS_END) { //either opaque or clear (or end, only for last update used)
|
||||
if (blind1.sense_status != blind1.last_sense_status) { //status changed
|
||||
if (blind1.last_sense_status == SENSESTATUS_CLEAR || blind1.last_sense_status == SENSESTATUS_OPAQUE) { //only changes from clear to opaque or opaque to clear
|
||||
|
||||
float _position_before=blind1.position;//only for text output debugging
|
||||
|
||||
if ((blind1.speedSimulated>0 && blind1.sense_status==SENSESTATUS_OPAQUE) || (blind1.speedSimulated<0 && blind1.sense_status==SENSESTATUS_CLEAR)) { //moving down from and clear to opaque OR moving up and from opaque to clear
|
||||
|
||||
//estimated_position_difference = -blind1.position + blind1.start_first_clear+blind1.length_clear + _n*(blind1.length_clear+blind1.length_opaque); //possible occurances. let estimated_position=0 , solve for n
|
||||
|
||||
int _n = round((blind1.position -blind1.start_first_clear - blind1.length_clear)/(blind1.length_clear+blind1.length_opaque)); //find closest n
|
||||
blind1.position = blind1.start_first_clear+blind1.length_clear + _n*(blind1.length_clear+blind1.length_opaque); //calculate position from _n
|
||||
float _filterdelay_correction=blind1.speedSimulated/100.0*blind1.speedfactor *SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL/1000.0;
|
||||
blind1.position += _filterdelay_correction; //correct for filter delay
|
||||
Serial.print("n=");
|
||||
Serial.println(_n);
|
||||
|
||||
Serial.print("posCorrection="); Serial.print(_filterdelay_correction);
|
||||
|
||||
Serial.print(", DOWN&CLE->OPA or UP&OPA->CLE, before=");
|
||||
|
||||
|
||||
}else if((blind1.speedSimulated>0 && blind1.sense_status==SENSESTATUS_CLEAR) || (blind1.speedSimulated<0 && blind1.sense_status==SENSESTATUS_OPAQUE)) { //the other transition
|
||||
//estimated_position_difference = -blind1.position + blind1.start_first_clear + _n*(blind1.length_clear+blind1.length_opaque); //possible occurances. let estimated_position=0 , solve for n
|
||||
|
||||
int _n = round(( blind1.position - blind1.start_first_clear)/(blind1.length_clear+blind1.length_opaque)); //find closest n
|
||||
Serial.print("n=");
|
||||
Serial.println(_n);
|
||||
blind1.position = blind1.start_first_clear + _n*(blind1.length_clear+blind1.length_opaque); //calculate position from _n
|
||||
float _filterdelay_correction=blind1.speedSimulated/100.0*blind1.speedfactor*SENSE_FILTER_SIZE/2*SENSOR_READ_INTERVAL/1000.0;
|
||||
blind1.position += _filterdelay_correction; //correct for filter delay
|
||||
|
||||
Serial.print("posCorrectoin="); Serial.print(_filterdelay_correction);
|
||||
|
||||
Serial.print(", UP&CLE->OPA or DOWN&OPA->CLE, before=");
|
||||
|
||||
}
|
||||
|
||||
Serial.print(_position_before);
|
||||
Serial.print(", after=");
|
||||
Serial.print(blind1.position);
|
||||
Serial.print(", diff=");
|
||||
Serial.println(blind1.position-_position_before);
|
||||
}
|
||||
|
||||
blind1.last_sense_status = blind1.sense_status; //update only if new one is opaque or clear
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//Update Motor Driver
|
||||
if (millis() > last_motor_send + MOTOR_UPDATE_INTERVAL) {
|
||||
if(blind1.speed<0){
|
||||
M1.setmotor( _UP, abs(blind1.speed));
|
||||
}else if(blind1.speed>0){
|
||||
M1.setmotor( _DOWN, abs(blind1.speed));
|
||||
}else{
|
||||
M1.setmotor(_STOP);
|
||||
}
|
||||
last_motor_send=millis();
|
||||
}
|
||||
}
|
||||
|
||||
int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort
|
||||
|
|
@ -206,6 +360,73 @@ uint8_t classifySensorValue(blindmodel &blind) {
|
|||
}
|
||||
}
|
||||
|
||||
void checkModes(blindmodel &blind) {
|
||||
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 (blind1.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=10; //down slow
|
||||
if (blind1.sense_status!=SENSESTATUS_END) {
|
||||
blind.speed=0; //stop
|
||||
blind.position=0;
|
||||
//blind.mode=MODE_IDLE;
|
||||
blind.mode=MODE_MEASURE_SPEED; //next measure speed
|
||||
blind.mode_find_end_state=0; //reset
|
||||
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case MODE_MEASURE_SPEED:
|
||||
switch(blind.mode_measure_speed_state) {
|
||||
case 0: //drive down, start timing at clear
|
||||
blind.speed=100; //down
|
||||
if (blind1.sense_status==SENSESTATUS_CLEAR) {
|
||||
blind.timing_start=millis();
|
||||
blind.mode_measure_speed_state++;
|
||||
}
|
||||
break;
|
||||
case 1: //on clear section
|
||||
blind.speed=100; //down
|
||||
if (blind1.sense_status==SENSESTATUS_OPAQUE) { //wait for opaque section
|
||||
blind.mode_measure_speed_state++;
|
||||
}
|
||||
break;
|
||||
case 2: //on opaque section
|
||||
blind.speed=100; //down
|
||||
if (blind1.sense_status==SENSESTATUS_CLEAR) { //wait for clear section
|
||||
blind.speedfactor=(blind.length_clear+blind.length_opaque)/ ((millis()-blind.timing_start)/1000.0); //calculate length per second
|
||||
Serial.print("speedfactor=");
|
||||
Serial.print(blind.speedfactor);
|
||||
Serial.println(" mm/s");
|
||||
blind.position = blind.length_opaque+blind.length_clear+blind.start_first_clear; //set position
|
||||
blind.speed=0; //stop
|
||||
blind.mode_measure_speed_state=0; // reset
|
||||
blind.mode=MODE_IDLE;
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case MODE_GO_TO_POS:
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
void loop() {
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue