#include #include #include #include // Enables all serial diagnostics output. Set to false for production #define DIAG false //****************************************************************************** // Pin Assignments const int PIN_LED_WEST = 9; const int PIN_LED_RUN = 6; const int PIN_LED_EAST = 5; const int PIN_BUTTON = 7; const int PIN_RELAY = 8; // for the MMA8451 Accelerometer const int PIN_SCL = 3; const int PIN_SDA = 2; // State Control const int STATE_INIT = 1; const int STATE_LIM_WEST = 2; const int STATE_LIM_EAST = 3; const int STATE_RUN = 4; const int STATE_STOP = 5; static int current_state = STATE_INIT; static int button_state; // Accelerometer Sensor Adafruit_MMA8451 accel = Adafruit_MMA8451(); // Control Structure #define EEPROM_ADDR 0x00 #define MAGIC 0x4B4F5231 // "KOR1" typedef struct accel_ctrl { unsigned long magic; double limit_west; double limit_east; } ACCEL_CTRL; ACCEL_CTRL accel_ctrl; // Misc #define INIT_LOOP_DURATION 5000 // msec #define LED_ON 1 // analogWrite() level - choose for appropriate dimmness #define LED_OFF 0 #define LED_FAST_BLINK 100 // msec #define LED_SLOW_BLINK 500 // msec jmp_buf INIT_ENV; // for button long press reset //****************************************************************************** #define BTN_DEBOUNCE 20 // msec #define BTN_HOLD 2000 // msec #define BTN_DOWN HIGH #define BTN_UP LOW static int button_pressed() { static int btn_cur_state = BTN_UP; static int btn_last_state = BTN_UP; static unsigned long btn_down_time = 0; static unsigned long btn_up_time = 0; unsigned long cur_time = millis(); btn_cur_state = digitalRead(PIN_BUTTON); // Button DOWN if (btn_cur_state == BTN_DOWN && btn_last_state == BTN_UP && (cur_time - btn_up_time) > BTN_DEBOUNCE) { DIAG && Serial.println("Button DOWN"); btn_down_time = cur_time; btn_last_state = BTN_DOWN; return false; } // Button UP if (btn_cur_state == BTN_UP && btn_last_state == BTN_DOWN) { if (cur_time - btn_down_time < BTN_DEBOUNCE) { DIAG && Serial.println("Button UP"); btn_up_time = cur_time; return false; } if (cur_time - btn_down_time < BTN_HOLD) { DIAG && Serial.println("Button PRESS"); btn_up_time = btn_down_time = 0; btn_last_state = BTN_UP; return true; } DIAG && Serial.println("Button HOLD"); btn_up_time = btn_down_time = 0; btn_last_state = BTN_UP; longjmp(INIT_ENV, 1); // Non-local GOTO for reset functionality. Don't worry; I // know what I'm doing! } return false; } //****************************************************************************** static void flash_LED(const int pin, const int rate) { unsigned long cur_time = millis(); int dbl_rate = rate * 2; if (cur_time % rate == 0) { if (cur_time % dbl_rate != 0) analogWrite(pin, LED_ON); else analogWrite(pin, LED_OFF); } return; } //****************************************************************************** static double get_roll_angle(sensors_event_t* event) { double roll_angle = 0.0; DIAG && Serial.print(" - acceleration - X:") && Serial.print(event->acceleration.x) && Serial.print(" Y:") && Serial.print(event->acceleration.y) && Serial.print(" Z:") && Serial.print(event->acceleration.z) && Serial.println(" "); roll_angle = atan2(-1 * event->acceleration.y, event->acceleration.z) * 180.0 / M_PI; return(roll_angle); } //****************************************************************************** static void EEPROM_store(ACCEL_CTRL *ctrl) { size_t len = sizeof(ACCEL_CTRL); for (size_t pos = 0; pos < len; pos++) EEPROM.write(EEPROM_ADDR + pos, ((byte*)ctrl)[pos]); return; } //****************************************************************************** static void EEPROM_load(ACCEL_CTRL *ctrl) { size_t len = sizeof(ACCEL_CTRL); for (size_t pos = 0; pos < len; pos++) ((byte*)ctrl)[pos] = EEPROM.read(EEPROM_ADDR + pos); return; } //****************************************************************************** static void state_init(void) { unsigned long start_time; unsigned long stop_time; DIAG && Serial.println("Entering STATE_INIT"); digitalWrite(PIN_RELAY, HIGH); // close RELAY analogWrite(PIN_LED_WEST, LED_OFF); // Turn all LEDs off analogWrite(PIN_LED_RUN, LED_OFF); analogWrite(PIN_LED_EAST, LED_OFF); DIAG && Serial.println(" - loading persistent control parameters"); EEPROM_load(&accel_ctrl); // If the control structure has never been stored, we initialize it and go // strait to liit setting instead of allowing the user to decide whether // to set limits or run. if (accel_ctrl.magic != MAGIC) { DIAG && Serial.println(" - initializing persistent control parameters"); accel_ctrl.magic = MAGIC; accel_ctrl.limit_west = 0.0; accel_ctrl.limit_east = 0.0; current_state = STATE_LIM_WEST; DIAG && Serial.println("Exiting STATE_INIT - force limit setting"); return; } // If the user presses the button during this loop, he can then set the // angle limits. DIAG && Serial.println(" - entering decition loop"); stop_time = start_time = millis(); while ((stop_time - start_time) < INIT_LOOP_DURATION) { flash_LED(PIN_LED_RUN, LED_FAST_BLINK); if (button_pressed() == true) { analogWrite(PIN_LED_RUN, LED_OFF); current_state = STATE_LIM_WEST; DIAG && Serial.println("Exiting STATE_INIT - set limits"); return; } stop_time = millis(); } analogWrite(PIN_LED_RUN, LED_OFF); current_state = STATE_RUN; DIAG && Serial.println("Exiting STATE_INIT - run with current limits"); return; } //****************************************************************************** static void state_lim_west(void) { sensors_event_t event; DIAG && Serial.println("Entereing STATE_LIM_WEST"); while (!button_pressed()) flash_LED(PIN_LED_WEST, LED_SLOW_BLINK); DIAG && Serial.println(" - getting angle limit from accelerometer"); accel.getEvent(&event); accel_ctrl.limit_west = get_roll_angle(&event); DIAG && Serial.print(" - West angle limit:") && Serial.println(accel_ctrl.limit_west); analogWrite(PIN_LED_WEST, LED_OFF); current_state = STATE_LIM_EAST; return; } //****************************************************************************** static void state_lim_east(void) { unsigned long start_time; unsigned long stop_time; sensors_event_t event; DIAG && Serial.println("Entereing STATE_LIM_EAST"); while (!button_pressed()) flash_LED(PIN_LED_EAST, LED_SLOW_BLINK); DIAG && Serial.println(" - getting angle limit from accelerometer"); accel.getEvent(&event); accel_ctrl.limit_east = get_roll_angle(&event); DIAG && Serial.print(" - EAST angle limit:") && Serial.println(accel_ctrl.limit_east); DIAG && Serial.println(" - storing persistent control variables"); EEPROM_store(&accel_ctrl); analogWrite(PIN_LED_EAST, LED_OFF); DIAG && Serial.println(" - delaying to allow movement away from EAST limit"); stop_time = start_time = millis(); while ((stop_time - start_time) < INIT_LOOP_DURATION) { flash_LED(PIN_LED_RUN, LED_SLOW_BLINK); stop_time = millis(); } current_state = STATE_RUN; return; } //****************************************************************************** static void state_run(void) { double current_angle = 0.0; sensors_event_t event; DIAG && Serial.print("Entereing STATE_RUN - angle limits West:") && Serial.print(accel_ctrl.limit_west) && Serial.print(" East:") && Serial.println(accel_ctrl.limit_east); analogWrite(PIN_LED_RUN, LED_ON); DIAG && Serial.println(" - entering processing loop"); while (1) { // We ignore a button press, but we need to check it so we can act on a // button hold. button_pressed(); accel.getEvent(&event); current_angle = get_roll_angle(&event); DIAG && Serial.print(" - current angle:") && Serial.println(current_angle); // Check the limits. If one is exceeded, set the appropriate LED and // enter STATE_STOP if (current_angle > accel_ctrl.limit_west) { analogWrite(PIN_LED_WEST, LED_ON); current_state = STATE_STOP; return; } if (current_angle < accel_ctrl.limit_east) { analogWrite(PIN_LED_EAST, LED_ON); current_state = STATE_STOP; return; } delay(100); } // Should never get here current_state = STATE_STOP; return; } //****************************************************************************** static void state_stop(void) { DIAG && Serial.println("Entereing STATE_STOP"); digitalWrite(PIN_RELAY, LOW); // open RELAY -- remove power from mount analogWrite(PIN_LED_RUN, LED_OFF); while (1) // Only BUTTON_HOLD exits STATE_STOP button_pressed(); // Should never get here return; } //****************************************************************************** void setup() { if (DIAG) { Serial.begin(9600); while (!Serial) {} } pinMode(PIN_LED_WEST, OUTPUT); pinMode(PIN_LED_RUN, OUTPUT); pinMode(PIN_LED_EAST, OUTPUT); pinMode(PIN_RELAY, OUTPUT); pinMode(PIN_BUTTON, INPUT); /* Initialise the sensor */ if (!accel.begin()) { DIAG && Serial.println("FATAL ERROR - sensor not detected"); while(1) { flash_LED(PIN_LED_WEST, LED_FAST_BLINK); flash_LED(PIN_LED_RUN, LED_FAST_BLINK); flash_LED(PIN_LED_EAST, LED_FAST_BLINK); } } return; } //****************************************************************************** void loop() { static boolean first_iteration = true; // Store the environment so that we can longjmp back to here on a button HOLD. // This simulates a reset since I can't seem to get a real rese to work. if (first_iteration) { first_iteration = false; switch(setjmp(INIT_ENV)) { case 0: break; case 1: DIAG && Serial.println("INTERRUPT by BUTTON HOLD - continuing in state_init()"); current_state = STATE_INIT; break; default: DIAG && Serial.println("FATAL ERROR - cannot create RESET LONGJUMP point"); while (1) {} } } // Although in this case the setjmp/longjmp is safe. Behavior is technically // undefined if the function that called setjmp() returns. So, I am making // a forever loop so that loop() doesn't have to return. while (true) { switch(current_state) { case STATE_INIT: state_init(); break; case STATE_LIM_WEST: state_lim_west(); break; case STATE_LIM_EAST: state_lim_east(); break; case STATE_RUN: state_run(); break; case STATE_STOP: state_stop(); break; default: DIAG && Serial.print("Invalid State: ") && Serial.println(current_state); break; } } }