Diff: proto_code.cpp

Revision:

23:26f27c462976

Parent:

19:afcbb425b3cf

Child:

26:40a0c775ff27

diff -r afcbb425b3cf -r 26f27c462976 proto_code.cpp
--- a/proto_code.cpp	Tue Dec 01 16:03:15 2015 +0000
+++ b/proto_code.cpp	Mon Dec 14 14:24:10 2015 +0000
@@ -10,9 +10,8 @@
 #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 NUM_CONES         2
 #define STATIONS         20  // max length of a pattern
 #define SEQUENCES         9  // number of patterns to store
 
@@ -46,9 +45,16 @@
 
 static bool active_cones[NUM_CONES + 1]; // + 1 so we can be 1 based like the cone numbers are
 
+#ifdef MASTER
 static Mode_t mode = PATTERN;
 static patternState_t state_p = IDLE_P;
 static inputState_t state_i = IDLE_I;
+#define ADDRESS           1
+#else
+State_t state = IDLE;
+#define CONTROL_CONE      1
+#define ADDRESS           2
+#endif
 
 static Message m1 = { 'm',0,2 };
 
@@ -97,6 +103,7 @@
 static bool lonely = false;
 
 // Function prototypes
+#ifdef MASTER
 void spin();
 patternState_t stateFromCone(uint8_t cone);
 void printSplit(unsigned long timer);
@@ -113,6 +120,9 @@
 Event getInputEvent(void); 
 void getRadioInput(char *ibuffer, int size);
 void interpret(char parameter, int value);
+#endif
+
+extern "C" void writeToPhone(char *format, ...);
 
 char local_input[50] = {0};
 
@@ -123,7 +133,8 @@
 //DigitalOut enable2(ENABLE_2);
 //DigitalOut enable3(ENABLE_3);
 
-void setup()
+#ifdef MASTER
+static void master_setup()
 {
     ta.initialize(ADDRESS);
   
@@ -135,9 +146,12 @@
   m->value = 0;
   m->cone = 0;
   
-  find_cones(); // this causes the beep to be interrupted
-  ta.beep(100);  
+  
+  //find_cones(); // this causes the beep to be interrupted
+  //ta.beep(100);  
   int i;
+  
+  //for(i=3;i<=3;i++)active_cones[i] = true;
   for(i=1;i<NUM_CONES+1;i++)active_cones[i] = true;
     
     unsigned long time = 0;
@@ -151,24 +165,213 @@
   
   ta.post_color(0xFF0000);
 }
+#else
+static void slave_setup()
+{
+    ta.initialize(ADDRESS);
+    ta.beep(250);
+}
+#endif
+
+void setup()
+{
+    #ifdef MASTER
+    master_setup();
+    #else
+    slave_setup();
+    #endif    
+}
+
+#ifdef SLAVE
+static void slave_loop()
+{
+    static unsigned long start = 0;
+    static unsigned long timeout;
+    static bool process_next_as_time = false;
+    static bool process_next_as_mask = false;
+    static unsigned long timer;
+    static State_t last_state = IDLE;
+  
+    if(last_state != state)
+    {
+        if(state == ACTIVE_TARGET)
+            writeToPhone("State is ACTIVE_TARGET\n");
+        if(state == IDLE)
+            writeToPhone("State is IDLE");
+        if(state == FAIL)
+            writeToPhone("State is FAIL");
+        if(state == SUCCESS)
+            writeToPhone("State is SUCCESS");
+    }
+     
+    last_state = state;
+  
+    char message = 'n';
+    timer = millis() - start;
+  
+    if(ta.recieve(m_in))
+    {
+        ta.beep(1000);
+        message = m_in->command;
+        switch(m_in->command)
+        {
+        case 't':
+            timeout = m_in->value;
+            //serial.printf("timeout: ");
+            //serial.printf("%d\n",timeout);
+            break;
+        case 'm':
+            //serial.printf("config bits: ");
+            mask = m_in->value;
+            ta.setMask(mask);// & LIGHTS);
+            //serial.printf("%s\n",byte_to_binary(mask));
+            fakeout = mask & FAKEOUT;
+            
+            //if(fakeout)
+            //    serial.printf("fakeout!");
+            
+            fail_quick = mask & FAIL_QUICK;
+            break;
+        default: break;
+    }
+  }
+  
+  if(message == 'x')
+  {
+    //a3 = 0;
+    wait_ms(100);
+    //a3 = 1;
+  }
+  
+  if(message == 'f')
+  { // Fail
+     //serial.printf("Fail!\n");
+     ta.pulse(25,200,3000,0xFF0000);
+     state = FAIL;
+  }
+  
+  if(message == 's')
+  {  // Success
+    //serial.printf("Success!\n");
+    ta.post_color(0x00FF00);
+    ta.beep(1500);
+    state = SUCCESS;
+  }
+
+  if(message == 'g')
+  { // This cone is the active target, GO!
+    start = millis();
+    warning = false;
+    penalty = false;
+    ta.pulse(50,750,~0L,0x00FF00);
+    state = ACTIVE_TARGET;
+    //serial.printf("fakeout, fail_quick\n");
+    //serial.printf("%d",fakeout);
+    //serial.printf(", ");
+    //serial.printf("%d\n",fail_quick);
+  }
+
+  if(message == 'q')
+  {  // Quit
+    //serial.printf("Clear!\n");
+    ta.pulse_off();
+    ta.mask_color(0xFF0000);
+    state = IDLE;
+  }
+  
+  if(message != 'n')
+  {
+    //serial.printf("%d",message);
+    //serial.printf(", ");
+    //serial.printf("%d\n",m_in->value);
+  }
+  
+   if(message == 'u')
+   {  // powerup noise
+      ta.powerup(m_in->value);
+   }
+   
+  if(message == 'z')
+  {
+    m->value = m_in->value;
+    m->command = 'z';
+    m->cone = 3;
+    ta.send(m);
+  }
+  
+  timer = millis() - start;
+  
+  switch(state)
+  {
+    case ACTIVE_TARGET:
+      if(ta.activated() || ((timer > timeout) && (fakeout)))
+      {
+        m->command = 'd';
+        m->cone = CONTROL_CONE;
+        ta.send(m);
+        ta.pulse_off();
+        state = IDLE;
+        //serial.printf("Done!\n");
+      }
+      else if(timer > timeout && !penalty)
+      {
+        //serial.printf("Penalty!\n");
+        penalty = true;
+        ta.pulse(50,325,~0L,0xFF00FF);
+      }
+      else if(timer > ((timeout*3)/4) && !warning)
+      {
+        //serial.printf("Warning!\n");
+        warning = true;
+        ta.pulse(50,750,~0L,0xFFFF00);
+      }
+      break;
+    case IDLE:
+      ta.post_color((ta.activated())?0xFF:0xFF0000);
+      break;
+    default:
+    break;
+  }
+}
+#endif
+
+#ifdef MASTER
+static void master_loop()
+{
+    static Mode_t last_mode = mode;
+    
+    if (last_mode != mode)
+    {
+        if (mode == FREEFORM)
+            writeToPhone("Now running random routes.\n");
+            
+        if (mode == PATTERN)
+            writeToPhone("Now running set patterns.\n");    
+    }
+    
+    last_mode = mode;
+    
+    ta.spin();
+    //spinButtons();
+    if((logging || micros() < dist_timeout) && ta.recieve(m_in))
+        writeToPhone("%c",m_in->command);
+    else 
+        spin();  
+}
+#endif  
 
 void loop()
 {
-    static Mode_t last_mode = mode;
-
-    last_mode = mode;
-    
-    //ta.spin();
-    DEBUG("spinning");
-    //spinButtons();
-    //if((logging || micros() < dist_timeout) && ta.recieve(m_in))
-    //    //////DEBUG("%c",m_in->command);
-    //else 
-    spin();
+    #ifdef MASTER
+    master_loop();
+    #else
+    slave_loop();
+    #endif
 }
 
-void getNext()
+static void getNext()
 {
+    #ifdef MASTER
     if(mode == FREEFORM)
     {
       DEBUG("get next freeform\r\n");
@@ -191,11 +394,12 @@
   index++;
   DEBUG("Next cone is \n");
   DEBUG("%d\n",active_cone);
+  #endif
 }
 #if 1
-void spin()
+static void spin()
 {
-    
+    #ifdef MASTER
     static patternState_t last_state = START_P;
     static uint8_t last_cone = 255;
     static unsigned long start;
@@ -417,37 +621,11 @@
   default:
     break;
   }  
-
+    #endif
 }
 #endif
 
-#if 0
-void clearCones()
-{
-    
-}
-
-void find_cones()
-{
-    
-}
-
-void powerupCones(unsigned char c)
-{
-    
-}
-
-void resetSensors()
-{
-    
-}
-
-void printMsAsSeconds(unsigned long)
-{
-    
-}
-#endif
-
+#ifdef MASTER
 void getRadioInput(char *ibuffer, int size)
 {
   static int i = 0;
@@ -497,7 +675,7 @@
   }
 }
 
-void interpret(char parameter, int value)
+static void interpret(char parameter, int value)
 {
   int remainder;
   uint16_t split;
@@ -601,8 +779,8 @@
   case 'q':
     state_p = IDLE_P;
     new_state = true; // force state reporting, even if we're already in IDLE
-    ta.pulse_off();
-    clearCones();
+    //ta.pulse_off();
+    //clearCones();
     break;
   case 'r':
     //Serial.println(F(""));
@@ -689,8 +867,8 @@
     break;
   }
 }
-#if 1
-patternState_t stateFromCone(uint8_t cone)
+
+static patternState_t stateFromCone(uint8_t cone)
 {
   if(cone == 0 || index == STATIONS) 
     return SUCCESS_P;
@@ -700,7 +878,7 @@
   return WAITING_P;
 }
 
-void printSplit(unsigned long timer)
+static void printSplit(unsigned long timer)
 {
   if(tag_start)
   {
@@ -721,7 +899,7 @@
   }
 }
 
-uint8_t getRandomCone(void)
+static uint8_t getRandomCone(void)
 {
   static uint8_t lastCone = 0;
   uint8_t cone;
@@ -738,8 +916,9 @@
   return cone;
 }
 
-void clearCones(void)
+static void clearCones(void)
 {
+  #if 0
   uint8_t i;
   m->command = 'q';
   
@@ -753,10 +932,12 @@
   }
   
   DEBUG("sent clear\r\n"); 
+  #endif
 }
 
-void powerupCones(uint8_t sound)
+static void powerupCones(uint8_t sound)
 {
+  #if 1
   uint8_t i;
   m->command = 'u';
   m->value = sound;
@@ -783,12 +964,13 @@
       //ta.send("f", i);
     }
   }
-  
+  #endif
   //DEBUG("sent powerup");
 }
 
-void failCones(void)
+static void failCones(void)
 {
+  #if 0
   uint8_t i;
   m->command = 'f';
   
@@ -807,10 +989,12 @@
   }
   
   //DEBUG("sent fail\n");
+  #endif
 }
 
-void resetSensors(void)
+static void resetSensors(void)
 {
+  #if 0
   uint8_t i;
   m->command = 'x';
   
@@ -823,10 +1007,12 @@
   }
   
   DEBUG("sent sensor reset\n");
+  #endif
 }
 
-void successCones(void)
+static void successCones(void)
 {
+  #if 0
   uint8_t i;
   m->command = 's';
   
@@ -844,21 +1030,25 @@
   }
   
   //DEBUG("sent success\n");
+  #endif
 }
 
-void find_cones(void)
+static void find_cones(void)
 {
-
+#if 1
   while(ta.get_buffer_size())
+  {
     ta.spin(); // wait for all messages to leave queue
+    }
   
+  ta.beep_off();
   uint8_t i;
   m->command = 'z';
 
   lonely = true;
   
-  DEBUG("Finding cones\r\n");
-  
+  writeToPhone("Finding cones\r\n");
+  //for (i=3; i <=3; ++i)
   for(i=2;i<NUM_CONES+1;i++)
   {
     active_cones[i] = false;
@@ -866,22 +1056,30 @@
     ta.send_immediate(m);
 
     unsigned long st = millis();
+    unsigned long current = 0;
     unsigned long delta = 0;
     
+    writeToPhone("start: %d\r\n", st);
+
     while(1)
     {
-      delta = millis() - st;
+      current = millis();
+      delta = current - st;
       
-      if(delta > 50) 
+      if(delta > 5000L) 
+      {
+        writeToPhone("Find timeout for: %d\r\n", i);
         break;
+        }
         
-      ta.spin();
+      //ta.spin();
       
       if(ta.recieve(m_in))
       {
         lonely = false;
         active_cones[m_in->cone] = true;
         
+        writeToPhone("Reply from: %d\r\n", m_in->cone);
         DEBUG("Reply from cone: ");
         DEBUG("%d",m_in->cone);
         DEBUG(", ");
@@ -890,9 +1088,9 @@
         break;
       }
     }
-    
   }
   
+  /*
   DEBUG("available cones are: (1");
   
   for(i=2;i<NUM_CONES+1;i++)
@@ -904,9 +1102,11 @@
     }
   }
   DEBUG(")\r\n");
+  */
+  #endif
 }
 
-void printMsAsSeconds(unsigned long number)
+static void printMsAsSeconds(unsigned long number)
 {
   uint16_t remainder;
   
@@ -928,7 +1128,7 @@
   writeToPhone( "%d.%03d", number/1000, remainder );
 }
 
- void spinButtons(void)
+ static 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)
@@ -1075,7 +1275,7 @@
   }
 }
 
-uint8_t checkButtons(void)
+static uint8_t checkButtons(void)
 {
   static unsigned long last_time = 0;
   //static uint8_t last = 0;
@@ -1093,8 +1293,7 @@
   return buttons;
 }
 
-
-Event getInputEvent(void)
+static Event getInputEvent(void)
 {
   static uint8_t last_buttons = 0;
   static unsigned long time1 = 0;