The Amazing Self-Righting Box is a device that automatically rights itself when tipped over. The deception is simple. The box indicates that the “right side” is one way, while the actual preferred orientation is another. This box exploits an inherent trust in labels. From nutritional labels to street signs, we are raised to trust the knowledge derived from our environment. But, not everything you read is true.

The orientation of the box is derived from an analog accelerometer. To reduce the noise from the servo motors, the Arduino is powered from a separate power supply. Additionally, a continuous software-based smoothing filter is applied to the data from the analog pins. A state machine based framework is used to control the logic of the box.

We used the design from Henry’s previous demo with a few modifications. Here are the original CAD file and modified bottom plate and new arm: demo5

/* Nicholas Paiva - 2018
 * State machine and analog measurement template code
 */

/******************** DEPENDENCIES ********************/
#include <Servo.h>


/******************** FORWARD DECLARATIONS ********************/
#define SECOND 1000000
#define MINUTE 60 * SECOND 

//Utility
bool wait(uint32_t delay_time);
int16_t get_analog(uint8_t pin);

//States
void transition_state(uint8_t state_num);
void attach_state(uint8_t state_num, void (*function)(void));
bool interrupt();

//Flags
void trigger_flag(uint8_t flag);
void reset_flag(uint8_t flag);
void set_flag(uint8_t flag, bool value);
bool get_flag(uint8_t flag);
void dig_read_and_set(uint8_t flag, uint8_t pin);

/******************** USER DEFINED SECTION ********************/
/* FLAG IDS */
#define TIP_FLAG  0
#define NUM_FLAGS 1

/* STATE IDS */
#define IDLE_STATE  0
#define TIP_STATE   1
#define NUM_STATES  2

/* MOVING AVERAGE PARAMETERS */
#define NUM_PINS    3
#define NUM_SAMPLES 8

/* PIN DEFINITIONS */
#define X_PIN 0
#define Y_PIN 1
#define Z_PIN 2
#define ARM_PIN 9

/* MISC CONSTANTS */
#define TIP_THRESHOLD 375

Servo arm_servo;
/*  user_setup:
 *  Put any setup code you need to add to this framework here.
 *  This function will run before the main loop starts.
 */
void user_setup(){
  Serial.begin(9600);
  arm_servo.attach(ARM_PIN);
  arm_servo.write(90);
  wait(SECOND);
  reset_flag(TIP_FLAG);
}

/* interrupt:
 * **ADVANCED**
 * This function is used for any code that must happen as immediately
 * as possible. Not a true hardware interrupt, but is called during 
 * delays or between states. This function can be used to interrupt
 * the normal state flow when specific events occur, or execute
 * user-defined background tasks. Use with caution, because this has
 * the potential to make your state machine harder to understand.
 * 
 * If this function returns true when called, the machine will stop
 * whatever it is doing and transition to the selected state.
 */
bool interrupt(){
  /*if(state == IDLE_STATE && false){
    transition_state(RUN1_STATE);
    return true;
  }*/
  
  return false;
}

/* set_flags:
 * This function is called every time the main loop runs, before the 
 * current state handler function is called. Use this function to 
 * check on any pins you need to keep track of and set flags for later.
 * 
 * For example, you can use "dig_read_and_set" to read a digital pin and
 * store its value in a flag, and then check that flag later in your
 * state machine.
*/
void set_flags(){
  /* USER DEFINED CODE HERE*/
  if(get_analog(Y_PIN) > TIP_THRESHOLD) trigger_flag(TIP_FLAG);
}

/* PUT YOUR STATE MACHINE HANDLERS HERE */
void idle_handler(){
  arm_servo.write(90);
  
  if(get_flag(TIP_FLAG)){
    transition_state(TIP_STATE);
  }
}

void tip_handler(){
  //Serial.println("1");
  wait(random(2*SECOND, SECOND*6));
  
  for(uint8_t i = 0; i < 20; i++){
    arm_servo.write(70-i);
    wait(50000);
    arm_servo.write(50+i);
    wait(50000);
  }
  
  arm_servo.write(0);
  
  wait(SECOND);
  reset_flag(TIP_FLAG);
  transition_state(IDLE_STATE);
}
/* END HANDLER SECTION */

/*  Helper function for defining our state machine.
 *  Use as follows:
 *  1. Create a handler function for each state of the type void fn(void)
 *  2. Count the number of states, n
 *  3. Create a unique ID for each state in the range [0, n)
 *  4. Call attach_state(ID, &fn) to define each state
 */
void define_states(){
  attach_state(IDLE_STATE, &idle_handler);
  attach_state(TIP_STATE, &tip_handler);
}

/******************** MOVING AVERAGE LIBRARY ********************/
int16_t avg_samples [NUM_PINS][NUM_SAMPLES];
uint8_t ind         [NUM_PINS];
int16_t avg_total   [NUM_PINS];
int16_t avg         [NUM_PINS];

/* Update one specific rolling average */
void update_avg(uint8_t pin){
  uint8_t i = ind[pin];
  
  //Subtract old value
  avg_total[pin] = avg_total[pin] - avg_samples[pin][i];

  //Measure and add new value
  avg_samples[pin][i] = analogRead(pin);
  avg_total[pin] = avg_total[pin] + avg_samples[pin][i];
  
  //Increment and wrap counter
  ind[pin] = i + 1;
  ind[pin] %= (uint8_t)NUM_SAMPLES;

  //Calculate the average
  avg[pin] = avg_total[pin] / NUM_SAMPLES;
}

/* Update all of the rolling averages */
void update_avgs(){
  for(uint8_t i = 0; i < NUM_PINS; i++){
    update_avg(i);
  }
}

/* Initializes the data to 0 for all of the rolling average code */
void init_avgs(){
  //Zero out AVG array
  for(uint8_t i = 0; i < NUM_PINS; i++){
    for(uint8_t j = 0; j < NUM_SAMPLES; j++){
      avg_samples[i][j] = 0;
    }
    ind[i] = 0;
    avg_total[i] = 0;
    avg[i] = 0; 
  }
}

/* Helper function for getting average value with error checking */
int16_t get_analog(uint8_t pin){
  if(pin < NUM_PINS)  //If a safe access, return the average
    return avg[pin];
  else                //Otherwise return an error
    return -1;
}

/******************** THRESHOLD / SWITCH CHECKING LIBRARY ********************/

bool flags[NUM_FLAGS] = {0};
/* Helper function for setting a particular flag, with error checking */
void set_flag(uint8_t flag, bool value){
  if(flag < NUM_FLAGS) //If outside of our array, do nothing
    flags[flag] = value;
}
/*Helper function to set a flag to 1 regardless of anything else */
void trigger_flag(uint8_t flag){
  set_flag(flag, 1);
}
/*Helper function to set a flag to 0 regardless of anything else */
void reset_flag(uint8_t flag){
  set_flag(flag, 0);
}
/* Helper function to set a flag to the digital read value of a pin */
void dig_read_and_set(uint8_t flag, uint8_t pin){
  set_flag(flag, digitalRead(pin));
}
/* Helper function for getting a flag value */
bool get_flag(uint8_t flag){
  if(flag < NUM_FLAGS) 
    return flags[flag];
  else 
    return false;
}

/******************** STATE MACHINE LIBRARY ********************/
/* Current state */
uint8_t state = 0;

/* Array for state handling functions */
void (*handler_array[NUM_STATES])(void) = {NULL};

/* Helper function for setting up our state machine */
void attach_state(uint8_t state_num, void (*function)(void)){
  handler_array[state_num] = function;
}
/* Helper function for changing the state */
void transition_state(uint8_t state_num){
  state = state_num;
}

/******************** ARDUINO BOILERPLATE ********************/

void setup() { 
  init_avgs(); //Initialize our analog reading code
  define_states(); //Setup our state machine
  user_setup(); //Give the user opportunities to setup things
}

void do_background_tasks(){
  //Do our analog measurements
  update_avgs();
  //Do our pin checks, set our flags
  set_flags();

  /*
  Serial.print(get_analog(X_PIN));
  Serial.print(",");
  Serial.print(get_analog(Y_PIN));
  Serial.print(",");
  Serial.print(get_analog(Z_PIN));
  Serial.println(",");
  */
  
  if(state == TIP_STATE){
    tone(5, get_analog(Y_PIN) - get_analog(X_PIN) + 300);
  } else {
    noTone(5);
  }
}

void loop() {
  //Do the stuff that goes on in the background
  do_background_tasks();

  //Allow for interrupts to the current state machine flow
  interrupt();

  //Call the current state handler
  handler_array[state]();
}

/*  wait:
 *  Special delay function that delays our current state machine code,
 *  but still executes background tasks. No state transition may occur
 *  during this time. Analog measurements are updated, and flags are
 *  set.
 */
bool wait(uint32_t delay_time){
  //Get the current time and setup a tracker variable
  uint32_t last_update = micros();
  uint32_t d_t = 0;

  //If we have not waited enough time yet, keep doing background tasks until we have
  while(d_t < delay_time){
    uint32_t now = micros();
    //Add the time elapsed to our tracker variable
    d_t += (now - last_update);
    last_update = now;

    //Do our background tasks while we wait
    do_background_tasks();

    //Allow for interrupts to the current state machine flow
    if(interrupt()) return true;
  }
  return false;
}