npaiva & kirmanh Demo 4: Robotic Pet Dragon

How to Train Your Dragon 3:
For this project we decided to iterate upon a previous demo, the rocking dragon from demo 3. The changes included better decorations and a different sensor array. Because the focus was on the code, not as much time was spent on the mechanics. The code we wrote is designed to spend as much time as possible tracking sensors to prevent any loss of user input.
We decided to create a template based on code from the previous project for this demo. The template includes a basic structure for a finite state machine and moving averages on analog pins. In order to make the state machine easier to write, we elected to form it out of handler functions instead of a switch statement. These functions are stored in an array with user-defined IDs, and then invoked in the main loop. The helper function “transition_state” allows for easy state switching, and the specially written “wait” function allows for non-wasteful delays. We also added an “interrupt” function to emulate hardware interrupts.
CAD: N/A
/* 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);
float 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 SW_1_FLG  0
#define U1_FLAG   1
#define U2_FLAG   2
#define NUM_FLAGS 3
 
/* STATE IDS */
#define IDLE_1_STATE    0
#define IDLE_2_STATE    1
#define APPROACH_STATE  2
#define HOVER_STATE     3
#define TOUCH_STATE     4
#define NUM_STATES      5
 
/* MOVING AVERAGE PARAMETERS */
#define NUM_PINS    2
#define NUM_SAMPLES 6
 
/* PIN DEFINITIONS */
#define SPKR_PIN    5
#define WING_PIN    9
#define HEAD_PIN    8
#define LED_PIN     2
#define SWITCH_PIN  3
 
#define ECHO_1 13
#define TRIG_1 12
#define ECHO_2 11
#define TRIG_2 10
 
/* MISC CONSTANTS */
#define LIGHT_THRESHOLD 500
 
Servo wing_servo;
Servo head_servo;
 
/******************** ULTRASOUND ********************/
 
void setup_ultrasound(){
  pinMode(ECHO_1, INPUT);
  pinMode(TRIG_1, OUTPUT);
  digitalWrite(TRIG_1, LOW);
 
  pinMode(ECHO_2, INPUT);
  pinMode(TRIG_2, OUTPUT);
  digitalWrite(TRIG_2, LOW);
}
 
/* measure_ultrasound:
 * Helper function for measuring ultrasound on given pins.
 */
float measure_ultrasound(uint8_t echo_pin, uint8_t trig_pin){
  //Send a 10 us pulse
  digitalWrite(trig_pin, LOW);
  delayMicroseconds(5);
  digitalWrite(trig_pin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig_pin, LOW);
 
  //Wait for it to come back
  long duration = pulseIn(echo_pin, HIGH);
 
  //Divide by two to get time to target, then by constant to get distance in cm
  return (duration / 2) / 29.1;
}
 
/******************** ULTRASOUND END ********************/
#define WING_MAX 150
#define WING_MIN 130
#define WING_INT (WING_MAX - WING_MIN)
#define DIV 20.0
 
#define HEAD_MIN 0
#define HEAD_MAX 50
 
uint8_t wing_pos(uint16_t counter){  
  return WING_MIN + WING_INT*((1 + sin( ((float)counter) / DIV ))/2); 
}
 
/*  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);
  setup_ultrasound();
 
  /* Initialize the wing servo */
  wing_servo.attach(WING_PIN);
  wing_servo.write(WING_MIN);
 
  /* Initialize the head servo */
  head_servo.attach(HEAD_PIN);
  head_servo.write(HEAD_MIN);
 
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, LOW);
}
 
/* 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(get_flag(U1_FLAG)){
    transition_state(APPROACH_STATE);
    reset_flag(U1_FLAG);
  }
 
  if(get_flag(U2_FLAG)){
    transition_state(HOVER_STATE);
    reset_flag(U2_FLAG);
  }
   
  if(get_flag(SW_1_FLG)){
    transition_state(TOUCH_STATE);
    reset_flag(SW_1_FLG);
    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.
*/
#define D1_THRESH 30.0
#define D2_THRESH 4.0
 
void set_flags(){
  /* USER DEFINED CODE HERE*/
 
  //Has the switch been triggered?
  if(digitalRead(SWITCH_PIN)) trigger_flag(SW_1_FLG);
 
  //Is anyone close to the ultrasound sensors?
  if(get_analog(0) < D1_THRESH) trigger_flag(U1_FLAG);
  if(get_analog(1) < D2_THRESH) trigger_flag(U2_FLAG);
   
}
 
/* Helper function for playing a random, low tone for a random amount
 *  of time on the speaker pin.
 */
void roar(){
  tone(SPKR_PIN, 
    random(40,80),        //40 to 80 Hz 
    random(2000, 4000));  //2 to 4 seconds
}
 
 
void happy(){
  tone(SPKR_PIN, 
    random(400,800),        //40 to 80 Hz 
    random(2000, 4000));  //2 to 4 seconds
}
 
uint16_t counter = 0;
bool friendly = false;
/* PUT YOUR STATE MACHINE HANDLERS HERE */
 
void idle_1_handler(){
  wing_servo.write(wing_pos(counter));
  head_servo.write(HEAD_MIN);
  counter++;
  friendly = false;
   
  digitalWrite(LED_PIN, LOW);
 
  if(counter > 200){
    counter = 0;
    transition_state(IDLE_2_STATE);
  }
}
 
void idle_2_handler(){
  if(wait(SECOND)) return;
   
  digitalWrite(LED_PIN, LOW);
 
  transition_state(IDLE_1_STATE);
}
 
void approach_handler(){
  roar();
  digitalWrite(LED_PIN, HIGH);
   
  if(wait(SECOND)) return;
   
  transition_state(IDLE_1_STATE);
}
 
void hover_handler(){
  if(wait(random(1, 5)*SECOND)) return;
 
  head_servo.write(HEAD_MAX);
  friendly = true;
  digitalWrite(LED_PIN, LOW);
 
  wait(5*SECOND);
   
  transition_state(IDLE_1_STATE);
}
 
void touch_handler(){
  if(friendly){ 
    happy();
     
    if(wait(2000)) return;
    transition_state(IDLE_1_STATE);
  } else {
    digitalWrite(LED_PIN, HIGH);
    roar();
     
    if(wait(2000)) return;
    transition_state(IDLE_1_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_1_STATE, &idle_1_handler);
  attach_state(IDLE_2_STATE, &idle_2_handler);
  attach_state(APPROACH_STATE, &approach_handler);
  attach_state(HOVER_STATE, &hover_handler);
  attach_state(TOUCH_STATE, &touch_handler);
}
 
/******************** MOVING AVERAGE LIBRARY ********************/
float avg_samples [NUM_PINS][NUM_SAMPLES];
uint8_t ind         [NUM_PINS];
float avg_total   [NUM_PINS];
float avg         [NUM_PINS];
 
/* Update one specific rolling average */
void update_avg(uint8_t pin, float val){
  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] = val;
  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(){
  //float d1 = 0.0;
  //float d2 = 0.0;
 
  //Measure the ultrasound distances
  //measure_ultrasounds(&d1, &d2);
 
  float d1 = measure_ultrasound(ECHO_1, TRIG_1);
  float d2 = measure_ultrasound(ECHO_2, TRIG_2);
   
  //Update their averages
  update_avg(0, d1);
  update_avg(1, d2);
  //Serial.println(d1);
}
 
/* 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 */
float 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
 
  for(uint8_t i = 0; i < NUM_FLAGS; i++){
    reset_flag(i);
  }
}
 
void do_background_tasks(){
  //Do our analog measurements
  update_avgs();
  //Do our pin checks, set our flags
  set_flags();
}
 
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;
}
16-223 Work

Categories

Archives

Meta

npaiva & kirmanh Demo 4: Robotic Pet Dragon

Leave a Reply Cancel reply
