@ -40,6 +40,12 @@ 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
@ -82,6 +88,8 @@ struct blindmodel
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
@ -98,6 +106,7 @@ struct blindmodel
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 ;
@ -112,16 +121,19 @@ Motor M2(0x30, _MOTOR_B, 1000); //Motor B
unsigned long last_motor_send = 0 ;
# define MOTOR_UPDATE_INTERVAL 100
# define DIFF_ERROR_FACTOR 0.35 //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
int getFitered ( int * values , uint8_t size ) ;
uint8_t classifySensorValue ( blindmodel & blind ) ;
void classifySensorValue ( blindmodel & blind ) ;
void checkButton ( button & btn ) ;
void checkModes ( blindmodel & blind ) ;
void manualMoveHandler ( button & btn , blindmodel & blind ) ;
void readSensor ( 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 ) ;
@ -134,17 +146,23 @@ void setup() {
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 = 1 00;
blind1 . sense_clear_upper = 2 00;
blind1 . sense_opaque_lower = 500 ;
blind1 . sense_opaque_upper = 7 50;
blind1 . sense_opaque_upper = 8 50;
blind1 . sense_end_lower = 850 ;
blind1 . sense_end_upper = 1024 ;
blind1 . speedfactor = 29 ;
@ -152,12 +170,13 @@ void setup() {
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 = 1 00;
blind2 . sense_clear_upper = 2 00;
blind2 . sense_opaque_lower = 500 ;
blind2 . sense_opaque_upper = 7 50;
blind2 . sense_opaque_upper = 8 50;
blind2 . sense_end_lower = 850 ;
blind2 . sense_end_upper = 1024 ;
blind2 . speedfactor = 29 ;
@ -169,8 +188,11 @@ void setup() {
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
//blind2.mode = MODE_FIND_END;
//blind2.mode_find_end_state=0; //reset mode find state
}
@ -189,8 +211,27 @@ void loop() {
//Read sensor/encoder
readSensor ( blind1 ) ;
readSensor ( blind2 ) ;
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 ) ;
@ -230,7 +271,7 @@ int getFitered(int* values,uint8_t size) {
return copied_values [ size / 2 ] ;
}
uint8_t classifySensorValue ( blindmodel & blind ) {
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 ;
@ -277,11 +318,11 @@ void manualMoveHandler(button &btn, blindmodel &blind)
}
}
void readSensor ( blindmodel & blind )
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 ] = analogRead( PIN_SENSE ) ;
blind . sense_read [ blind . sense_read_pos ] = value ;
classifySensorValue ( blind ) ; //writes to blindmodel.sense_status
@ -289,7 +330,7 @@ void readSensor(blindmodel &blind)
last_sensor_read = millis ( ) ;
}
if ( millis ( ) > last_print + 500 ) {
if ( millis ( ) > last_print + 500 & & blind . speedSimulated ! = 0 ) {
Serial . print ( " SenseStatus= " ) ;
switch ( blind . sense_status ) {
case SENSESTATUS_UNKNOWN :
@ -337,6 +378,8 @@ void errorCheck(blindmodel &blind) {
blind . speed = 0 ;
}
}
//TODO: led self test. turn off ,should be high value
}
void estimatePosition ( blindmodel & blind ) {
@ -388,7 +431,7 @@ void estimatePosition(blindmodel &blind) {
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 ( " posCorrect o in=" ) ; Serial . print ( _filterdelay_correction ) ;
Serial . print ( " posCorrect io n=" ) ; Serial . print ( _filterdelay_correction ) ;
Serial . print ( " , UP&CLE->OPA or DOWN&OPA->CLE, before= " ) ;
@ -399,6 +442,10 @@ void estimatePosition(blindmodel &blind) {
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
@ -409,6 +456,9 @@ void estimatePosition(blindmodel &blind) {
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 ) ) ;
@ -487,3 +537,11 @@ void checkModes(blindmodel &blind) {
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 ) ;
}