Diff: TA.cpp

Revision:

23:26f27c462976

Parent:

18:affef3a7db2a

Child:

25:99f1399d40b3

diff -r afcbb425b3cf -r 26f27c462976 TA.cpp
--- a/TA.cpp	Tue Dec 01 16:03:15 2015 +0000
+++ b/TA.cpp	Mon Dec 14 14:24:10 2015 +0000
@@ -4,6 +4,7 @@
 #include <mbed.h>
 #include <device.h>
 #include <vector> 
+#include <RFM69.h>
 
 /* !SR
  * Can't find any boards which use the nrf51822 and have analog output enabled. Currently
@@ -20,12 +21,18 @@
 #else
 #define DEBUG(...) /* nothing */
 #endif /* #if NEED_CONSOLE_OUTPUT */
+extern "C" void writeToPhone(char *format, ...);
  
 
 extern unsigned long millis();
 extern unsigned long micros();
 extern int random(int numberone, int numbertwo);
 
+extern void radio_send(Message *m);
+extern bool radio_receive(Message *m);
+extern bool radio_ack_received(int cone);
+extern bool radio_receive_complete();
+
 ByteBuffer TA::send_buffer;
 ByteBuffer TA::receive_buffer;
 
@@ -54,6 +61,7 @@
 
 unsigned long micros();
 
+extern RFM69 radio;
 
 /** Macro for min() 
  *
@@ -111,10 +119,10 @@
 DigitalOut TA::enable_1(p4);
 DigitalOut TA::enable_2(p7);
 DigitalOut TA::enable_3(p8);
-
+#endif
 
-DigitalOut TA::cap_enable(A3);
-#endif
+//DigitalOut TA::cap_enable(p1);
+
 DigitalOut TA::buzzPin(p20);
 
 #if 0
@@ -124,17 +132,13 @@
 DigitalIn TA::touch_3(A2);
 #endif
 
-std::vector<Message*> *messages;
-
 //NeoStrip *neo;
 
 TA::TA()
 {
-    buzzPin = 1;
+    buzzPin = 0;
     neopixel_init(&m_strip, dig_pin_num, leds_per_strip);
-    neopixel_clear(&m_strip);
-    messages = new std::vector<Message*>();
-    
+    neopixel_clear(&m_strip); 
 }
 
 void TA::post_color(uint32_t rgb)
@@ -167,7 +171,7 @@
   
   for (int i = 0; i <= leds_per_strip; ++i)
     {
- //       neopixel_set_color_and_show(&m_strip, i, (rgb >> 16) & 0xFF, (rgb >> 8) & 0xFF, rgb & 0xFF);
+          neopixel_set_color_and_show(&m_strip, i, (rgb >> 16) & 0xFF, (rgb >> 8) & 0xFF, rgb & 0xFF);
     } 
 }
 
@@ -232,21 +236,7 @@
 
 void TA::send_immediate(Message *m)
 {
-    Message *tmp = new Message;
-    DEBUG("in send_imediate: %d '%c'\n", m->cone, m->command);
-    
-    tmp->command = m->command;
-    tmp->cone    = m->cone;
-    
-    // Fake version of the function, push this onto the reply queue
-    if (messages->size() < 50)
-    {
-        messages->push_back(tmp);
-    }
-    else
-    {
-        DEBUG("TA message queue too full!\n");    
-    }
+    radio_send(m);
 }
 
 bool TA::sendRaw(uint8_t *message, uint8_t len, uint8_t cone)
@@ -257,240 +247,184 @@
 
 bool TA::recieve(Message *m)
 {
-   bool processed = true;
-top:
-    processed = true;
-
-   //DEBUG("receiving\n");
-   if (messages->size() > 0)
-   {
-       Message *lm = messages->front();
-       
-       DEBUG("got messaage with command: '%c'\n", lm->command);
-       
-       // Now populate the outgoing message based on the incoming
-       if (lm->command == 'z')
-       {
-            m->cone = lm->cone;   
-        } 
-        else
-        {
-            DEBUG("Unknown command: '%c'\r\n", lm->command);   
-            processed = false; 
-        }
-        
-        messages->erase(messages->begin());
-        
-        if (lm)
-        {
-           delete lm;    
-        }
-        
-        if (!processed)
-        {
-            // Clear out the queue if we don't know what
-            // to do with the message
-            goto top;    
-        }
-   }
-   else
-   {
-        processed = false;
-    }
-    
-    return processed;   
+    return radio_receive(m);
 }
 
 bool TA::waitForAck(int cone) 
 {
-   
-    return true;
+    long start = micros();
+    long timeout = ack_time;// + random(0,2000);
+  while (micros() - start < timeout){
+    if (radio_ack_received(cone)){
+      //Serial.println(millis() - now);
+      return true;
+    }
+  }
+  //Serial.println(millis() - start);
+  return false;
 }
 
 void TA::spin(void)
 {
-    #if 0
-    static byte payload [6];
-    static bool message_in_queue = false;
-    static bool waiting_for_ack = false;
-    static byte dest_cone = 0;
-    static uint16_t index = 0;
-    static uint16_t ack_tries = 0;
-    static uint8_t send_tries = 0;
-    static unsigned long ack_start = 0;
-    static unsigned long random_wait = 0;
-    static unsigned long random_wait_start = 0;
-    static unsigned long mem_monitor_start = 0;
+  static byte payload [6];
+  static bool message_in_queue = false;
+  static bool waiting_for_ack = false;
+  static byte dest_cone = 0;
+  static uint16_t index = 0;
+  static uint16_t ack_tries = 0;
+  static uint8_t send_tries = 0;
+  static unsigned long ack_start = 0;
+  static unsigned long random_wait = 0;
+  static unsigned long random_wait_start = 0;
+  static unsigned long mem_monitor_start = 0;
 
-    // if(millis() - mem_monitor_start > 2000){
-    //   mem_monitor_start = millis();
-    //   Ram_TableDisplay();
-    // }
-
-#if 0
-    if(tripped())
-    {//buttons() && millis() - activated_start > 1000){
-        cap_enable = 1;
-        wait_ms(100);
-        cap_enable = 0;
-        //activated_start = millis();
-        //Serial.println(F("toggled cap sense power"));
+  if(tripped())
+  {
+    //cap_enable = 1;
+    //wait_ms(100);
+    //cap_enable = 0;
+    //activated_start = millis();
+    //Serial.println(F("toggled cap sense power"));
     }
-#endif    
-    if(powering_up2)
+  if(powering_up2)
+  {
+    unsigned long t = millis() - powerup_start;
+    if(t > powerup_toggle)
+    {
+      buzzPin = !buzzPin;
+      powerup_toggle *= 1.2;
+    }
+    if(t > 1250)
     {
-        unsigned long t = millis() - powerup_start;
-        
-        if(t > powerup_toggle)
-        {
-            buzzPin = !buzzPin;
-            powerup_toggle *= 1.2;
-        }
-        
-        if(t > 1250)
-        {
-            buzzPin = 0;
-            powering_up2 = false;
-        }
+      buzzPin = 0;
+      powering_up2 = false;
+    }
+  }
+  else if(powering_up1)
+  {
+    unsigned long t = millis() - powerup_start;
+    if(t > powerup_toggle)
+    {
+      buzzPin = !buzzPin;
+      powerup_toggle *= 0.95;
+      powerup_start = millis();
     }
-    else if(powering_up1)
+    if(powerup_toggle < 10)
     {
-        unsigned long t = millis() - powerup_start;
-        
-        if(t > powerup_toggle)
-        {
-            buzzPin = !buzzPin;
-            powerup_toggle *= 0.95;
-            powerup_start = millis();
-        }
-    
-        if(powerup_toggle < 10)
-        {
-            buzzPin = 0;
-            powering_up1 = false;
-        }
+      buzzPin = 0;
+      powering_up1 = false;
+    }
+  }
+  else
+  {
+    if(beeping && (millis()-beep_start > beep_duration)) 
+        beeping = false;
+    if(pulsing && (millis()-pulse_start > pulse_duration)) 
+        pulsing = false;
+    if(beeping)
+        buzzPin = 1;
+    else if( pulsing && (((millis()-pulse_start) % pulse_period) < pulse_on))
+    {
+      if(!(mask & SILENT))
+        buzzPin = 1;
+      mask_color(0);
     }
     else
     {
-        if(beeping && (millis()-beep_start > beep_duration)) 
-            beeping = false;
-        if(pulsing && (millis()-pulse_start > pulse_duration)) 
-            pulsing = false;
-        if(beeping)
-            buzzPin = 1;
-        else if( pulsing && (((millis()-pulse_start) % pulse_period) < pulse_on))
-        {
-            if(!(mask & SILENT))
-                buzzPin = 1;
-            mask_color(0);
-        }
-        else
-        {
-            if(pulsing)
-                mask_color(current_color);
-            buzzPin = 0;
-        }
+      if(pulsing)mask_color(current_color);
+      buzzPin = 0;
     }
-/*
-    if(!waiting_for_ack && radio.ReceiveComplete())
+  }
+
+  if(!waiting_for_ack && radio_receive_complete())
+  {
+    //if(radio.CRCPass())
     {
-        if(radio.CRCPass())
-        {
-            receive_buffer.put(radio.GetSender());
-            for(byte i = 0; i < *radio.DataLen; i++)
-                receive_buffer.put(radio.Data[i]);
-        }
-       
-        if(radio.ACKRequested())
-        {
-            wait_us(1300);
-            radio.SendACK();
-            //Serial.println(F("Sent ACK"));
-        }
+      receive_buffer.put(radio.SENDERID);
+      
+      for(byte i = 0; i < radio.DATALEN; i++)
+      {
+        receive_buffer.put(radio.DATA[i]);
+      }
     }
-    */
-
-    //if(index > 4999 || waiting_for_ack){
-    if(waiting_for_ack)
+    
+    if(radio.ACKRequested())
     {
-        //Serial.println(F("Waiting for ack"));
-        bool success = radio.ACKReceived(dest_cone);
-        
-        if(success || send_tries > 15)
-        {
-            //Serial.print(F("dequeued: "));
-            //Serial.println((uint16_t)payload[1]<<8 + payload[2]);
-            message_in_queue = false;
-            waiting_for_ack = false;
-            unsigned long t = micros() - ack_start;
-            //Serial.print(F("Received ACK, microseconds: "));
-            //Serial.println(t);
-            //ack_tries = 0;
-          
-            message_in_queue = false;
-            send_tries = 0;
-                
-            /*Serial.print(F("Sent "));
-            Serial.print((char)payload[0]);
-            Serial.print(F(", to cone "));
-            Serial.println(dest_cone);*/
-        }
-        else
-        {
-            if(micros() - ack_start > 10000)
-            {
-                //Serial.println(F("no ACK"));
-                waiting_for_ack = false;
-                random_wait_start = micros();
-                random_wait = random(1500,3000);
-                /*if(send_tries > 15){
-                Serial.print(F("Failed to deliver message to cone "));
-                Serial.println(dest_cone);
-                message_in_queue = false;
-                send_tries = 0;
-                }*/
-                if(send_tries > 4) 
-                    random_wait = random(3000,9000);
-                //Serial.print(F("Failed to deliver message, waiting "));
-                //Serial.println(random_wait);
-                //ack_tries = 0;
+      wait_ms(1300);
+      radio.sendACK();
+      //Serial.println(F("Sent ACK"));
+    }
+  }
+
+  //if(index > 4999 || waiting_for_ack){
+  if(waiting_for_ack){
+    //Serial.println(F("Waiting for ack"));
+    bool success = radio.ACKReceived(dest_cone);
+    if(success || send_tries > 15){
+      //Serial.print(F("dequeued: "));
+      //Serial.println((uint16_t)payload[1]<<8 + payload[2]);
+      message_in_queue = false;
+      waiting_for_ack = false;
+      unsigned long t = micros() - ack_start;
+      //Serial.print(F("Received ACK, microseconds: "));
+      //Serial.println(t);
+      //ack_tries = 0;
+ 
+      message_in_queue = false;
+      send_tries = 0;
+      /*Serial.print(F("Sent "));
+    Serial.print((char)payload[0]);
+    Serial.print(F(", to cone "));
+    Serial.println(dest_cone);*/
+    }
+    else{
+      if(micros() - ack_start > 10000){
+    //Serial.println(F("no ACK"));
+    waiting_for_ack = false;
+    random_wait_start = micros();
+    random_wait = random(1500,3000);
+    /*if(send_tries > 15){
+      Serial.print(F("Failed to deliver message to cone "));
+      Serial.println(dest_cone);
+      message_in_queue = false;
+      send_tries = 0;
+      }*/
+    if(send_tries > 4) random_wait = random(3000,9000);
+    //Serial.print(F("Failed to deliver message, waiting "));
+    //Serial.println(random_wait);
+    //ack_tries = 0;
     
-                /*if(send_tries%50 == 0){
-                Serial.print(send_tries);
-                Serial.println(F(" tries"));
-                }*/
-            }
-        }
-        // ack_tries++;
+    /*if(send_tries%50 == 0){
+      Serial.print(send_tries);
+      Serial.println(F(" tries"));
+      }*/
+      }
     }
-    else if(message_in_queue && micros() - random_wait > random_wait_start)
-    {// && index%64 == 0){
-        requestACK = true;
-        //Serial.println(F("sending"));
-        radio.Send(dest_cone, payload, 6, requestACK);
-        //Serial.println(F("sent"));
-        //Serial.print(F("Trying to send: "));
-        //uint16_t temp = (uint16_t)payload[1]<<8;
-        //temp += (uint8_t)payload[2];
-        //Serial.println((uint8_t)payload[0]);
-        send_tries++;
-        ack_start = micros();
-        
-        if(!radio.ACKReceived(dest_cone)) 
-            waiting_for_ack = true; // the 'if' is here to prevent the radio from going to sleep and missing the ACK
-        else 
-            message_in_queue = false;
-    }
-    else 
-    {
+    // ack_tries++;
+  }
+  else if(message_in_queue && micros() - random_wait > random_wait_start){// && index%64 == 0){
+    requestACK = true;
+    //Serial.println(F("sending"));
+    writeToPhone("Sending message to: %d\r\n", dest_cone);
+    radio.send(dest_cone, payload, 6, requestACK);
+    //Serial.println(F("sent"));
+    //Serial.print(F("Trying to send: "));
+    //uint16_t temp = (uint16_t)payload[1]<<8;
+    //temp += (uint8_t)payload[2];
+    //Serial.println((uint8_t)payload[0]);
+    send_tries++;
+    ack_start = micros();
+    if(!radio.ACKReceived(dest_cone)) waiting_for_ack = true; // the 'if' is here to prevent the radio from going to sleep and missing the ACK
+    else message_in_queue = false;
+  }
+  else {
     /*if(send_buffer.getSize() > 0 && message_in_queue == false){
       payload[0] = send_buffer.get();
       //Serial.print(F("Got from queue: "));
       //Serial.println((char)payload[0]);
       message_in_queue = true;
     }*/
-    while(send_buffer.getSize() > 4 && message_in_queue == false)
-    {
+    while(send_buffer.getSize() > 4 && message_in_queue == false){
       payload[0] = send_buffer.get();
       payload[1] = send_buffer.get();
       payload[2] = send_buffer.get();
@@ -508,33 +442,51 @@
       Serial.println(dest_cone, BIN);      
       Serial.println(payload[5], BIN);
       Serial.println(F(""));*/
-      if((char)payload[5] == '%')
-        message_in_queue = true;
-      else 
-      {
-        while(send_buffer.getSize() > 0 && send_buffer.get() != '%'); // if we didn't land on the end of a message, peel stuff off until we are
+      if((char)payload[5] == '%')message_in_queue = true;
+      else {
+    //Serial.println(F("bad message format"));
+    while(send_buffer.getSize() > 0 && send_buffer.get() != '%'); // if we didn't land on the end of a message, peel stuff off until we are
       }
     }
   }
-  
-  #endif
 }
 
 bool TA::activated(void)
 {
-    return true;   
+    return (buttons() & mask)?true:false; 
 }
 
 bool TA::tripped(void)
 {
-    return false;
+  // detect if something is staying on the cap sensor (i.e. the floor is causing the sensor to overload)
+  // in some cases the sensor output is choppy, so we must detect those situations also.
+  uint8_t current = 0;
+  static bool sensor_hysteresis = false;
+  static uint8_t last;
+  static unsigned long activated_start;
+  static unsigned long sensor_hyst_start;
+
+  current = buttons();
+  if(current && !last && (millis() - sensor_hyst_start > 250)){
+     activated_start = millis();
+     //Serial.println(current | 0b10000000, BIN);
+  }
+  if(!current && last) sensor_hyst_start = millis();
+  last = current;
+
+  return (current && (millis() - activated_start > 1000))?true:false;
+
 }
 
 uint8_t TA::buttons(void)
 {
-    uint8_t buttons = 0;
+  uint8_t buttons = 0;
 
-    return buttons;  
+  //buttons |= touch_1?1:0;
+  //buttons |= touch_2?2:0;
+  //buttons |= touch_3?4:0;
+  return buttons;
+
 }
 
 void TA::setMask(uint8_t the_mask)