blinds_motorcontrol/src/main.cpp

247 lines
6.3 KiB
C++
Raw Normal View History

2021-01-31 12:08:48 +00:00
#include<Arduino.h>
#include <Wire.h>
#include "WEMOS_Motor.h"
//follow firmware flash guide for new wemos motor shield v1.0 https://github.com/thomasfredericks/wemos_motor_shield
2021-01-31 16:59:57 +00:00
#define BUTTON_DEBOUNCE 200
2021-01-31 16:59:57 +00:00
#define PIN_BUTTON1 D5
struct button{
uint8_t pin;
unsigned long last_time_read=0;
bool down=false;
bool changed=false;
};
button button1;
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
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
2021-01-31 16:59:57 +00:00
#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;
//model parameters/variables
struct blindmodel
{
unsigned long lastreadtime=0;
float position=0; //0 is furthest open. positive is down (closing). unit is mm
float length_clear; //length of clear part in position units
float length_opaque; //lengt of opaque part in position units
float speed_estimate=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
2021-01-31 16:59:57 +00:00
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;
int sense_read[SENSE_FILTER_SIZE] = {0};
uint8_t sense_read_pos=0; //position of last element written to
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.
};
blindmodel blind1;
2021-01-31 12:08:48 +00:00
//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
2021-01-31 16:59:57 +00:00
int getFitered(int* values,uint8_t size);
uint8_t classifySensorValue(blindmodel &blind);
2021-01-31 12:08:48 +00:00
void setup() {
Serial.begin(57600);
2021-01-31 16:59:57 +00:00
Serial.println("Starting");
button1.pin=PIN_BUTTON1;
2021-01-31 12:08:48 +00:00
2021-01-31 16:59:57 +00:00
pinMode(button1.pin, INPUT_PULLUP);
pinMode(PIN_SENSE, INPUT);
M1.setmotor(_STOP);
M2.setmotor(_STOP);
2021-01-31 16:59:57 +00:00
//settings for blind
blind1.length_clear=50;
blind1.length_opaque=74;
blind1.sense_clear_lower=40;
blind1.sense_clear_upper=100;
blind1.sense_opaque_lower=600;
blind1.sense_opaque_upper=750;
blind1.sense_end_lower=850;
blind1.sense_end_upper=1024;
blind1.speed_estimate=20;
2021-01-31 12:08:48 +00:00
}
void loop() {
2021-01-31 16:59:57 +00:00
button1.changed=false;
if (millis() > button1.last_time_read + BUTTON_DEBOUNCE) {
bool new_pin_button_down=!digitalRead(button1.pin);
if (button1.down != new_pin_button_down) { //changed
button1.down = new_pin_button_down; //update
button1.changed=true;
button1.last_time_read=millis(); //delay next check
}
}
2021-01-31 16:59:57 +00:00
//Manual movement by button
if (button1.changed) {
if (button1.down) { //changed to pressed
if (manual_drive_direction) {
2021-01-31 16:59:57 +00:00
M1.setmotor( _CW, 100);
Serial.print("CW PWM: ");
}else{
2021-01-31 16:59:57 +00:00
M1.setmotor( _CCW, 100);
Serial.print("CCW PWM: ");
}
2021-01-31 16:59:57 +00:00
Serial.println(100);
manual_drive_direction=!manual_drive_direction; //switch direction every new press
2021-01-31 16:59:57 +00:00
}else{ //changed to released
Serial.println("Motor STOP");
M1.setmotor(_STOP);
}
}
2021-01-31 16:59:57 +00:00
//Read sensor/encoder
if (millis() > last_sensor_read + SENSOR_READ_INTERVAL) {
blind1.sense_read_pos=(blind1.sense_read_pos+1)%SENSE_FILTER_SIZE; //next element
blind1.sense_read[blind1.sense_read_pos]=analogRead(PIN_SENSE);
classifySensorValue(blind1); //writes to blindmodel.sense_status
last_sensor_read=millis();
}
if (millis() > last_print + 100) {
2021-01-31 16:59:57 +00:00
Serial.println(blind1.sense_status);
last_print=millis();
}
}
2021-01-31 16:59:57 +00:00
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];
}
uint8_t 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();
}
}
/*
2021-01-31 12:08:48 +00:00
void loop() {
int pwm;
for (pwm = 0; pwm <= 100; pwm++)
{
M1.setmotor( _CW, pwm);
M2.setmotor(_CW, pwm);
Serial.print("Clockwise PWM: ");
Serial.println(pwm);
delay(100);
}
Serial.println("Motor STOP");
M1.setmotor(_STOP);
M2.setmotor( _STOP);
delay(1000);
for (pwm = 0; pwm <= 100; pwm++)
{
M1.setmotor(_CCW, pwm);
//delay(1);
M2.setmotor(_CCW, pwm);
Serial.print("Counterclockwise PWM: ");
Serial.println(pwm);
delay(100);
}
Serial.println("Motor A&B STANDBY");
M1.setmotor(_STANDBY);
M2.setmotor( _STANDBY);
delay(1000);
}
*/