247 lines
6.3 KiB
C++
247 lines
6.3 KiB
C++
#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
|
|
|
|
|
|
#define BUTTON_DEBOUNCE 200
|
|
#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
|
|
#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
|
|
|
|
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;
|
|
|
|
|
|
//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
|
|
|
|
|
|
|
|
int getFitered(int* values,uint8_t size);
|
|
uint8_t classifySensorValue(blindmodel &blind);
|
|
|
|
void setup() {
|
|
|
|
|
|
Serial.begin(57600);
|
|
Serial.println("Starting");
|
|
|
|
button1.pin=PIN_BUTTON1;
|
|
|
|
pinMode(button1.pin, INPUT_PULLUP);
|
|
pinMode(PIN_SENSE, INPUT);
|
|
|
|
M1.setmotor(_STOP);
|
|
M2.setmotor(_STOP);
|
|
|
|
|
|
//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;
|
|
|
|
}
|
|
|
|
|
|
void loop() {
|
|
|
|
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
|
|
}
|
|
}
|
|
|
|
|
|
//Manual movement by button
|
|
if (button1.changed) {
|
|
if (button1.down) { //changed to pressed
|
|
if (manual_drive_direction) {
|
|
M1.setmotor( _CW, 100);
|
|
Serial.print("CW PWM: ");
|
|
}else{
|
|
M1.setmotor( _CCW, 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);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
//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) {
|
|
Serial.println(blind1.sense_status);
|
|
last_print=millis();
|
|
}
|
|
}
|
|
|
|
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();
|
|
}
|
|
}
|
|
|
|
/*
|
|
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);
|
|
|
|
}
|
|
*/ |