Diff: proto_code.cpp

Revision:

17:d8b901d791fd

Child:

18:affef3a7db2a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/proto_code.cpp	Tue Nov 03 07:05:15 2015 +0000
@@ -0,0 +1,1247 @@
+#include <TA.h>
+#include <types.h>
+
+#define RED p3
+#define GREEN p5
+#define BLUE p6
+
+#define ENABLE_1 p4
+#define ENABLE_2 p7
+#define ENABLE_3 p8
+
+#define DEBUG_BAUD  57600 //57600
+#define ADDRESS           1
+#define MAX_LEN          24  // buffer input commands up to this length
+#define NUM_CONES         6
+#define STATIONS         20  // max length of a pattern
+#define SEQUENCES         9  // number of patterns to store
+
+#define TRILAT_CONE      99
+
+#define DEBOUNCE_MS 100
+#define LIGHTS 0x07
+#define FAKEOUT 0x08
+#define FAIL_QUICK 0x10
+#define SILENT 0x20
+#define GRACE_PERIOD 3000
+
+#define NEED_CONSOLE_OUTPUT 1 /* Set this if you need //////////////////////DEBUG messages on the console;
+                               * it will have an impact on code-size and power consumption. */
+
+#define LOOPBACK_MODE       0  // Loopback mode
+
+#if NEED_CONSOLE_OUTPUT
+#define DEBUG(...) { printf(__VA_ARGS__); }
+#else
+#define DEBUG(...) /* nothing */
+#endif /* #if NEED_CONSOLE_OUTPUT */
+
+extern int random(int numberone, int numbertwo);
+
+void writeToPhone(char *data);
+unsigned long millis();
+unsigned long micros();
+
+TA ta;
+
+bool active_cones[NUM_CONES + 1]; // + 1 so we can be 1 based like the cone numbers are
+
+Mode_t mode = PATTERN;
+patternState_t state_p = IDLE_P;
+inputState_t state_i = IDLE_I;
+
+Message m1 = { 'm',0,2 };
+
+Message *m = &m1;
+Message m2 = { 'm',0,2 };
+Message *m_in = &m2;
+
+uint8_t active_cone = 0;
+unsigned long timeout = 0;
+uint8_t mask = 0x07;
+uint8_t fakeout = 0;
+uint8_t fail_quick = 0;
+uint8_t index = 0;
+bool new_state = false;
+bool tag_start = false; // flag to indicate we should wait for the user to activate the first station before going through the sequence
+static bool in_menu = false;
+static bool warning = false;
+static bool penalty = false;
+static bool logging = false;
+
+// course setup (probably should make some of these persistant settings)
+uint8_t active_sequence = 0;
+//uint8_t pattern = 1;
+uint8_t station = 1;
+uint8_t cone = 0;
+uint16_t ltime = 0;
+
+volatile bool triggered;
+volatile bool pin;
+volatile bool ping = false;
+static volatile bool captured = false;
+static volatile unsigned long ping_timer = 0;
+static volatile unsigned long dist_timeout = 0;
+
+// all sequence data
+uint8_t cone_table[STATIONS * SEQUENCES];
+uint8_t mask_table[STATIONS * SEQUENCES];
+uint16_t time_table[STATIONS * SEQUENCES];
+
+// pointer to active table
+uint8_t *cones = (uint8_t*)cone_table;
+uint8_t *masks = (uint8_t*)mask_table; 
+uint16_t *times = (uint16_t*)time_table;
+
+static bool lonely = false;
+
+// Function prototypes
+void spin();
+patternState_t stateFromCone(uint8_t cone);
+void printSplit(unsigned long timer);
+uint8_t getRandomCone(void);
+void clearCones(void);
+void powerupCones(uint8_t sound);
+void failCones(void);
+void resetSensors(void);
+void successCones(void);
+void find_cones(void);
+void printMsAsSeconds(unsigned long number);
+void spinButtons(void);
+uint8_t checkButtons(void);
+Event getInputEvent(void); 
+void getRadioInput(char *ibuffer, int size);
+void interpret(char parameter, int value);
+
+char local_input[50] = {0};
+
+//DigitalOut red(RED);
+//DigitalOut enable1(ENABLE_1);
+//DigitalOut green(GREEN);
+//DigitalOut blue(BLUE);
+//DigitalOut enable2(ENABLE_2);
+//DigitalOut enable3(ENABLE_3);
+
+void setup()
+{
+    ta.initialize(ADDRESS);
+  
+  uint16_t sent = 0;
+  uint16_t lost = 0;
+  unsigned long start = micros();
+  //while (millis() - now <= ACK_TIME){
+  m->command = 'p';
+  m->value = 0;
+  m->cone = 0;
+  
+  find_cones(); // this causes the beep to be interrupted
+  ta.beep(100);  
+  int i;
+  for(i=1;i<NUM_CONES+1;i++)active_cones[i] = true;
+    
+    unsigned long time = 0;
+  
+  // pull course sequences from non-volatile memory  
+  for(uint8_t i=0;i<STATIONS * SEQUENCES;i++){
+    cone_table[i] = i+1;
+    mask_table[i] = 1;
+    time_table[i] = 1000;
+  }    
+}
+
+void loop()
+{
+ //   wdt_reset();
+    
+    static Mode_t last_mode = mode;
+    
+    if (last_mode != mode)
+    {
+        ////DEBUG("\n");
+        
+       // if (mode == FREEFORM)
+            //////DEBUG("Now running random routes.\n");
+            
+        //if (mode == PATTERN)
+            //////DEBUG("Now running set patterns.\n");    
+    }
+    
+    last_mode = mode;
+    
+    //getRadioInput();
+    ta.spin();
+    //spinButtons();
+    //if((logging || micros() < dist_timeout) && ta.recieve(m_in))
+    //    //////DEBUG("%c",m_in->command);
+    //else 
+        spin();
+}
+
+void getNext()
+{
+    if(mode == FREEFORM)
+    {
+      DEBUG("get next freeform\r\n");
+      ++active_cone;//getRandomCone(); 
+      if (active_cone > 6)
+      {
+        active_cone = 1;    
+      }
+      mask = 0x07;
+      ta.setMask(mask & LIGHTS);
+      timeout = ~0;
+      return;
+  }
+  
+  active_cone = cones[index];
+  mask = masks[index];
+  fakeout = mask & FAKEOUT;
+  fail_quick = mask & FAIL_QUICK;
+  timeout = times[index];
+  index++;
+  DEBUG("Next cone is \n");
+  DEBUG("%d\n",active_cone);
+}
+
+void spin()
+{
+    
+    static patternState_t last_state = START_P;
+    static uint8_t last_cone = 255;
+    static unsigned long start;
+    static unsigned long timer;
+    static bool first;  // first should be true when we first enter a state (even if we just exited the same state)
+    char command = 'n';
+    unsigned long value = 0;  
+    
+    m_in->command = 0;
+    m_in->value = 0;
+    m_in->cone = 0;
+  
+    if(ta.recieve(m_in))
+    {
+        DEBUG("spin received: %d '%c'\r\n", m_in->cone, m_in->command);
+        command = m_in->command;
+        value = m_in->value;
+    }
+  
+  first = false;
+  
+  if(last_state != state_p || new_state)
+  {
+    if(state_p == START_P)
+        DEBUG("State is START\n");
+    if(state_p == WAITING_P)
+        DEBUG("State is WAITING\n");
+    if(state_p == ACTIVE_TARGET_P)
+        DEBUG("State is ACTIVE_TARGET\n");
+    if(state_p == IDLE_P)
+        DEBUG("State is IDLE\n");
+    if(state_p == FAIL_P)
+        DEBUG("State is FAIL\n");
+    if(state_p == SUCCESS_P)
+        DEBUG("State is SUCCESS)\n");
+  
+    first = true;
+    new_state = false;
+  }
+
+  last_state = state_p;
+  last_cone = active_cone;
+  
+  timer = millis() - start;
+
+  switch(state_p)
+  {
+  case IDLE_P:
+    if(!in_menu)
+        ta.post_color((ta.activated())?0xFF:0xFF0000);
+    break;
+      
+  case START_P:
+    //clearCones();
+    tag_start = false;
+    ta.post_color(0xFF0000);
+    index = 0;
+    getNext();
+    
+    if(timeout == 0)
+    {  // timeout of 0 means wait indefinitely for the first cone to be activated, then start the rest of the sequence
+     timeout = ~0;
+     tag_start = true; 
+     DEBUG("Activate cone ");
+     DEBUG("%d",active_cone);
+     DEBUG(" to start.\n");
+    }
+    
+    start = millis();
+    state_p = stateFromCone(active_cone);
+    if(active_cone == 0)
+    {  // in this case stateFromCone returns SUCCESS_P, but really there was no course sequence
+      DEBUG("\n");
+      DEBUG("Empty course sequence!\n");
+      ta.post_color(0x0000FF);
+      state_p = IDLE_P;
+      break;
+    }
+    break;
+    
+  case WAITING_P:
+    if(first)
+    {
+      m->command = 'm';
+      m->value = mask;
+      m->cone = active_cone;
+      ta.send(m);
+      m->command = 't';
+      m->value = (uint32_t)timeout;
+      ta.send(m);
+      m->command = 'g';
+      ta.send(m);
+      
+      DEBUG("m_in cone=%d\n", active_cone);
+      m_in->cone = active_cone;
+      command = 'd';
+      
+    }
+    
+    ta.post_color(( ta.activated())?0xFF:0xFF0000);
+    
+    DEBUG("Waiting: %d %d %c\n", m_in->cone, active_cone, command);
+    
+    if((m_in->cone == active_cone && command == 'd' ) || ((timer >= timeout) && fakeout))
+    {
+      if((timer >= timeout) && !fakeout)
+      {
+        DEBUG("Timesplit penalty! ");
+      }
+      printSplit(timer);
+      getNext(); 
+      start = millis();
+      
+      state_p = stateFromCone(active_cone); 
+      
+      if(state_p == WAITING_P) 
+      {
+      DEBUG("New state from waiting\n");
+        new_state = true;
+        }
+    }
+    else if(~timeout != 0 && mode == PATTERN && timer >= (timeout + ((fail_quick)?0:GRACE_PERIOD)))
+    {
+      DEBUG("Failing from wait\n");
+      state_p = FAIL_P;
+    }
+    break;
+    
+  case ACTIVE_TARGET_P:
+    if(first)
+    {
+      ta.setMask(mask);
+      
+      warning = false;
+      penalty = false;
+      ta.pulse(50,750,~0L,0x00FF00);
+      //if(!(mask & SILENT))ta.pulse(50,750,~0L,0c00FF00);
+    }
+    if(timer >= timeout)
+    {
+      if(!penalty)
+      {
+        ta.pulse(50,325,~0L,0xFF00FF);
+        
+        penalty = true;
+      }
+      ta.mask_color(0xFF00FF);
+    }
+    else if(timer > ((timeout*3)/4) && !warning)
+    {
+      warning = true;
+      ta.pulse(50,750,~0L,0xFFFF00);
+    }
+
+    if(ta.activated() || ((timer >= timeout) && fakeout))
+    {
+      if((timer >= timeout) && !fakeout)
+        DEBUG("Timesplit penalty! ");
+        
+      printSplit(timer);
+      ta.post_color(0xFF0000);
+      getNext();
+        start = millis();
+      state_p = stateFromCone(active_cone); 
+      
+      if(state_p == ACTIVE_TARGET_P)
+        new_state = true;
+      ta.pulse_off();
+      
+    }
+    else if(~timeout != 0 && mode == PATTERN && timer >= (timeout + ((fail_quick)?0:GRACE_PERIOD)))
+    {
+      state_p = FAIL_P;
+      ta.pulse_off();
+    }
+    //DEBUG.println(timeout + ((fail_quick)?0:GRACE_PERIOD));
+    //DEBUG.println(timeout);
+    
+    break;
+    
+  case FAIL_P:
+    if(first)
+    {
+      start = millis();
+      DEBUG("\n");
+      DEBUG("Timeout\n");
+      //ta.post_color();
+      ta.pulse(25,200,3000,0xFF0000);
+      
+      failCones(); 
+      //ta.fail();
+    }
+    else if(timer > 3000)
+    {
+      DEBUG("Clear!\n");
+      state_p = IDLE_P;
+      clearCones();
+    }
+    break;
+  case SUCCESS_P:
+    if(first)
+    {
+      start = millis();
+      DEBUG("\n");
+      DEBUG("Done!\n");
+      ta.post_color(0x00FF00);
+      ta.beep(1500);
+      successCones(); 
+      //ta.success();
+    }
+    else if(timer > 1500)
+    {
+      DEBUG("Clear!\n");
+      state_p = IDLE_P;
+      clearCones();
+    }
+    break;
+    
+  default:
+    break;
+  }  
+
+}
+
+void getRadioInput(char *ibuffer, int size)
+{
+  static int i = 0;
+  int buffer_counter = 0;
+  static char parameter = '_';
+  static char buffer[MAX_LEN + 1];
+  int value = 0;
+  char *endp = NULL;
+  
+  // listen for commands coming over bluetooth
+  while (buffer_counter < size)
+  {
+    char ch = ibuffer[buffer_counter++];
+
+    if((ch == '\r' || ch == ';' || ch == '\n') && parameter != '_')
+    {
+      if(i > 1)
+      {
+        buffer[i-1] = 0;
+        value = atoi(buffer);
+        
+        if(parameter == 'l')
+            value = strtoul(buffer, &endp, 2);
+      }
+      //////DEBUG.print(parameter);
+      //////DEBUG.println(value);
+      //////DEBUG.println("not _");
+      interpret(parameter, value);
+      DEBUG("After interp: '%c'\r\n", parameter);
+      parameter = '_';
+      buffer[0] = 0;
+      i=0;
+    }
+    else
+    {
+      if(i==0)
+        parameter = ch;
+      else 
+        buffer[i-1] = ch;
+      i++;
+    }
+    
+    if(ch == '_' || ch == '\r' || ch == ';' || ch == '\n')
+    {
+      parameter = '_';
+      buffer[0] = 0;
+      i=0;
+    }
+  }
+  
+  DEBUG("End of radio input\r\n");
+}
+
+void interpret(char parameter, int value)
+{
+  int remainder = 0;
+  uint16_t split = 0;
+  uint16_t t = 0;
+  uint8_t c = 0;
+  uint8_t l = 0;
+  int last = 0;
+  int middle = 0;
+  uint8_t length = 0;
+  uint8_t offset = 0;
+  int i = 0;
+  uint8_t v = 0;
+  uint16_t val = 0;
+  char phone_buffer[150] = {0};
+  
+  switch(parameter)
+  {
+    case 'S': 
+      ta.spin();
+      break;
+  case 'f':
+    if(lonely)
+    {
+      DEBUG("Sorry, no other cones detected, please try detecting cones first.");
+      break;
+    }
+    DEBUG("Entered freeform");
+    mode = FREEFORM;
+    state_p = START_P;
+    clearCones();
+    find_cones();
+    break;
+  case 'p':
+    if(value == 0)
+    {
+      DEBUG("\n");
+      DEBUG("Running pattern ");
+      //DEBUG(pattern);
+      DEBUG("%d\n",active_sequence + 1);
+      mode = PATTERN;
+      state_p = START_P;
+      active_cone = 0;
+      clearCones();
+    }
+    else
+    {
+      DEBUG("Selected pattern ");
+      DEBUG("%d\n",value);
+      if(value <= SEQUENCES && value > 0)
+      {
+        active_sequence = value - 1;
+        cones = (uint8_t*)cone_table + (value-1)*STATIONS;
+        times = (uint16_t*)time_table + (value-1)*STATIONS;
+        masks = (uint8_t*)mask_table + (value-1)*STATIONS;
+      }
+      else
+      {
+        DEBUG("This pattern is not available.  Please select a value between 1 and ");
+        DEBUG("%d\n",SEQUENCES);  
+      }
+    }
+    break;
+  case 's':
+    station = value;
+    DEBUG("Selected station ");
+    DEBUG("%d\n",value);
+    break;
+  case 'd':
+    if(value == 0)
+    {
+      logging = false;
+      m->value = 0;
+      dist_timeout = micros() + 2000000;
+    }
+    if(value == 1)
+    {
+      logging = true;
+      m->value = 1;
+    }
+    
+    m->command = 'd';
+    m->cone = TRILAT_CONE;
+    ta.send(m);
+    DEBUG("Sent d");
+    //DEBUG(value);
+    break;
+  case 'c':
+    c = value;
+    DEBUG("Station ");
+    DEBUG("%d",station);
+    DEBUG(" will be cone ");
+    DEBUG("%d\n",value);
+    
+    if(station <= STATIONS && station > 0)
+        cones[station-1] = c;
+    break;
+  case 't':
+    t = (uint16_t)value;
+    //snprintf(buffer, sizeof(buffer), "station");
+    snprintf(phone_buffer, sizeof(phone_buffer), "Station %d split time is: %d.\n", station, t/1000);
+    DEBUG(phone_buffer);
+    //writeToPhone(phone_buffer);
+    //writeToPhone("%d",station);
+    //writeToPhone(" split time is: ");
+    //writeToPhone("%d",t/1000);
+    //writeToPhone(".");
+    remainder = t%1000;
+    
+    //if(remainder < 100)
+        //writeToPhone("0");
+    //if(remainder < 10)
+       // writeToPhone("0");
+    //snprintf(buffer, sizeof(buffer), "%d\n seconds\n\n", remainder);
+    //writeToPhone(buffer);
+    //writeToPhone("%d\n",remainder);
+    //writeToPhone(" seconds.\n");
+    //writeToPhone("\n");
+    
+    if(station <= STATIONS && station > 0)
+        times[station-1] = t;
+    break;
+  case 'l':
+    l = 0;
+    l = (uint8_t)value;
+    masks[station-1] = l;
+    DEBUG("Station ");
+    DEBUG("%d",station);
+    //DEBUG(" config bits: ");
+    //DEBUG("%s",byte_to_binary(l));
+    //DEBUG("\n");
+      //}
+    break;
+  case 'q':
+    state_p = IDLE_P;
+    new_state = true; // force state reporting, even if we're already in IDLE
+    ta.pulse_off();
+    clearCones();
+    break;
+  case 'r':
+    //////DEBUG.println(F(""));
+    DEBUG("Current pattern is ");
+    DEBUG("%d\n",active_sequence + 1);
+    DEBUG(":\n");
+    
+    for(int i=0; i<STATIONS; i++)
+    {
+      DEBUG("Station ");
+      DEBUG("%d",i+1);
+      DEBUG(": cone ");
+      DEBUG("%d\n",cones[i]);
+      DEBUG(", ");
+      
+      split = times[i];
+      printMsAsSeconds(split); 
+      //DEBUG.print(F("s timeout, lights: "));
+      DEBUG("s timeout, config bits: ");
+      l = masks[i];
+      /*
+      if(l<0x80)
+        ////DEBUG("0");
+      if(l<0x40)
+        ////DEBUG("0");
+      if(l<0x20)
+        ////DEBUG("0");
+      if(l<0x10)
+        ////DEBUG("0");
+      if(l<0x08)
+        ////DEBUG("0");
+      if(l<0x04)
+        ////DEBUG("0");
+      if(l<0x02)
+        ////DEBUG("0");
+      //////DEBUG("%s\n",byte_to_binary(l));
+      */
+    }
+    break;
+  case 'u':
+    // let any pending messages clear
+    while(ta.get_buffer_size())
+    {
+      ta.spin();
+    }
+    
+    if(value == 1)
+    {
+      DEBUG("Course leader!");
+      powerupCones(value);
+      ta.powerup(value);
+    }
+    
+    if(value == 2)
+    {
+      DEBUG("Split leader!");
+      powerupCones(value);
+      ta.powerup(value);
+    }
+    
+    if(value > 10 && value < 5000)
+    {
+      ta.beep(value);
+    }
+    break;
+  case 'w':
+    if(value > 0 && value <= SEQUENCES)
+    {
+      length = STATIONS;
+      offset = (value - 1) * STATIONS;
+      DEBUG("Saved sequence ");
+      DEBUG("%d\n",value);
+    }
+    else
+    {
+      length = STATIONS * SEQUENCES;
+      offset = 0;
+      DEBUG("Saved all sequences.\n");
+    }
+    
+    for(i=offset;i<length+offset;i++)
+    {
+      //eeprom_update_byte(addressConeTable + i, cone_table[i]); !SR
+      //eeprom_update_byte(addressMaskTable + i, mask_table[i]); !SR
+      //eeprom_update_word(addressTimeTable + i, time_table[i]); !SR
+    }
+    break;
+   case 'x':
+     resetSensors();
+     //a3 = 0;
+     wait_ms(100);
+     //a3 = 1;
+     break;
+     
+   case 'z':
+    find_cones();
+    DEBUG("end of 'z'\r\n");
+    /*!SR
+    m->value = value;
+    m->command = 'z';
+    m->cone = 2;
+    DEBUG("sending...");
+    ta.send(m);
+    DEBUG("sent");
+    */
+    break;
+      
+  default:
+    // seems like this is overflowing some memory somewhere, so commented out for now
+    /*////DEBUG.println(F(""));
+      ////DEBUG.println(F("Enter:"));
+      ////DEBUG.println(F("'f;' to run freeform patterns"));
+      ////DEBUG.println(F("'p;' to run the previously selected programmed pattern"));
+      ////DEBUG.println(F("'r;' to review the custom pattern"));
+      ////DEBUG.println(F("'p#;' to select a pattern"));
+      ////DEBUG.println(F("'s#;' to select a station"));
+      ////DEBUG.println(F("'c#;' to assign a cone to the selected station"));
+      ////DEBUG.println(F("'t#;' to assign a time to a station (in milliseconds)"));
+      ////DEBUG.println(F("'l###;' to set the active lights (111 = on,on,on 000=off,off,off"));
+      ////DEBUG.println(F("All commands must be terminated by ';'"));
+      ////DEBUG.println(F(""));
+      ////DEBUG.println(F("Examples:"));
+      ////DEBUG.println(F("'s1;'"));
+      ////DEBUG.println(F("Select the first station"));
+      ////DEBUG.println(F(""));
+      ////DEBUG.println(F("'c4;t5000;'"));
+      ////DEBUG.println(F("Assign cone 4 and a time split of 5 seconds "));
+      ////DEBUG.println(F("to the current station 1"));
+      ////DEBUG.println(F(""));
+      ////DEBUG.println(F("'s2;c4;t1500;'"));
+      ////DEBUG.println(F("Assign cone 2 as the next station 2, "));
+      ////DEBUG.println(F("and require a time split of 1.5 seconds."));*/
+    break;
+  }
+}
+
+patternState_t stateFromCone(uint8_t cone)
+{
+  if(cone == 0 || index == STATIONS) 
+    return SUCCESS_P;
+  if(cone == 1) 
+    return ACTIVE_TARGET_P;
+    
+  return WAITING_P;
+}
+
+void printSplit(unsigned long timer)
+{
+  if(tag_start)
+  {
+    //DEBUG("Sequence started at cone ");
+    //DEBUG("%d",active_cone);
+    //DEBUG(" (");
+    printMsAsSeconds(timer);
+    //DEBUG(" seconds).\n");
+    tag_start = false;
+  }
+  else
+  {
+   //DEBUG("Target cone is: ");
+   //DEBUG("%d",active_cone);
+   //DEBUG(", ");
+   printMsAsSeconds(timer);
+   //DEBUG(" split");
+  }
+}
+
+uint8_t getRandomCone(void)
+{
+  static uint8_t lastCone = 0;
+  uint8_t cone;
+  
+  do
+  {
+    cone = random(1, NUM_CONES + 1);
+  }
+  while(cone == lastCone || active_cones[cone] == false);
+  
+  //DEBUG("Target cone is ");
+  //DEBUG("%d\n",cone);
+  lastCone = cone;
+  return cone;
+}
+
+void clearCones(void)
+{
+  uint8_t i;
+  m->command = 'q';
+  
+  for(i=2;i<NUM_CONES+1;i++)
+  {
+    m->cone = i;
+    if(active_cones[i])
+        ta.send(m);
+    active_cones[i] = false;
+    //ta.send("q", i);
+  }
+  
+  DEBUG("sent clear\r\n"); 
+}
+
+void powerupCones(uint8_t sound)
+{
+  uint8_t i;
+  m->command = 'u';
+  m->value = sound;
+  
+  if(sound == 2)
+  {
+    if(active_cone == ADDRESS)
+    {
+        ta.powerup(sound);
+    }
+    else
+    {
+      m->cone = active_cone;
+      ta.send(m);
+    }
+  }
+  else
+  {
+    for(i=2;i<NUM_CONES+1;i++)
+    {
+      m->cone = i;
+      if(active_cones[i])
+        ta.send(m);
+      //ta.send("f", i);
+    }
+  }
+  
+  //DEBUG("sent powerup");
+}
+
+void failCones(void)
+{
+  uint8_t i;
+  m->command = 'f';
+  
+  for(i=2;i<NUM_CONES+1;i++)
+  {
+    m->cone = i;
+    
+    if(active_cones[i])
+        ta.send(m);
+    //ta.send("f", i);
+  }
+  
+  while(ta.get_buffer_size())
+  {
+    ta.spin();
+  }
+  
+  //DEBUG("sent fail\n");
+}
+
+void resetSensors(void)
+{
+  uint8_t i;
+  m->command = 'x';
+  
+  for(i=2;i<NUM_CONES+1;i++)
+  {
+    m->cone = i;
+    if(active_cones[i])
+        ta.send(m);
+    //ta.send("f", i);
+  }
+  
+  DEBUG("sent sensor reset\n");
+}
+
+void successCones(void)
+{
+  uint8_t i;
+  m->command = 's';
+  
+  for(i=2;i<NUM_CONES+1;i++)
+  {
+    m->cone = i;
+    if(active_cones[i])
+        ta.send(m);
+    //ta.send("s", i);
+  }
+  
+  while(ta.get_buffer_size())
+  {
+    ta.spin();
+  }
+  
+  //DEBUG("sent success\n");
+}
+
+void find_cones(void)
+{
+
+  while(ta.get_buffer_size())
+    ta.spin(); // wait for all messages to leave queue
+  
+  uint8_t i;
+  m->command = 'z';
+
+  lonely = true;
+  
+  DEBUG("Finding cones\r\n");
+  
+  for(i=2;i<NUM_CONES+1;i++)
+  {
+    active_cones[i] = false;
+    m->cone = i;
+    ta.send_immediate(m);
+
+    unsigned long st = millis();
+    unsigned long delta = 0;
+    
+    while(1)
+    {
+      delta = millis() - st;
+      
+      if(delta > 50) 
+        break;
+        
+      ta.spin();
+      
+      if(ta.recieve(m_in))
+      {
+        lonely = false;
+        active_cones[m_in->cone] = true;
+        
+        DEBUG("Reply from cone: ");
+        DEBUG("%d",m_in->cone);
+        DEBUG(", ");
+        DEBUG("%d",delta);
+        DEBUG("ms\n");
+        break;
+      }
+    }
+    
+  }
+  
+  DEBUG("available cones are: (1");
+  
+  for(i=2;i<NUM_CONES+1;i++)
+  {
+    if(active_cones[i])
+    {
+      DEBUG(", ");
+      DEBUG("%d",i);
+    }
+  }
+  DEBUG(")\r\n");
+}
+
+void printMsAsSeconds(unsigned long number)
+{
+  uint16_t remainder;
+  
+  //DEBUG("%d",number/1000);
+  //DEBUG(".");
+  
+  remainder = number%1000;
+  /*
+  if(remainder < 100)
+    //DEBUG("0");
+    
+  if(remainder < 10)
+    //DEBUG("0");
+    
+  //DEBUG("%d",remainder);
+  */
+}
+
+ void spinButtons(void)
+ {
+  static inputState_t last_state = IDLE_I;
+  static bool first_i;  // first should be true when we first enter a state (even if we just exited the same state)
+  static Event event;
+  static uint8_t sequence = 0;
+  static unsigned long start = 0;
+  uint8_t section = 0;
+
+  //uint8_t buttons;
+  
+  //timer = millis() - start;
+  if(millis() > 500)
+    event = getInputEvent();
+    
+  if(event.type == Event::press)
+  {
+    in_menu = true;
+    state_i = MENU_I;
+  }
+
+  first_i = false;
+  
+  if(last_state != state_i)
+  {
+    //if(state_i == IDLE_I)////////////DEBUG.println(F("");
+    //if(state_i == RUNNING_I)//////////////DEBUG.println(F("State is WAITING");
+    // need to print menu timeout?
+    if(state_i == MENU_I)
+        ////DEBUG("Menu\n");
+    if(state_i == PATTERN_SELECT_I)
+        ////DEBUG("Choosing pattern");
+    first_i = true;
+  }
+
+  last_state = state_i;
+  
+  if(state_i == RUNNING_I)
+    in_menu = false;
+  
+  switch(state_i)
+  {
+  case IDLE_I:
+    // display something distinctive
+    if(event.type == Event::tap)
+    {
+      state_i = RUNNING_I;
+      //send 'p' or 'f' command as appropriate
+      if(mode == FREEFORM)
+        interpret('f', 0);
+      else 
+        interpret('p', 0);
+    }
+    break;
+  case RUNNING_I:
+    if(event.type == Event::finish) 
+        state_i = IDLE_I;
+    break;
+  case MENU_I:
+    if(first_i)
+        interpret('q', 0);
+    // every 3 seconds we cycle through the menu
+    // one option per second
+    // light changes color and stays on for 500ms
+    section = ((millis() - start)%3000)/500;
+    
+    if(section == 0) 
+        ta.post_color(0xFF);
+    if(section == 1) 
+        ta.post_color(0);
+    if(section == 2) 
+        ta.post_color(0xFF00);
+    if(section == 3) 
+        ta.post_color(0);
+    if(section == 4) 
+        ta.post_color(0xFF0000);
+    if(section == 5) 
+        ta.post_color(0);
+    if(event.type == Event::tap)
+    {
+      // set state here based on current light color
+      if(section < 2)
+      {
+        mode = FREEFORM;
+        ta.post_color(0xFF);
+        ////////DEBUG.println(F("Freeform mode.");
+        state_i = IDLE_I;
+        break;
+      }
+      
+      if(section < 4)
+      {
+        mode = PATTERN;
+        ta.post_color(0xFF00);
+        ////////DEBUG.println(F("Pattern mode.");
+        state_i = IDLE_I;
+        break;
+      }
+      
+      if(section < 6)
+      {
+        state_i = PATTERN_SELECT_I;
+        ta.post_color(0xFF0000);
+        break;
+      }
+    }
+    break;
+  case PATTERN_SELECT_I:
+    if(first_i)
+    {
+      sequence = active_sequence;
+      mode = PATTERN;
+      //start = millis();
+    }
+    if(event.type == Event::tap)
+    {
+      ta.beep(50);
+      if(event.value == 1 && sequence > 0)
+      {
+        sequence--;
+      }
+      
+      if(event.value == 2 && sequence < SEQUENCES - 1)
+      {
+        sequence++;
+      }
+      
+      if(event.value == 3)
+      {
+        interpret('p', sequence + 1);
+        state_i = IDLE_I;
+      }
+      
+      if(event.value != 3)
+      {
+        ////DEBUG("Menu says: sequence ");
+        ////DEBUG("%d\n",sequence + 1);
+      }
+      break;
+    case TEACH_I:
+      // not implemented yet
+      break;
+    default: break;    
+    }
+  }
+}
+
+uint8_t checkButtons(void)
+{
+  static unsigned long last_time = 0;
+  //static uint8_t last = 0;
+  static uint8_t buttons = 0;
+  
+  // listen for commands from the buttons
+  unsigned long time = millis();
+  // only check every DEBOUNCE_MS to avoid jitter
+  if(time - last_time > DEBOUNCE_MS)
+  {
+    last_time = time;
+    buttons = ta.buttons();
+  }
+  
+  return buttons;
+}
+
+
+Event getInputEvent(void)
+{
+  static uint8_t last_buttons = 0;
+  static unsigned long time1 = 0;
+  static unsigned long time2 = 0;
+  static unsigned long time3 = 0;
+  unsigned long duration = 0;
+  uint8_t buttons = 0;
+  Event event;
+  event.type = Event::none;
+  event.value = 0;
+
+  buttons = checkButtons(); 
+  uint8_t rising = buttons & ~last_buttons;
+  uint8_t falling = ~buttons & last_buttons;
+
+  /*if(rising){
+    //DEBUG.print("Rising ");
+    //DEBUG.println(rising);
+  }
+  if(falling){
+    //DEBUG.print("Falling ");
+    //DEBUG.println(falling);
+    }*/
+  if(rising & 0x01)
+    time1 = millis();
+  if(rising & 0x02)
+    time2 = millis();
+  if(rising & 0x04)
+    time3 = millis();
+  
+  // simultaneous falling edges will cause lower values to be ignored
+  if(falling & 0x01)
+  {
+    duration = millis() - time1;
+    event.value = 1;
+  }
+  
+  if(falling & 0x02)
+  {
+    duration = millis() - time2;
+    event.value = 2;
+  }
+  
+  if(falling & 0x04)
+  {
+    duration = millis() - time3;
+    event.value = 3;
+  }
+  
+  if(duration > 0)
+    event.type = Event::tap;
+  if(duration > 2000)
+    event.type = Event::press;
+  
+  // give feedback that we've waited lng enough for a press
+  unsigned long t = millis() - time1;
+  
+  if(2020 > t && t > 2000 && (buttons & 0x01)) 
+    ta.beep(20);
+    
+  t = millis() - time2;
+  
+  if(2020 > t && t > 2000 && (buttons & 0x02)) 
+    ta.beep(20);
+    
+  t = millis() - time3;
+  
+  if(2020 > t && t > 2000 && (buttons & 0x04))
+     ta.beep(20);
+
+  if(event.type != Event::none)
+  {
+    //DEBUG("Event: ");
+    if(event.type == Event::tap) 
+        ////DEBUG("tap, ");
+    if(event.type == Event::press) 
+        ////DEBUG("press, ");
+        
+    uint8_t val = event.value;
+    //DEBUG("%d\n",val);
+    //////DEBUG.println(duration);
+  }
+  
+  last_buttons = buttons;
+  return event;
+}