blinds_motorcontrol/src/main.cpp

#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 drive to position
  - implement failure detection
  - implement homie
*/

#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 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;



#define PIN_SENSE A0
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 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

//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 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

  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;

  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)

  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.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 //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);
void classifySensorValue(blindmodel &blind);
void checkButton(button &btn);
void checkModes(blindmodel &blind);
void manualMoveHandler(button &btn, blindmodel &blind);
void readSensor(blindmodel &blind, int value);
void estimatePosition(blindmodel &blind);
void errorCheck(blindmodel &blind);
void updateMotor(blindmodel &blind, Motor motor);
void setError(blindmodel &blind, uint8_t errorcode);

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=500;
  blind1.sense_opaque_upper=850;
  blind1.sense_end_lower=850;
  blind1.sense_end_upper=1024;
  blind1.speedfactor=29;
  blind1.start_first_clear=27;
  blind1.simulated_acc_dec=-150;
  blind1.simulated_acc_inc=200;

  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=500;
  blind2.sense_opaque_upper=850;
  blind2.sense_end_lower=850;
  blind2.sense_end_upper=1024;
  blind2.speedfactor=29;
  blind2.start_first_clear=27;
  blind2.simulated_acc_dec=-150;
  blind2.simulated_acc_inc=200;

  //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


  
  
}


void loop() {

  checkButton(button1);
  checkButton(button2);
  

  //Manual movement by button
  manualMoveHandler(button1, blind1);
  manualMoveHandler(button2, blind2);



  //Read sensor/encoder
  if (millis() > last_sensor_led_switch + SENSOR_SWITCH_INTERVAL) {
    int rawsensorvalue=analogRead(PIN_SENSE);
    switch (sensorreadID) {
      case 0:
        readSensor(blind1,rawsensorvalue);
        sensorreadID++;
        digitalWrite(blind1.pin_sensor_led,LOW); //turn self off
        digitalWrite(blind2.pin_sensor_led,HIGH); //turn next on
        break;
      case 1:
        readSensor(blind2,rawsensorvalue);
        sensorreadID++;
        digitalWrite(blind2.pin_sensor_led,LOW); //turn self off
        digitalWrite(blind1.pin_sensor_led,HIGH); //turn next on
        break;
      default:
        sensorreadID=0; //reset
        break;
    }
    last_sensor_led_switch=millis();
  }  


  checkModes(blind1);
  checkModes(blind2);

  errorCheck(blind1);
  errorCheck(blind2);
  
  //Estimate blind position and correct
  estimatePosition(blind1);
  estimatePosition(blind2);

  //Update Motor Driver
  updateMotor(blind1, M1);
  updateMotor(blind2, M2);
}

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) {
    blind.sense_status=SENSESTATUS_CLEAR;
    blind.last_sense_ok=millis();
  } else if (filtered>=blind.sense_opaque_lower && filtered<=blind.sense_opaque_upper) {
    blind.sense_status=SENSESTATUS_OPAQUE;
    blind.last_sense_ok=millis();
  } else if (filtered>=blind.sense_end_lower && filtered<=blind.sense_end_upper) {
    blind.sense_status=SENSESTATUS_END;
    blind.last_sense_ok=millis();
  } 
}

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) {
    blind.mode=MODE_IDLE;
    if (btn.down) { //changed to pressed
      if (btn.manual_drive_direction) {
        //M1.setmotor( _CW, 100);
        blind.speed=-100;
        //Serial.print("CW PWM: ");
      }else{
        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.speed=0;
    }
  }
}

void readSensor(blindmodel &blind, int value)
{
  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;

    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("("); Serial.print(getFitered(blind.sense_read, SENSE_FILTER_SIZE)); Serial.print(")");
    Serial.print(", mode=");
    switch(blind.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(blind.speed);
    Serial.print(", speedSim=");
    Serial.print(blind.speedSimulated);    
    Serial.print(", pos=");
    Serial.println(blind.position);
    last_print=millis();
  }
}

void errorCheck(blindmodel &blind) {
  if (blind.sense_status==SENSESTATUS_END) {
    if (blind.speed<0) { //stop driving up
      blind.speed=0;
    }
  }

  //TODO: led self test. turn off ,should be high value
}

void estimatePosition(blindmodel &blind) {
  if (millis() > last_calculate_position_estimate + CALCULATEPOSITIONESTIMATE_INTERVAL) {
    float _actualTime=(millis()-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*blind.speedfactor * _actualTime;
    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;
      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
      Serial.print("Reached End marker. set Position="); Serial.println(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*blind.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)) { //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(", 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*blind.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);

          if (abs(blind.position-_position_before)>=(blind.length_clear+blind.length_opaque)/2.0*DIFF_ERROR_FACTOR) {
            setError(blind, ERRORCODE_POSITIONDIFFTOOHIGH);
          }
        }

        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 (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();
  }
}

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 (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=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
            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
          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 setError(blindmodel &blind, uint8_t errorcode){
  if (blind.error==0) { //only set error if no error appeared before
    blind.error=errorcode;
  }
  Serial.print("ERROR CODE="); Serial.println(errorcode);
}