REV7 stack overflow debugging

REV7 stack overflow debugging

- All changes for testing marked XXX
- Likely OTAESGCM using ~220 bytes more stack than is apparent.

--- Arduino/hardware/V0p2_Main_PCB_REV7_DORM1_and_REV8/REV7RadValve/REV7RadValve.ino
+++ Arduino/hardware/V0p2_Main_PCB_REV7_DORM1_and_REV8/REV7RadValve/REV7RadValve.ino
@@ -139,14 +139,7 @@
 static constexpr uint8_t RFM23B_RX_QUEUE_SIZE = OTV0P2BASE::fnmax(uint8_t(2), uint8_t(OTRFM23BLink::DEFAULT_RFM23B_RX_QUEUE_CAPACITY)) - 1;
 static constexpr int8_t RFM23B_IRQ_PIN = PIN_RFM_NIRQ;
 static constexpr bool RFM23B_allowRX = false;
-OTRFM23BLink::OTRFM23BLink<OTV0P2BASE::V0p2_PIN_SPI_nSS, RFM23B_IRQ_PIN, RFM23B_RX_QUEUE_SIZE, RFM23B_allowRX> RFM23B;
-
-// Assigns radio to PrimaryRadio alias
-OTRadioLink::OTRadioLink &PrimaryRadio = RFM23B;
-
-// When RX not enabled, switch in dummy version (base class implements stubs)
-OTRadioLink::OTMessageQueueHandlerNull messageQueue;  // fixme delete
-
+OTRFM23BLink::OTRFM23BLink<OTV0P2BASE::V0p2_PIN_SPI_nSS, RFM23B_IRQ_PIN, RFM23B_RX_QUEUE_SIZE, RFM23B_allowRX> PrimaryRadio;

 ////// SENSORS

@@ -215,7 +208,6 @@
 // Singleton scheduler instance.
 // Dummy scheduler to simplify coding.
 typedef OTRadValve::NULLValveSchedule Scheduler_t;
-extern Scheduler_t Scheduler;
 Scheduler_t Scheduler;

 // Radiator valve mode (FROST, WARM, BAKE).
@@ -225,7 +217,6 @@
 typedef OTRadValve::DEFAULT_ValveControlParameters PARAMS;
 // Choose which subtype to use depending on enabled settings and board type.
 typedef OTRadValve::TempControlTempPot<decltype(TempPot), &TempPot, PARAMS, decltype(RelHumidity), &RelHumidity> TempControl_t;
-#define TempControl_DEFINED
 // Temperature control object.
 TempControl_t tempControl;

@@ -251,8 +242,6 @@
   setbackLockout>
   cttBasic;

-#define ENABLE_MODELLED_RAD_VALVE
-#define ENABLE_NOMINAL_RAD_VALVE
 // Internal model of controlled radiator valve position.
 OTRadValve::ModelledRadValve NominalRadValve(
   &cttBasic,
@@ -413,7 +402,7 @@
 // Will be run after all stats for the current hour have been updated.
 static void endOfDayTasks() {
     // Count down the setback lockout if not finished...  (TODO-786, TODO-906)
-    OTRadValve::countDownSetbackLockout();
+//    OTRadValve::countDownSetbackLockout();
 }

@@ -428,8 +417,10 @@
 // If sending encrypted then ID/counter fields (eg @ and + for JSON) are omitted
 // as assumed supplied by security layer to remote recipent.
 void bareStatsTX() {
-    // Capture heavy stack usage from local allocations here.
-    OTV0P2BASE::MemoryChecks::recordIfMinSP();
+
+    OTV0P2BASE::MemoryChecks::resetMinSP();
+//    // Capture heavy stack usage from local allocations here.
+//    OTV0P2BASE::MemoryChecks::recordIfMinSP();
     const bool neededWaking = OTV0P2BASE::powerUpSerialIfDisabled<>();
     static_assert(OTV0P2BASE::FullStatsMessageCore_MAX_BYTES_ON_WIRE <= STATS_MSG_MAX_LEN, "FullStatsMessageCore_MAX_BYTES_ON_WIRE too big");
     static_assert(OTV0P2BASE::MSG_JSON_MAX_LENGTH+1 <= STATS_MSG_MAX_LEN, "MSG_JSON_MAX_LENGTH too big"); // Allow 1 for trailing CRC.
@@ -543,6 +534,7 @@
     // Send directly to the primary radio...
     if((!sendingJSONFailed) && (!PrimaryRadio.queueToSend(realTXFrameStart, wrote))) { sendingJSONFailed = true; }
     if(neededWaking) { OTV0P2BASE::flushSerialProductive(); OTV0P2BASE::powerDownSerial(); }
+    stackCheck();
 }

@@ -568,7 +560,7 @@

 void pollCLI(const uint8_t maxSCT, const bool startOfMinute, const OTV0P2BASE::ScratchSpace &s)
 {
-    // Perform any once-per-minute operations. // XXX
+    // Perform any once-per-minute operations.
     if(startOfMinute) { OTV0P2BASE::CLI::countDownCLI(); }
     const bool neededWaking = OTV0P2BASE::powerUpSerialIfDisabled<V0P2_UART_BAUD>();
     // Wait for input command line from the user (received characters may already have been queued)...
@@ -582,50 +574,50 @@
         OTV0P2BASE::CLI::resetCLIActiveTimer();
         // Process the input received, with action based on the first char...
         switch(buf[0]) {
-            // Explicit request for help, or unrecognised first character.
-            // Avoid showing status as may already be rather a lot of output.
-            default: case '?': { dumpCLIUsage(); break; }
-            // Exit/deactivate CLI immediately.
-            // This should be followed by JUST CR ('\r') OR LF ('\n')
-            // else the second will wake the CLI up again.
-            case 'E': { OTV0P2BASE::CLI::makeCLIInactive(); break; }
-            // Show/set generic parameter values (eg "G N [M]").
-            case 'G': { OTV0P2BASE::CLI::GenericParam().doCommand(buf, n); break; }
-            // Reset or display ID.
-            case 'I': { OTV0P2BASE::CLI::NodeIDWithSet().doCommand(buf, n); break; }
-            // Status line stats print and TX.
-            case 'S': {
-                Serial.print(F("Resets: "));
-                const uint8_t resetCount = eeprom_read_byte((uint8_t *)V0P2BASE_EE_START_RESET_COUNT);
-                Serial.print(resetCount);
-                Serial.println();
-                // Show stack headroom.
-                OTV0P2BASE::serialPrintAndFlush(F("SH ")); OTV0P2BASE::serialPrintAndFlush(OTV0P2BASE::MemoryChecks::getMinSPSpaceBelowStackToEnd()); OTV0P2BASE::serialPrintlnAndFlush();
-                // Default light-weight print and TX of stats.
-//                bareStatsTX();  // FIXME No way this won't overflow!
-                break; // Note that status is by default printed after processing input line.
-            }
-            // Switch to FROST mode OR set FROST/setback temperature (even with temp pot available).
-            // With F! force to frost and holiday (long-vacant) mode.  Useful for testing and for remote CLI use.
-            case 'F': {
-                valveMode.setWarmModeDebounced(false); // No parameter supplied; switch to FROST mode.
-                break;
-            }
-            /**
-            * Set secret key.
-            * @note  The OTRadioLink::SimpleSecureFrame32or0BodyTXV0p2::resetRaw3BytePersistentTXRestartCounterCond
-            *        function pointer MUST be passed here to ensure safe handling of the key and the Tx message
-            *        counter.
-            */
-            case 'K': { OTV0P2BASE::CLI::SetSecretKey(OTRadioLink::SimpleSecureFrame32or0BodyTXV0p2::resetRaw3BytePersistentTXRestartCounterCond).doCommand(buf, n); break; }
-            // Switch to WARM (not BAKE) mode OR set WARM temperature.
-            case 'W':{
-                valveMode.cancelBakeDebounced(); // Ensure BAKE mode not entered.
-                valveMode.setWarmModeDebounced(true); // No parameter supplied; switch to WARM mode.
-                break;
-            }
-            // Zap/erase learned statistics.
-            case 'Z': { OTV0P2BASE::CLI::ZapStats().doCommand(buf, n); break; }
+//            // Explicit request for help, or unrecognised first character.
+//            // Avoid showing status as may already be rather a lot of output.
+//            default: case '?': { dumpCLIUsage(); break; }
+//            // Exit/deactivate CLI immediately.
+//            // This should be followed by JUST CR ('\r') OR LF ('\n')
+//            // else the second will wake the CLI up again.
+//            case 'E': { OTV0P2BASE::CLI::makeCLIInactive(); break; }
+//            // Show/set generic parameter values (eg "G N [M]").
+//            case 'G': { OTV0P2BASE::CLI::GenericParam().doCommand(buf, n); break; }
+//            // Reset or display ID.
+//            case 'I': { OTV0P2BASE::CLI::NodeIDWithSet().doCommand(buf, n); break; } // XXX
+//            // Status line stats print and TX.
+//            case 'S': {
+//                Serial.print(F("Resets: "));
+//                const uint8_t resetCount = eeprom_read_byte((uint8_t *)V0P2BASE_EE_START_RESET_COUNT);
+//                Serial.print(resetCount);
+//                Serial.println();
+//                // Show stack headroom.
+//                OTV0P2BASE::serialPrintAndFlush(F("SH ")); OTV0P2BASE::serialPrintAndFlush(OTV0P2BASE::MemoryChecks::getMinSPSpaceBelowStackToEnd()); OTV0P2BASE::serialPrintlnAndFlush();
+//                // Default light-weight print and TX of stats.
+////                bareStatsTX();  // FIXME No way this won't overflow!
+//                break; // Note that status is by default printed after processing input line.
+//            }
+//            // Switch to FROST mode OR set FROST/setback temperature (even with temp pot available).
+//            // With F! force to frost and holiday (long-vacant) mode.  Useful for testing and for remote CLI use.
+//            case 'F': {
+//                valveMode.setWarmModeDebounced(false); // No parameter supplied; switch to FROST mode.
+//                break;
+//            }
+//            /**
+//            * Set secret key.
+//            * @note  The OTRadioLink::SimpleSecureFrame32or0BodyTXV0p2::resetRaw3BytePersistentTXRestartCounterCond
+//            *        function pointer MUST be passed here to ensure safe handling of the key and the Tx message
+//            *        counter.
+//            */
+//            case 'K': { OTV0P2BASE::CLI::SetSecretKey(OTRadioLink::SimpleSecureFrame32or0BodyTXV0p2::resetRaw3BytePersistentTXRestartCounterCond).doCommand(buf, n); break; }  // XXX
+//            // Switch to WARM (not BAKE) mode OR set WARM temperature.
+//            case 'W':{
+//                valveMode.cancelBakeDebounced(); // Ensure BAKE mode not entered.
+//                valveMode.setWarmModeDebounced(true); // No parameter supplied; switch to WARM mode.
+//                break;
+//            }
+//            // Zap/erase learned statistics.
+//            case 'Z': { OTV0P2BASE::CLI::ZapStats().doCommand(buf, n); break; } // XXX
         }
         // Else show ack of command received.
         Serial.println(F("OK"));
@@ -717,7 +709,7 @@
     updateSensorsFromStats();

     OTV0P2BASE::MemoryChecks::resetMinSP();
-    OTV0P2BASE::MemoryChecks::recordIfMinSP();
+    OTV0P2BASE::MemoryChecks::recordIfMinSP(1);
     stackCheck();

     // Do early 'wake-up' stats transmission if possible // XXX
@@ -726,15 +718,15 @@
     // Attempt to maximise chance of reception with a double TX.
     // Assume not in hub mode (yet).
     // Send all possible formats, binary first (assumed complete in one message).
-//    bareStatsTX();  // XXX
+    bareStatsTX();  // XXX
     // Send JSON stats repeatedly (typically once or twice)
     // until all values pushed out (no 'changed' values unsent)
     // or limit reached.
-//    for(uint8_t i = 5; --i > 0; ) {
-//        ::OTV0P2BASE::nap(WDTO_120MS, false); // Sleep long enough for receiver to have a chance to process previous TX.
-//        bareStatsTX();  // XXX
-//        if(!ss1.changedValue()) { break; }
-//    }
+    for(uint8_t i = 5; --i > 0; ) {
+        ::OTV0P2BASE::nap(WDTO_120MS, false); // Sleep long enough for receiver to have a chance to process previous TX.
+        bareStatsTX();  // XXX
+        if(!ss1.changedValue()) { break; }
+    }
     // Start local counters in randomised positions to help avoid inter-unit collisions,
     // eg for mains-powered units starting up together after a power cut,
     // but without (eg) breaking any of the logic about what order things will be run first time through.
@@ -759,7 +751,7 @@
     const unsigned long usStart = micros();
 #endif
     OTV0P2BASE::MemoryChecks::resetMinSP();
-    OTV0P2BASE::MemoryChecks::recordIfMinSP();
+    OTV0P2BASE::MemoryChecks::recordIfMinSP(2);
     stackCheck();

     // Main loop for OpenTRV radiator control.
@@ -810,7 +802,7 @@
     TIME_LSD = newTLSD;
     // Reset and immediately re-prime the RTC-based watchdog.
     OTV0P2BASE::resetRTCWatchDog();
-    OTV0P2BASE::enableRTCWatchdog(true);
+    OTV0P2BASE::enableRTCWatchdog(false);

     // START LOOP BODY
     // ===============
@@ -842,7 +834,7 @@
             // Tasks that must be run every minute.
             ++minuteCount; // Note simple roll-over to 0 at max value.
             // Force to user's programmed schedule(s), if any, at the correct time.
-            Scheduler.applyUserSchedule(&valveMode, OTV0P2BASE::getMinutesSinceMidnightLT());
+//            Scheduler.applyUserSchedule(&valveMode, OTV0P2BASE::getMinutesSinceMidnightLT());  // XXX
             // Ensure that the RTC has been persisted promptly when necessary.
             OTV0P2BASE::persistRTC();
             // Run hourly tasks at the end of the hour.
@@ -926,8 +918,8 @@
             // Race-free.
             const uint_least16_t msm = OTV0P2BASE::getMinutesSinceMidnightLT();
             const uint8_t mm = msm % 60;
-            if(59 == mm) { statsU.sampleStats(true, uint8_t(msm / 60)); }
-            else if((statsU.maxSamplesPerHour > 1) && (29 == mm)) { statsU.sampleStats(false, uint8_t(msm / 60)); }
+//            if(59 == mm) { statsU.sampleStats(true, uint8_t(msm / 60)); }
+//            else if((statsU.maxSamplesPerHour > 1) && (29 == mm)) { statsU.sampleStats(false, uint8_t(msm / 60)); }
             break;
         }
     }
@@ -966,10 +958,12 @@
     // and which should also allow some energy-saving sleep.
     if(OTV0P2BASE::CLI::isCLIActive()) {
         const uint8_t stopBy = nearOverrunThreshold - 1;
-        char buf[BUFSIZ_pollUI]; // XXX
+        char buf[BUFSIZ_pollUI];
         OTV0P2BASE::ScratchSpace s((uint8_t*)buf, sizeof(buf));
         pollCLI(stopBy, 0 == TIME_LSD, s);
     }
+
+    panic();
 #if defined(EST_CPU_DUTYCYCLE)
     const unsigned long usEnd = micros();
     // Nominal loop time should be 2s x 1MHz clock, ie 2,000,000 if CPU running all the time.


Update ValuePush.ino

Update ValuePush.ino

--- examples/GettingStarted/ValuePush/ValuePush.ino
+++ examples/GettingStarted/ValuePush/ValuePush.ino
@@ -22,14 +22,14 @@
  *   http://playground.arduino.cc/Code/SimpleTimer
  *
  * App dashboard setup:
- *   Value Display widget on V5
+ *   Value Display widget attached to V5
  *
  **************************************************************/

 #include <SPI.h>
 #include <Ethernet.h>
 #include <BlynkSimpleEthernet.h>
-#include <SimpleTimer.h> //Download Simple Timer library here: https://github.com/jfturcot/SimpleTimer
+#include <SimpleTimer.h>

 // You should get Auth Token in the Blynk App.
 // Go to the Project Settings (nut icon).
@@ -45,17 +45,19 @@
   timer.setInterval(1000, sendUptime);
 }

-// A function that sends how many seconds Arduino is running
+// This function sends Arduino's up time every second to Virtual Pin (5).
+// In the app, Widget's reading frequency should be set to PUSH. This means
+// that you define how often to send data to Blynk App.
 void sendUptime()
 {
   // You can send any value at any time.
-  // Please don't send more that 10 values per second!
+  // Please don't send more that 10 values per second.
   Blynk.virtualWrite(5, millis()/1000);
 }

 void loop()
 {
-  Blynk.run();
-  timer.run();
+  Blynk.run(); // Initiates Blynk
+  timer.run(); // Initiates SimpleTimer
 }


fix code

fix code

--- little-switches/arduino/abbinamento/abbinamento.ino
+++ little-switches/arduino/abbinamento/abbinamento.ino
@@ -14,6 +14,15 @@
 #define tamburo A1

 PS2Keyboard keyboard;
+
+long t1 = 0;
+long t2 = 0;
+long t3 = 0;
+long t4 = 0;
+long t5 = 0;
+long t6 = 0;
+
+int tempo = 0;

 void setup() {
   delay(1000);
@@ -32,35 +41,50 @@
 }

 void loop() {
+  tempo = map(analogRead(pot),0,1023,50,200);
+  Serial.print("tempo: ");
+  Serial.print(tempo);
+  Serial.println(" ms");
+
   if (keyboard.available()) {

     char c = keyboard.read();

     if (c == PS2_LEFTARROW) {
-      digitalWrite
+      digitalWrite(zampaAS,HIGH);
+      t1 = millis();
+    } else if (c == PS2_UPARROW) {
+      digitalWrite(zampaAD,HIGH);
+      t2 = millis();
     } else if (c == PS2_RIGHTARROW) {
-      Serial.print("[Right]");
-    } else if (c == PS2_UPARROW) {
-      Serial.print("[Up]");
+      digitalWrite(zampaPS,HIGH);
+      t3 = millis();
     } else if (c == PS2_DOWNARROW) {
-      Serial.print("[Down]");
+      digitalWrite(zampaPD,HIGH);
+      t4 = millis();
+    } else if (c == 'p') {
+      digitalWrite(piede,HIGH);
+      t5 = millis();
+    } else if (c == 't') {
+      digitalWrite(tamburo,HIGH);
+      t6 = millis();
     }

-    }
+  }

   else {
     if ((millis() - t1) > tempo) {
-      digitalWrite(2,LOW);
+      digitalWrite(zampaAS,LOW);
     }
     if ((millis() - t2) > tempo) {
-      digitalWrite(4,LOW);
+      digitalWrite(zampaAD,LOW);
     }
     if ((millis() - t3) > tempo) {
-      digitalWrite(5,LOW);
+      digitalWrite(zampaPS,LOW);
     }
     if ((millis() - t4) > tempo) {
-      digitalWrite(6,LOW);
+      digitalWrite(zampaPD,LOW);
     }
     if ((millis() - t5) > tempo) {
       digitalWrite(7,LOW);
@@ -68,17 +92,5 @@
     if ((millis() - t6) > tempo) {
       digitalWrite(8,LOW);
     }
-    if ((millis() - t7) > tempo) {
-      digitalWrite(A0,LOW);
-    }
-    if ((millis() - t8) > tempo) {
-      digitalWrite(A1,LOW);
-    }
-    if ((millis() - t9) > tempo) {
-      digitalWrite(A2,LOW);
-    }
-    if ((millis() - t0) > tempo) {
-      digitalWrite(A3,LOW);
-    }
   }
 }


InertialSensors demo: Add labels to the serial output.

--- examples/InertialSensors/InertialSensors.ino
+++ examples/InertialSensors/InertialSensors.ino
@@ -1,9 +1,6 @@
 // This example reads the raw values from the LSM6DS33
 // accelerometer and gyro and and the LIS3MDL magnetometer on the
-// Romi 32U4, and sends those raw values to the computer.  To
-// view the raw numbers, you can use the Arduino IDE's Serial
-// Monitor.  To view a graph of the numbers, you can use the
-// Arduino IDE's Serial Plotter
+// Romi 32U4, and prints those raw values to the serial monitor.
 //
 // The accelerometer readings can be converted to units of g using
 // the conversion factors specified in the "Mechanical characteristics"
@@ -74,7 +71,7 @@
   lis3mdl.read();

   snprintf_P(report, sizeof(report),
-    PSTR("%6d %6d %6d    %6d %6d %6d    %6d %6d %6d"),
+    PSTR("A: %6d %6d %6d    M: %6d %6d %6d    G: %6d %6d %6d"),
     lsm6.a.x, lsm6.a.y, lsm6.a.z,
     lis3mdl.m.x, lis3mdl.m.y, lis3mdl.m.z,
     lsm6.g.x, lsm6.g.y, lsm6.g.z);


refactored Simple7SegmentDisplay code with flashing dot (ooh!)


hardware reset added

hardware reset added

--- Software/src/centauri_hardware/arduino/MainBoard/Serial.ino
+++ Software/src/centauri_hardware/arduino/MainBoard/Serial.ino
@@ -79,4 +79,9 @@
     poweroff();
   }

+  // RESET
+  else if (part1.equalsIgnoreCase("99")) {
+    resetFunc();
+  }
+
 }


Further improvements to final

Further improvements to final

--- turtleCode/arduino/final/LCD12864_test/LCD12864_test.ino
+++ turtleCode/arduino/final/LCD12864_test/LCD12864_test.ino
@@ -18,6 +18,15 @@

 ***********************************************/

+/*********************************
+Manual for sending math stuff:
+
+1: Serial receives an "s" as the first character to indicate a sum is coming.
+2:  first number after the s can be anything from 0 up to and including 9.
+    second number after the s should be either ":", ";" or "<". these signal +, - and * respectively.
+    third number after the s can be anything from 0 up to and including 9.
+*********************************/
+
 #include "LCD12864RSPI.h"
 #include "DFrobot_bmp.h"
 #include "DFrobot_char.h"
@@ -25,73 +34,92 @@
 #include <SPI.h>
 #include "MFRC522.h"

+#include <Servo.h>
+
 #define RST_PIN	        9
-#define SS_PIN		10
+#define SS_PIN		      10
+#define servopin        6
+
+#define mouth_open_pos    120
+#define mouth_closed_pos  40

 MFRC522 mfrc522(SS_PIN, RST_PIN);	// Create MFRC522 instance
 MFRC522::MIFARE_Key key;
 byte cardUID;

+Servo mouthservo; //our servo
+int servo_pos = 0; //var to store servo position

 unsigned char fuse[1024];
-void setup(){
-Serial.begin(9600);
-SPI.begin();
-mfrc522.PCD_Init();
-
-
-    for (byte i = 0; i < 6; i++) {
-        key.keyByte[i] = 0xFF;
-    }
+
+void setup(){ //////////////SETUP////////////////////////
+  Serial.begin(9600);
+  mouthservo.attach(servopin);
+  SPI.begin();
+  mfrc522.PCD_Init();
+  for (byte i = 0; i < 6; i++) {
+    key.keyByte[i] = 0xFF;
+  }

-readloop(100);
-   LCDA.initDriverPin(2,3,4);
+  readloop(100);
+  LCDA.initDriverPin(2,3,4);
   LCDA.Initialise(); // INIT SCREEN

+  openMouth();
+
   //Serial.println(F("boot completed"));
 }
-unsigned char* fuse3(const unsigned char* a ,const unsigned char* b,const unsigned char* c){
-
+
+void openMouth(){ //open the mouth
+  mouthservo.write(mouth_open_pos);
+}
+
+void closeMouth(){ //close the mouth
+  mouthservo.write(mouth_closed_pos);
+}
+
+
+/*unsigned char* fuse3(const unsigned char* a ,const unsigned char* b,const unsigned char* c){ /////////////////////////////////// IS THIS OBSOLETE? /////////
  unsigned char* fused =(unsigned char*)malloc(sizeof(char)*1024);
-  int i=0;
-  int ac=0;//a counter
-  int bc=0; //b counter
-  int cc=0;//c counter
-
-
-  for(int n=0;n<64;n++){
-    for(int k=0;k<5;k++){
-      memcpy_P(&fused[i],&a[ac],sizeof(char)*5);
-      ac++;
-       i++;
-    }
-
-        for(int k=0;k<5;k++){
+ int i=0;
+ int ac=0;//a counter
+ int bc=0; //b counter
+ int cc=0;//c counter
+
+ for(int n=0;n<64;n++){
+   for(int k=0;k<5;k++){
+     memcpy_P(&fused[i],&a[ac],sizeof(char)*5);
+     ac++;
+      i++;
+   }
+
+     for(int k=0;k<5;k++){
       memcpy_P(&fused[i],&b[bc],sizeof(char)*5);
       bc++;
-       i++;
-    }
-        for(int k=0;k<5;k++){
+      i++;
+     }
+     for(int k=0;k<5;k++){
       memcpy_P(&fused[i],&c[cc],sizeof(char)*5);
       cc++;
-       i++;
-    }
+      i++;
+     }
     fused[i]=0x00;
     i++;
   }
-
   return fused;
-}
-const unsigned char* getMatch(int input){
+}*/
+
+const unsigned char* getMatch(int input){ //getmatch function
  if(input>0||input<13){
   return index[input];
- }
-
-   return index[0];
-}
+ }
+ return index[0];
+}
+
+
 void displayfuse(int firstnumber, int operation , int secondnumber ){
  LCDA.CLEAR();
-   readloop(100);
+ readloop(100);

  const unsigned char* a = getMatch(firstnumber);
  const unsigned char* b = getMatch(operation);
@@ -99,10 +127,12 @@

 // const unsigned char* d = fuse3(a,b,c);
 // LCDA.DrawFullScreen(d);
-LCDA.DrawFullScreen_F3(a,b,c);
+  LCDA.DrawFullScreen_F3(a,b,c);
   readloop(5000);
 }
-void readrfid(){
+
+
+void readrfid(){ //read rfid ball!
     // Look for new cards
   if ( ! mfrc522.PICC_IsNewCardPresent()) {
     return;
@@ -151,7 +181,9 @@
     // Stop encryption on PCD
     mfrc522.PCD_StopCrypto1();
 }
-void dump_byte_array(byte *buffer, byte bufferSize) {
+
+
+void dump_byte_array(byte *buffer, byte bufferSize) { //dump byte array function
     for (byte i = 0; i < bufferSize; i++) {
         Serial.print(buffer[i] < 0x10 ? " 0" : " ");
         Serial.print(buffer[i], HEX);
@@ -159,27 +191,25 @@
 }

 void loop(){
- waiting();
-}
-void waiting(){
-
-   int r1=0;
-   int r2=0;
-   int r3=0;
-   int r4=0;
-   byte b1,b2,b3,b4;
-      if(Serial.available()>3){
-         b1=Serial.read();
-         b2=Serial.read();
-         b3=Serial.read();
-         b4=Serial.read();
-         if(b1=='s'){
-            displayfuse(b2-48,b3-48,b4-48);
-        }
-    }
-
-  readrfid();
-  }
+
+ mouthservo.write(40);
+
+ int r1=0;
+ int r2=0;
+ int r3=0;
+ int r4=0;
+ byte b1,b2,b3,b4;
+ if(Serial.available()>3){
+   b1=Serial.read();
+   b2=Serial.read();
+   b3=Serial.read();
+   b4=Serial.read();
+   if(b1=='s'){ //indicates a math problem is coming
+     displayfuse(b2-48,b3-48,b4-48); //fuses the three numbers after the s to a math problem.
+   }
+ }
+ readrfid();
+}

 void readloop(int length){
  long starttime= millis();


Added tools used.

Added tools used.

--- MagLabCoinOpAirconditioner/MagLabCoinOpAirconditioner.ino
+++ MagLabCoinOpAirconditioner/MagLabCoinOpAirconditioner.ino
@@ -1,3 +1,5 @@
+// Fubarino SD, Arduino IDE 1.6.7, chipKIT core 1.1.0
+
 #define COIN_COUNTER 0 //PIN_INT1

 volatile uint32_t pennies = 0;


Spelling corrections (#161)

Spelling corrections (#161)

--- examples/Advanced/Display/JpegDraw/JpegDraw.ino
+++ examples/Advanced/Display/JpegDraw/JpegDraw.ino
@@ -4,7 +4,7 @@

   M5.begin();

-  //Draw the jpeg file form TF card
+  //Draw the jpeg file "p2.jpg" from TF(SD) card
   M5.Lcd.setBrightness(200);
   M5.Lcd.drawJpgFile(SD, "/p2.jpg");
   // M5.Lcd.drawJpgFile(SD, "/p1.jpg", 20, 30, 40, 50);


Version 0.9!

Version 0.9!

Finally finished data transmission/reception code! A few other small
changes (see log for details).

--- FlightCode/FlightCode.ino
+++ FlightCode/FlightCode.ino
@@ -1,10 +1,12 @@
 /*
- * Notre Dame Rocket Team Roll Control Payload Master Code V. 0.8.3
+ * Notre Dame Rocket Team Roll Control Payload Master Code V. 0.9
  * Aidan McDonald, 2/1/17
  *
  * Most recent changes:
- * Fixed some errors with variable names
- * Reconfigured the Adafruit sensor variable types
+ * Moved the GPS enable code again, to <600 ft post-apogee
+ * Incorporated receiving data from the ground station
+    * Added two master flags to control sensor enabling and fin control disabling
+ * Revised the data transmission section to send altitude data when gps/gyro data isn't being sent
  *
  * To-dones:
   * Basic switch-case structure
@@ -15,14 +17,10 @@
   * Running-average calcs for critical sensor values and Simpson's Rule integration of gyro data
   * Multi-sensor verification of flight progress switch-case transitions (untested)
   * Github Sync system working
+  * Packet transmission/reception over radio (ADD THIS TO GROUND STATION CODE TOO!!)
  *
  * To-dos:
- *  THE CODE IS ALREADY TOO BIG!!!!!
-      *  This is a pretty serious problem, considering there is still a significant amount of code that needs to be written. Part of the problem is that the
-      *  Adafruit flight controller uses four different libraries, each of which eats up memory space. Maybe replace our flight sensor package? Otherwise,
-      *  we're going to need to severely ramp down the scale of our operations...
-  * Figure out what the ground station will be transmitting and configure the receiver accordingly
-  * Figure out what data the payload transmits pre-burnout
+  * Reconfigure the servo control for the new control scheme!!!!!!!
   * Revise and enhance the staging/thresholds, particularly burnout/apogee accel values
   * Reconfigure the SD datalogging section to allow for easy spreadsheet conversion
   * TEST EVERYTHING
@@ -32,9 +30,6 @@
    *  Servo Control
    *  Flight Staging
    *  Etc., etc.
- * Receive from ground station:
-    *  GPS Sleep
-    *  Fin Override (only to home)
  *
  */

@@ -58,7 +53,7 @@
 #define RFM95_INT 3
 #define RF95_FREQ 915.0 // Must match RX's freq; in this case must be either 915 or 868 MHz

-// Singleton instance of the radio driver
+// Single instance of the radio driver
 RH_RF95 rf95(RFM95_CS, RFM95_INT);

 #define GPSSerial Serial1
@@ -117,6 +112,8 @@
 const int LANDED_BARO_THRESHOLD = 6;
 const int MIN_ROLL_THRESHOLD = 0;

+const int GPS_BARO_THRESHOLD = 200; //600 feet/200m is when the recovery system deploys
+
 bool startRollFlag = false;
 bool endRollFlag = false; //Flags for tracking roll/counter-roll progress
 bool finState = false; //Variable for tracking current fin position
@@ -128,7 +125,10 @@
 bool sendFlag = true; //Flag for toggling send/receive mode; makes 2-way communication much easier
 bool radioWorkingFlag = true; //Flag to determine if radio initialized properly. If not, then it continues on without comms
 bool dataFlag = false; //Since SD saving and radio transmission occur in two different functions, the SD routine uses this flag to tell the radio routine if data is ready for transmission
-bool gpsOnFlag = false; //Flag to track whether GPS is currently operating/enabled
+bool gpsOnFlag = false; //Flag to determine whether GPS is currently operating/enabled
+
+bool masterEnableFlag = false; //Flag that puts the Arduino in/out of "sleep mode."
+bool finOverrideFlag = false; //Flag that acts as a "big red button" to stop the arduino's roll-control.

 void setup() {
@@ -177,23 +177,27 @@

 void loop() {

+  if(masterEnableFlag) //Only save/buffer data if we're actively sensing data
+  {
 /* Get a new sensor event */
   sensors_event_t mainEvent;
   gyro.getEvent(&mainEvent);
   accel.getEvent(&mainEvent); // Assume Z-axis is vertical for accel/gyro?
   bmp.getEvent(&mainEvent);
+
+  Record_Data(mainEvent);//Save sensor data (accel, baro, time, GPS) to the SD card
+  BufferUpdate(mainEvent);
+  }
+
   if(gpsOnFlag)
   char c = GPS.read(); //Must call GPS.read() at some point for data transmission to occur

   if(startTime != 0)
   flightTime = millis() - startTime; //Timer backup variable
-
-  Record_Data(mainEvent);//Save sensor data (accel, baro, time, GPS) to the SD card
+
   if(radioWorkingFlag) {
   Radio_Transmit(); //Transmit/receive select data
   }
-
-  BufferUpdate(mainEvent);

 switch(flightState) { //Switch statement for entire rocket flight
   /*
@@ -203,7 +207,7 @@
    */

 case waiting:
-if(accelAverage > LIFTOFF_ACCEL_THRESHOLD)
+if(accelAverage > LIFTOFF_ACCEL_THRESHOLD && masterEnableFlag)
 {
   flightState = launched;
   startTime = millis();
@@ -224,21 +228,25 @@
 if(accelAverage > APOGEE_ACCEL_THRESHOLD || flightTime > APOGEE_TIME_THRESHOLD) //Add a baro test here?
 {
   flightState = falling;
+}
+break;
+
+
+case falling:
+if(baroAverage < LANDED_BARO_THRESHOLD)
+{
+  flightState = landed;
+}
+
+if(baroAverage < GPS_BARO_THRESHOLD) //Enable GPS if we're low enough
+{
   GPS.begin(9600); //Initialize GPS; define output set and data rate
   GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
   GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
   gpsOnFlag = true;
 }
+
 break;
-
-
-case falling:
-if(baroAverage < LANDED_BARO_THRESHOLD)
-{
-  flightState = landed;
-}
-break;
-

 case landed:

@@ -383,42 +391,52 @@
       dataFlag = false; //Only turn off this flag if Arduino is actively sending data
       sendFlag = false; //Once we send data, wait for data to be received

-      uint8_t radioPacket[19]; //Buffer of bytes for radio transmission
-      int packetSize = 19; //Depending on the flight state, the actual packet size may change
+      uint8_t radioPacket[21]; //Buffer of bytes for radio transmission
+      int packetSize = 21; //Depending on the flight state, the actual packet size may change

       radioPacket[0] = flightState; //Start every packet with the current flight staging (lets the receiver know what data is going to come at the end of the packet)
-      radioPacket[1] = servoOnFlag;
-      radioPacket[2] = servoHomeFlag; //The next three items in each packet report the outputs to the servo.
-      radioPacket[3] = finState;
+      radioPacket[1] = masterEnableFlag;
+      radioPacket[2] = finOverrideFlag; //Report back on the perceived state of affairs with the critical flags, in order for adjustments to be made if necessary.
+      radioPacket[3] = 42; //Answer to life, the universe, and everything. Used to confirm packet validity.
+
+      radioPacket[4] = gpsOnFlag;
+      radioPacket[4] = servoOnFlag;
+      radioPacket[5] = servoHomeFlag; //The next items in each packet report the outputs to the servo and whether we are running GPS.
+      radioPacket[6] = finState;

 union{ //Float-to-byte-string converter, needed for GPS data
   float tempFloat;
   byte tempArray[3];
 } u;

-if(flightState > burnout)
+if(!masterEnableFlag) {
+  packetSize = 6; //If sensors aren't enabled yet, only send the above data
+}
+
+else if(gpsOnFlag)
 {
 u.tempFloat = gpsLatitude;
 for(int c=0; c<4; c++){
-  radioPacket[c+4] = u.tempArray[c];
+  radioPacket[c+7] = u.tempArray[c];
 }
 radioPacket[4] = gpsLatDirect; //N or S
 u.tempFloat = gpsLongitude;
 for(int c=0; c<4; c++){
-  radioPacket[(c+9)] = u.tempArray[c];
+  radioPacket[(c+11)] = u.tempArray[c];
 }
 radioPacket[9] = gpsLonDirect; //E or W
 u.tempFloat = gpsAltitude;
 for(int c=0; c<4; c++){
-  radioPacket[(c+14)] = u.tempArray[c];
-}
-radioPacket[18] = gpsFix;
-radioPacket[19] = gpsFixQuality; //Useful for determining valid/invalid GPS data; if fix or quality are 0 the data isn't reliable
+  radioPacket[(c+16)] = u.tempArray[c];
+}
+radioPacket[20] = gpsFix;
+radioPacket[21] = gpsFixQuality; //Useful for determining valid/invalid GPS data; if fix or quality are 0 the data isn't reliable
 }

 else if(flightState = burnout) {

-  radioPacket[4] = endRollFlag; //If we're in burnout, send the "end roll flag." It determines whether we send the running tally (float) or the current rotation speed (int)
+  radioPacket[7] = endRollFlag; //If we're in burnout, send the "end roll flag." It determines whether we send the running tally (float) or the current rotation speed (int)
+
 if(endRollFlag) {
 union { //Since we're sending an integer, we need a different-sized memory union to work with
   int tempInt;
@@ -426,32 +444,33 @@
 } uInt;

 uInt.tempInt = gyroData[2]; //Remember: THIS ASSUMES Z-AXIS IS VERTICAL!!!
-radioPacket[5] = uInt.tempArray[0];
-radioPacket[6] = uInt.tempArray[1];
-packetSize = 6;
+radioPacket[8] = uInt.tempArray[0];
+radioPacket[9] = uInt.tempArray[1];
+packetSize = 9;
 }

 else {
   u.tempFloat = rotationCounter;
 for(int c=0; c<4; c++){
-  radioPacket[(c+5)] = u.tempArray[c];
-}
-packetSize = 8;
-}
-
-}
-
-else { //If we're not in the burnout phase yet, then send... what?
-
-
-
-}
-
+  radioPacket[(c+8)] = u.tempArray[c];
+}
+packetSize = 11;
+}
+
+}
+
+else { //If we're not in the burnout phase and haven't turned on the GPS, then send current altitude
+u.tempFloat = baroData[2];
+for(int c=0; c<4; c++){
+  radioPacket[(c+7)] = u.tempArray[c];
+}
+packetSize = 10;
+}

 rf95.send((uint8_t *)radioPacket, packetSize); //Once packet has been constructed, transmit
 rf95.waitPacketSent(); //Short, necessary wait for packet transmission to complete
-  }
+}

  else { //Receive data mode

@@ -462,10 +481,19 @@
     uint8_t len = sizeof(buf); //Variables to contain received message
     if (rf95.recv(buf, &len)) //If message is intact; this call also fills buf and len with data
    {
-    /*
-     * This is where code should go to do things or set flags based on input from the ground station.
-     * Update when you've figured out what goes here!!!!!
-     */
+
+    //Two very important flags from the ground station, sent in triplicate.
+    //masterEnableFlag enables the flight sensors to begin liftoff detection; finOverrideFlag acts as a master interrupt to halt in-flight fin rotation.
+
+    if(buf[0] || buf[1] || buf[2])
+        masterEnableFlag = true;
+      else
+        masterEnableFlag = false;
+
+     if(buf[3] || buf[4] || buf[5])
+        finOverrideFlag = true;
+     else
+        finOverrideFlag = false;
    }
   }
   }


Improve clock handling.

Improve clock handling.

--- examples/seq/seq.ino
+++ examples/seq/seq.ino
@@ -25,7 +25,6 @@
 #include <du-ino_dsp.h>
 #include <TimerOne.h>
 #include <avr/pgmspace.h>
-#include <util/atomic.h>

 #define DIGITAL_THRESH 3.0 // V

@@ -79,6 +78,8 @@
 void clock_isr();
 void reset_isr();

+void update_clock();
+
 class DU_SEQ_Function : public DUINO_Function {
  public:
   DU_SEQ_Function() : DUINO_Function(0b0111111111) { }
@@ -93,14 +94,11 @@
   virtual void loop()
   {
     // cache stage, step, and clock gate (so that each loop is "atomic")
-    ATOMIC_BLOCK(ATOMIC_FORCEON)
-    {
-      cached_stage = stage;
-      cached_step = step;
-      cached_clock_gate = clock_gate;
-      cached_retrigger = retrigger;
-      retrigger = false;
-    }
+    cached_stage = stage;
+    cached_step = step;
+    cached_clock_gate = clock_gate;
+    cached_retrigger = retrigger;
+    retrigger = false;

     if(cached_retrigger)
     {
@@ -276,7 +274,6 @@
     }
     seq_values.clock_period = bpm_to_us(params.vals.clock_bpm);
     seq_values.clock_ext = !(bool)params.vals.clock_bpm;
-    Timer1.initialize();
     update_clock();

     if(params.vals.gate_time < 0 || params.vals.gate_time > 16)
@@ -576,15 +573,6 @@
     }
   }

-  void update_clock()
-  {
-    Timer1.detachInterrupt();
-    if(!seq_values.clock_ext)
-    {
-      Timer1.attachInterrupt(clock_isr, seq_values.clock_period);
-    }
-  }
-
  private:
   float note_to_cv(int8_t note)
   {
@@ -790,13 +778,24 @@
 void reset_isr()
 {
   stage = step = 0;
-  interface->update_clock();
+  update_clock();
+}
+
+void update_clock()
+{
+  Timer1.detachInterrupt();
+  if(!seq_values.clock_ext)
+  {
+    Timer1.attachInterrupt(clock_isr, seq_values.clock_period);
+  }
 }

 void setup()
 {
   stage = step = 0;
   gate = clock_gate = retrigger = false;
+
+  Timer1.initialize();

   slew_filter = new DUINO_Filter(DUINO_Filter::LowPass, 1.0, 0.0);


Revert "Converted most other strings to program memory (requires SyrotaAutomation library update)"

This reverts commit 6352d9a9bb002631ecab05c55ee7798ea051c195.

--- projects/SumpPumpController/Arduino/Arduino.ino
+++ projects/SumpPumpController/Arduino/Arduino.ino
@@ -82,49 +82,49 @@
 {
   // Process RS-485 commands
   if (net.messageReceived()) {
-    if (net.assertCommand(PSTR("alertPresent")) || net.assertCommand(PSTR("alarmPresent"))) {
+    if (net.assertCommand("alertPresent") || net.assertCommand("alarmPresent")) {
       if (alert.present) {
         if (alert.acknowledged) {
-          net.sendResponse(PSTR("YES, Ack'ed"));
+          net.sendResponse("YES, Ack'ed");
         } else {
-          net.sendResponse(PSTR("YES"));
+          net.sendResponse("YES");
         }
       } else {
-        net.sendResponse(PSTR("NO"));
+        net.sendResponse("NO");
       }
-    } else if (net.assertCommand(PSTR("alertReason"))) {
+    } else if (net.assertCommand("alertReason")) {
       net.sendResponse(alert.condition);
-    } else if (net.assertCommand(PSTR("getDistance"))) {
+    } else if (net.assertCommand("getDistance")) {
       net.sendResponse(itoa(readDistance(), buf, 10));
-    } else if (net.assertCommand(PSTR("getPressure"))) {
+    } else if (net.assertCommand("getPressure")) {
       net.sendResponse(itoa(readPressure(), buf, 10));
-    } else if (net.assertCommand(PSTR("getBattVoltage"))) {
+    } else if (net.assertCommand("getBattVoltage")) {
       sprintf(buf, "%dmV", readBatteryVoltage());
       net.sendResponse(buf);
-    } else if (net.assertCommand(PSTR("getAcPumpOnTime"))) {
+    } else if (net.assertCommand("getAcPumpOnTime")) {
       if (acpump.currentlyOn) {
         sprintf(buf, "%d", acpump.onSeconds + (now() - acpump.switchOnTime));
       } else {
         sprintf(buf, "%d", acpump.onSeconds);
       }
       net.sendResponse(buf);
-    } else if (net.assertCommand(PSTR("getLastSelfTest"))) {
+    } else if (net.assertCommand("getLastSelfTest")) {
       sendSelfTestResponse();
-    } else if (net.assertCommand(PSTR("forceAlert"))) {
-      raiseAlert(ExternallyForced, PSTR("Force command"));
-      net.sendResponse(PSTR("OK"));
-    } else if (net.assertCommand(PSTR("resetAlert"))) {
+    } else if (net.assertCommand("forceAlert")) {
+      raiseAlert(ExternallyForced, "Force command");
+      net.sendResponse("OK");
+    } else if (net.assertCommand("resetAlert")) {
       resetAlert();
-      net.sendResponse(PSTR("OK"));
-    } else if (net.assertCommand(PSTR("startSelfTest"))) {
+      net.sendResponse("OK");
+    } else if (net.assertCommand("startSelfTest")) {
       dcSelfTestStart();
-      net.sendResponse(PSTR("OK"));
-    } else if (net.assertCommandStarts(PSTR("set"), buf)) {
+      net.sendResponse("OK");
+    } else if (net.assertCommandStarts("set", buf)) {
       processSetCommands();
-    } else if (net.assertCommand(PSTR("debug"))) {
+    } else if (net.assertCommand("debug")) {
       sendDebugResponse();
     } else {
-      net.sendResponse(PSTR("Unrecognized command"));
+      net.sendResponse("Unrecognized command");
     }
   }

@@ -169,25 +169,25 @@
 // Write to the configuration when we receive new parameters
 void processSetCommands()
 {
-  if (net.assertCommandStarts(PSTR("setAlertBatteryVoltage:"), buf)) {
+  if (net.assertCommandStarts("setAlertBatteryVoltage:", buf)) {
     unsigned int tmp = strtol(buf, NULL, 10);
     if (tmp > 9000 && tmp < 15000) {
       conf.alertBatteryVoltage = tmp;
       saveConfig();
-      net.sendResponse(PSTR("OK"));
-    } else {
-      net.sendResponse(PSTR("ERROR"));
-    }
-  } else if (net.assertCommandStarts(PSTR("setAlertWaterLevel:"), buf)) {
+      net.sendResponse("OK");
+    } else {
+      net.sendResponse("ERROR");
+    }
+  } else if (net.assertCommandStarts("setAlertWaterLevel:", buf)) {
     int tmp = strtol(buf, NULL, 10);
     if (tmp > -100 && tmp < 100) {
       conf.alertWaterLevel = tmp;
       saveConfig();
-      net.sendResponse(PSTR("OK"));
-    } else {
-      net.sendResponse(PSTR("ERROR"));
-    }
-  } else if (net.assertCommandStarts(PSTR("setBaudRate:"), buf)) {
+      net.sendResponse("OK");
+    } else {
+      net.sendResponse("ERROR");
+    }
+  } else if (net.assertCommandStarts("setBaudRate:", buf)) {
     long tmp = strtol(buf, NULL, 10);
     // Supported: 9600, 14400, 19200, 28800, 38400, 57600, or 115200
     if (tmp == 9600 ||
@@ -200,111 +200,111 @@
     ) {
       conf.baudRate = tmp;
       saveConfig();
-      net.sendResponse(PSTR("OK"));
+      net.sendResponse("OK");
       Serial.end();
       Serial.begin(tmp);
     } else {
-      net.sendResponse(PSTR("ERROR"));
-    }
-  } else if (net.assertCommandStarts(PSTR("setAcPumpOnThreshold:"), buf)) {
+      net.sendResponse("ERROR");
+    }
+  } else if (net.assertCommandStarts("setAcPumpOnThreshold:", buf)) {
     int tmp = strtol(buf, NULL, 10);
     if (tmp > 0 && tmp < 1025) {
       conf.acPumpOnThreshold = tmp;
       saveConfig();
-      net.sendResponse(PSTR("OK"));
-    } else {
-      net.sendResponse(PSTR("ERROR"));
-    }
-  } else if (net.assertCommandStarts(PSTR("setAlertPressureLevel:"), buf)) {
+      net.sendResponse("OK");
+    } else {
+      net.sendResponse("ERROR");
+    }
+  } else if (net.assertCommandStarts("setAlertPressureLevel:", buf)) {
     int tmp = strtol(buf, NULL, 10);
     if (tmp > 0 && tmp < 1025) {
       conf.alertPressureLevel = tmp;
       saveConfig();
-      net.sendResponse(PSTR("OK"));
-    } else {
-      net.sendResponse(PSTR("ERROR"));
-    }
-  } else if (net.assertCommandStarts(PSTR("setAcPumpOnTimeWarning:"), buf)) {
+      net.sendResponse("OK");
+    } else {
+      net.sendResponse("ERROR");
+    }
+  } else if (net.assertCommandStarts("setAcPumpOnTimeWarning:", buf)) {
     int tmp = strtol(buf, NULL, 10);
     if (tmp > 0 && tmp < 30000) {
       conf.acPumpOnTimeWarning = tmp;
       saveConfig();
-      net.sendResponse(PSTR("OK"));
-    } else {
-      net.sendResponse(PSTR("ERROR"));
-    }
-  } else if (net.assertCommandStarts(PSTR("setSelftestTimeBetween:"), buf)) {
+      net.sendResponse("OK");
+    } else {
+      net.sendResponse("ERROR");
+    }
+  } else if (net.assertCommandStarts("setSelftestTimeBetween:", buf)) {
     long tmp = strtol(buf, NULL, 10);
     if (tmp > 0 && tmp < 1000000L) {
       conf.selftestTimeBetween = tmp;
       saveConfig();
-      net.sendResponse(PSTR("OK"));
-    } else {
-      net.sendResponse(PSTR("ERROR"));
-    }
-  } else if (net.assertCommandStarts(PSTR("setSelfTestTimeLimit:"), buf)) {
+      net.sendResponse("OK");
+    } else {
+      net.sendResponse("ERROR");
+    }
+  } else if (net.assertCommandStarts("setSelfTestTimeLimit:", buf)) {
     int tmp = strtol(buf, NULL, 10);
     // More than 6 seconds, as we're checking that DC pump is on at 6 second mark.
     if (tmp > 6 && tmp < 200) {
       conf.selfTestTimeLimit = tmp;
       saveConfig();
-      net.sendResponse(PSTR("OK"));
-    } else {
-      net.sendResponse(PSTR("ERROR"));
-    }
-  } else if (net.assertCommandStarts(PSTR("setSelfTestMinDepth:"), buf)) {
+      net.sendResponse("OK");
+    } else {
+      net.sendResponse("ERROR");
+    }
+  } else if (net.assertCommandStarts("setSelfTestMinDepth:", buf)) {
     int tmp = strtol(buf, NULL, 10);
     if (tmp > -100 && tmp < 100) {
       conf.selfTestMinDepth = tmp;
       saveConfig();
-      net.sendResponse(PSTR("OK"));
-    } else {
-      net.sendResponse(PSTR("ERROR"));
-    }
-  } else if (net.assertCommandStarts(PSTR("setSelfTestDepthDiff:"), buf)) {
+      net.sendResponse("OK");
+    } else {
+      net.sendResponse("ERROR");
+    }
+  } else if (net.assertCommandStarts("setSelfTestDepthDiff:", buf)) {
     int tmp = strtol(buf, NULL, 10);
     if (tmp > 0 && tmp < 100) {
       conf.selfTestDepthDiff = tmp;
       saveConfig();
-      net.sendResponse(PSTR("OK"));
-    } else {
-      net.sendResponse(PSTR("ERROR"));
-    }
-  } else if (net.assertCommandStarts(PSTR("setDepthMeasureTime:"), buf)) {
+      net.sendResponse("OK");
+    } else {
+      net.sendResponse("ERROR");
+    }
+  } else if (net.assertCommandStarts("setDepthMeasureTime:", buf)) {
     int tmp = strtol(buf, NULL, 10);
     if (tmp >= 0 && tmp < 120) {
       conf.depthMeasureTime = tmp;
       saveConfig();
-      net.sendResponse(PSTR("OK"));
-    } else {
-      net.sendResponse(PSTR("ERROR"));
-    }
-  } else if (net.assertCommandStarts(PSTR("setBuzzerEnabled:"), buf)) {
+      net.sendResponse("OK");
+    } else {
+      net.sendResponse("ERROR");
+    }
+  } else if (net.assertCommandStarts("setBuzzerEnabled:", buf)) {
     int tmp = strtol(buf, NULL, 10);
     if (tmp == 1 || tmp == 0) {
       conf.buzzerEnabled = tmp;
       saveConfig();
-      net.sendResponse(PSTR("OK"));
-    } else {
-      net.sendResponse(PSTR("ERROR"));
-    }
-  } else if (net.assertCommandStarts(PSTR("setZeroPressure:"), buf)) {
+      net.sendResponse("OK");
+    } else {
+      net.sendResponse("ERROR");
+    }
+  } else if (net.assertCommandStarts("setZeroPressure:", buf)) {
     int tmp = strtol(buf, NULL, 10);
     if (tmp >= 0 && tmp < 1023) {
       conf.zeroPressure = tmp;
       saveConfig();
-      net.sendResponse(PSTR("OK"));
-    } else {
-      net.sendResponse(PSTR("ERROR"));
-    }
-  } else if (net.assertCommandStarts(PSTR("setPointsPerCm:"), buf)) {
+      net.sendResponse("OK");
+    } else {
+      net.sendResponse("ERROR");
+    }
+  } else if (net.assertCommandStarts("setPointsPerCm:", buf)) {
     int tmp = strtol(buf, NULL, 10);
     if (tmp >= -1024 && tmp < 1024) {
       conf.pointsPerCm = tmp;
       saveConfig();
-      net.sendResponse(PSTR("OK"));
-    } else {
-      net.sendResponse(PSTR("ERROR"));
+      net.sendResponse("OK");
+    } else {
+      net.sendResponse("ERROR");
     }
   } else if (net.assertCommandStarts(PSTR("setDcPumpOnTimeWarning:"), buf)) {
     int tmp = strtol(buf, NULL, 10);
@@ -316,7 +316,7 @@
       net.sendResponse(PSTR("ERROR: 1-3600 expected."));
     }
   } else {
-    net.sendResponse(PSTR("Unrecognized command"));
+    net.sendResponse("Unrecognized command");
   }
 }


changed topic name

changed topic name

--- jsk_2016_01_baxter_apc/firmware/arm_manager.ino
+++ jsk_2016_01_baxter_apc/firmware/arm_manager.ino
@@ -3,6 +3,7 @@
 #include <std_msgs/String.h>
 #include <std_msgs/Bool.h>
 #include <std_msgs/Float64.h>
+#include <std_msgs/Float32.h>
 #include <std_msgs/Int16.h>
 #include <Servo.h>

@@ -25,57 +26,62 @@
 ros::NodeHandle nh; //write with IDE
 // ros::NodeHandle_<ArduinoHardware, 1, 2, 512, 512> nh;

-std_msgs::Float64 float_msg;
+std_msgs::Float64 pressure_msg;
 std_msgs::Bool bool_msg;
-std_msgs::Bool servo_state_msg;
-std_msgs::Int16 servo_angle_msg;
-
-ros::Publisher pressure_pub("vacuum_gripper/limb/right/pressure", &float_msg);
-ros::Publisher state_pub("gripper_grabbed/limb/right/state", &bool_msg);
-ros::Publisher servo_state_pub("vacuum_gripper/limb/right/servo_state_info", &servo_state_msg);
-ros::Publisher servo_angle_pub("vacuum_gripper/limb/right/servo_angle_info", &servo_angle_msg);
+std_msgs::Bool servo_torque_msg;
+std_msgs::Float32 servo_angle_msg;
+
+ros::Publisher pressure_pub("gripper_front/limb/right/pressure/state", &pressure_msg);
+ros::Publisher state_pub("gripper_front/limb/right/pressure/grabbed/state", &bool_msg);
+ros::Publisher servo_torque_pub("gripper_front/limb/right/servo/torque/state", &servo_torque_msg);
+ros::Publisher servo_angle_pub("gripper_front/limb/right/servo/angle/state", &servo_angle_msg);

 unsigned long  publisher_timer = 0;

 Servo myservo;

-void servoCb(const std_msgs::Int16& angle_msg)
-{
-    if (servo_state_msg.data == false) return;
-    if (angle_msg.data < 0) servo_angle_msg.data = 0;
-    else if (angle_msg.data > 180) servo_angle_msg.data = 180;
+void set_angle(void)
+{
+    myservo.write((int)(-servo_angle_msg.data + 90.0));
+}
+
+void servo_angleCb(const std_msgs::Float32& angle_msg)
+{
+    if (servo_torque_msg.data == false) return;
+    if (angle_msg.data < -90.0) servo_angle_msg.data = -90.0;
+    else if (angle_msg.data > 90.0) servo_angle_msg.data = 90.0;
     else servo_angle_msg.data = angle_msg.data;
-    myservo.write(servo_angle_msg.data);
-}
-
-void servo_toggleCb(const std_msgs::Bool& servo_toggle_msg)
+    set_angle();
+}
+
+void servo_torqueCb(const std_msgs::Bool& servo_toggle_msg)
 {
     if (servo_toggle_msg.data) {
-        servo_state_msg.data = true;
+        servo_torque_msg.data = true;
         if (!myservo.attached()) myservo.attach(5);
-        myservo.write(servo_angle_msg.data);
+        set_angle();
     } else {
-        servo_state_msg.data = false;
+        servo_torque_msg.data = false;
         if (myservo.attached()) myservo.detach();
     }
 }

-ros::Subscriber<std_msgs::Int16> servo_angle_sub("vacuum_gripper/limb/right/servo_angle", &servoCb);
-ros::Subscriber<std_msgs::Bool> servo_state_sub("vacuum_gripper/limb/right/servo_state", &servo_toggleCb);
+ros::Subscriber<std_msgs::Float32> servo_angle_sub("gripper_front/limb/right/servo/angle", &servo_angleCb);
+ros::Subscriber<std_msgs::Bool> servo_torque_sub("gripper_front/limb/right/servo/torque", &servo_torqueCb);

 void setup() {
-    servo_state_msg.data = false;
-    servo_angle_msg.data = 90;
+    servo_torque_msg.data = false;
+    servo_angle_msg.data = 0;
     init_servo();
     pinMode(SS, OUTPUT);
     digitalWrite(SS, HIGH);
     nh.getHardware()->setBaud(115200);
     nh.initNode();
     nh.subscribe(servo_angle_sub);
-    nh.subscribe(servo_state_sub);
+    nh.subscribe(servo_torque_sub);
     nh.advertise(pressure_pub);
     nh.advertise(state_pub);
-    nh.advertise(servo_state_pub);
+    nh.advertise(servo_torque_pub);
     nh.advertise(servo_angle_pub);

     SPI.begin();
@@ -97,11 +103,11 @@
         press_act = (float)press_cal / 100.0;

         bool_msg.data = (press_act < 900);
-        float_msg.data = press_act;
+        pressure_msg.data = press_act;

         state_pub.publish(&bool_msg);
-        pressure_pub.publish(&float_msg);
-        servo_state_pub.publish(&servo_state_msg);
+        pressure_pub.publish(&pressure_msg);
+        servo_torque_pub.publish(&servo_torque_msg);
         servo_angle_pub.publish(&servo_angle_msg);

         publisher_timer = millis() + 100;
@@ -111,8 +117,8 @@

 void init_servo()
 {
-    myservo.attach(5);
-    myservo.write(servo_angle_msg.data);
+    //myservo.attach(5);
+    set_angle();
 }

 void initBME()


Added the capability of the program to run when the device is not connected to the server.

--- BackEnd/BackEnd.ino
+++ BackEnd/BackEnd.ino
@@ -629,7 +629,7 @@
   memset(buf,'\0',LWS_SEND_BUFFER_PRE_PADDING + WEB_SOCKET_BUFFER_SIZE + LWS_SEND_BUFFER_POST_PADDING);
   unsigned char *p = &buf[LWS_SEND_BUFFER_PRE_PADDING];

-  updateBoardState();
+//  updateBoardState();

   aJsonObject *msg = getJsonBoardState();

@@ -1237,13 +1237,6 @@

 //  Serial.print("Msg recvd: ");
 //  Serial.println(_acMsg);
-
-  debug_serial_print("Msg received. Size: ");
-//  debug_serial_println(sizeof(_acMsg));
-    #ifdef DEBUG_CAT
-      Serial.println(strlen(_acMsg)); // debug
-    #endif
-  debug_serial_print(_acMsg);

   // Ignore messages bigger than we can handle
   if( sizeof(_acMsg) >= WEB_SOCKET_BUFFER_SIZE )
@@ -1700,6 +1693,9 @@
 void loop()
 {

+    // Update HW
+    updateBoardState();
+
 // TESTING
 //  getSerialCommand();


Fixed compile warnings

Fixed compile warnings

    * examples/SodaqOne-TTN-Mapper-binary/SodaqOne-TTN-Mapper-binary.ino:


Use BUILTIN_LED

Use BUILTIN_LED

--- libraries/esp8266/examples/BlinkWithoutDelay/BlinkWithoutDelay.ino
+++ libraries/esp8266/examples/BlinkWithoutDelay/BlinkWithoutDelay.ino
@@ -3,11 +3,12 @@
  Blink the blue LED on the ESP-01 module
  Based on the Arduino Blink without Delay example
  This example code is in the public domain
+
+ The blue LED on the ESP-01 module is connected to GPIO1
+ (which is also the TXD pin; so we cannot use Serial.print() at the same time)
+
+ Note that this sketch uses BUILTIN_LED to find the pin with the internal LED
 */
-
-const int ledPin =  1;  // The blue LED on the ESP-01 module is connected to GPIO1
-                        // (which is also the TXD pin; so we cannot use
-                        // Serial.print() at the same time

 int ledState = LOW;

@@ -15,7 +16,7 @@
 const long interval = 1000;

 void setup() {
-  pinMode(ledPin, OUTPUT);
+  pinMode(BUILTIN_LED, OUTPUT);
 }

 void loop()
@@ -27,7 +28,6 @@
       ledState = HIGH;  // Note that this switches the LED *off*
     else
       ledState = LOW;   // Note that this switches the LED *on*
-    digitalWrite(ledPin, ledState);
+    digitalWrite(BUILTIN_LED, ledState);
   }
 }
-


added choreo function for Chimes by Hudson Mohawke

--- work-in-progress/work-in-progress.ino
+++ work-in-progress/work-in-progress.ino
@@ -12,6 +12,44 @@
   FastLED.addLeds<NEOPIXEL, PIN>(leds, NUM_LEDS);
 }

+
 void loop() {
-  jiggly.runAllAnimations(leds, NUM_LEDS, 40, 2000);
+//  jiggly.runAllAnimations(leds, NUM_LEDS, 40, 2000);
+  chimes();
 }
+
+
+void chimes(){
+  jiggly.runner(leds, NUM_LEDS, CRGB(70,0,100), 6600);
+  jiggly.runner(leds, NUM_LEDS, CRGB(100,60,10), 6600);
+//  jiggly.runner(leds, NUM_LEDS, CRGB(0,100,30), 6600);
+
+  jiggly.flashyShit(leds, NUM_LEDS, 50, 6600);
+  jiggly.flashyShit(leds, NUM_LEDS, 50, 6600);
+
+  jiggly.progressbarCentered(leds, NUM_LEDS, CRGB(30,0,100), 3300);
+  delay(390);
+  for (int i = 0; i < 3; i++){
+    jiggly.blinking(leds,NUM_LEDS,CRGB(0,60,100), 200);
+  }
+  jiggly.blinking(leds,NUM_LEDS,CRGB(0,60,100), 300);
+  jiggly.blinking(leds,NUM_LEDS,CRGB(0,60,100), 210);
+
+
+  jiggly.progressbarCentered(leds, NUM_LEDS, CRGB(100,0,0), 3300);
+  delay(395);
+  for (int i = 0; i < 3; i++){
+    jiggly.blinking(leds,NUM_LEDS,CRGB(100,20,20), 200);
+  }
+  jiggly.blinking(leds,NUM_LEDS,CRGB(100,20,20), 300);
+  jiggly.blinking(leds,NUM_LEDS,CRGB(100,20,20), 210);
+
+  jiggly.progressbarCentered(leds, NUM_LEDS, CRGB(0,100,30), 3300);
+  delay(395);
+  for (int i = 0; i < 3; i++){
+    jiggly.blinking(leds,NUM_LEDS,CRGB(0,100,40), 200);
+  }
+  jiggly.blinking(leds,NUM_LEDS,CRGB(0,100,40), 300);
+  jiggly.blinking(leds,NUM_LEDS,CRGB(0,100,40), 210);
+  delay(10000);
+}


More brackets to be sure

More brackets to be sure

--- DebugManchester.ino
+++ DebugManchester.ino
@@ -128,7 +128,7 @@
         }//end of detecting a "zero" inside a header
         else{
           //we have our header, chewed up any excess and here is a zero
-          if (!firstZero && (headerHits>=headerBits)){ //if first zero, it has not been found previously
+          if ((!firstZero) && (headerHits>=headerBits)){ //if first zero, it has not been found previously
             firstZero=true;
           }//end of finding first zero
           add(bitState);


add Serial.flush(); wait until end of out buffer

--- ESP8266/ESP8266.ino
+++ ESP8266/ESP8266.ino
@@ -14,6 +14,7 @@
 void debugPrint(String strMessage) {
   Serial.print("Debug: ");
   Serial.println(strMessage);
+  Serial.flush();
 }

 // callback method to handle received mqtt messages
@@ -27,6 +28,7 @@
   Serial.print(strTopic);
   Serial.print(":");
   Serial.println(strMessage);
+  Serial.flush();
 }

 // handle received serial data


Update HelloWorld.ino

Update HelloWorld.ino

Adding power connections, just to be thorough

--- libraries/LiquidCrystal/examples/HelloWorld/HelloWorld.ino
+++ libraries/LiquidCrystal/examples/HelloWorld/HelloWorld.ino
@@ -17,6 +17,8 @@
  * LCD D6 pin to digital pin 3
  * LCD D7 pin to digital pin 2
  * LCD R/W pin to ground
+ * LCD VSS pin to ground
+ * LCD VCC pin to 5V
  * 10K resistor:
  * ends to +5V and ground
  * wiper to LCD VO pin (pin 3)


Can upload and flash

Can upload and flash

--- esppic/esppic.ino
+++ esppic/esppic.ino
@@ -102,10 +102,10 @@
 }

-File UploadFile;
+//File UploadFile;
 String filename;
 File fsUploadFile;
-String webString;
+//String webString;

 //
 //
@@ -142,16 +142,15 @@

 void handleFileUpload() {
+  if(server.uri() != "/upload") return;
   Serial.println("handleFileUpload()");
-  if(server.uri() != "/upload") return;
   HTTPUpload& upload = server.upload();
   if(upload.status == UPLOAD_FILE_START){
-    String filename = upload.filename;
+    filename = upload.filename;
     filename = "/payloads/"+filename;
     Serial.print("Uploading file ");
-    Serial.print(filename+" ... ");
+    Serial.println(filename);
     fsUploadFile = SPIFFS.open(filename, "w");
-    filename = String();
   }
   else if(upload.status == UPLOAD_FILE_WRITE){
     if(fsUploadFile)
@@ -213,6 +212,26 @@
     server.send_P(200, "image/png", logo_png, sizeof(logo_png));
   });

+  server.on("/reset/yes", HTTP_GET, []() {
+    Serial.println("HTTP_GET /reset/yes");
+    RESET_LOW;
+    server.send_P(200,"text/html", index_html);
+  });
+
+  server.on("/reset/no", HTTP_GET, []() {
+    Serial.println("HTTP_GET /reset/no");
+    RESET_HIGH;
+    server.send_P(200,"text/html", index_html);
+  });
+
+  server.on("/reset/pulse", HTTP_GET, []() {
+    Serial.println("HTTP_GET /reset/pulse");
+    RESET_LOW;
+    delay(100);
+    RESET_HIGH;
+    server.send_P(200,"text/html", index_html);
+  });
+
   server.on("/readconfigs", HTTP_GET, []() {
     Serial.println("HTTP_GET /readconfig");
     char *html=(char *)malloc(10000);
@@ -220,6 +239,7 @@
     EnterLVPmode();
     CmdResetAddress();
     DumpConfig(tmps);
+    DumpMemory();
     RESET_HIGH;
     strcpy(html,"
CONFIG AREAS
<tt>");
     strcat(html,tmps);
@@ -241,6 +261,34 @@
     Store(USERID+3,SWAP16(0xE12F)); delay(5);
     Store(CONFIG1, SWAP16(0xE4C9)); delay(5);
     Store(CONFIG2, SWAP16(0xFBFF)); delay(5);
+
+    // CODE
+
+    Serial.print("Trying to open ");
+    Serial.println(filename);
+    File f = SPIFFS.open(filename, "r");
+    if (!f) {
+      Serial.print("Couldn't open ");
+      Serial.println(filename);
+    }
+    while(f.available()) {
+      uint8_t d_len;
+      uint16_t d_addr;
+      uint8_t d_typ;
+      String s = f.readStringUntil('\n');
+      Serial.println(s);
+      d_len=HexDec2(s[1],s[2]);
+      d_addr=HexDec4(s[3],s[4],s[5],s[6]);
+      d_typ=HexDec2(s[7],s[8]);
+      Serial.printf("len=%02x addres=%04x type=%02x\n",d_len,d_addr,d_typ);
+      if (d_typ==0x00) {
+        for (uint8_t i=0; i<d_len*2; i+=4) {
+          Store(d_addr/2+i/4,HexDec4(s[11+i],s[12+i],s[9+i],s[10+i]));
+        }
+      }
+    }
+    f.close();
+
     RESET_HIGH;
     strcpy(html,"
FLASH DONE
Back");
     server.send(200, "text/html", html);


Actually used the crappy "pullse" mode. Changed a few values to make things look a little better. Added a brighter and possible more customizable rainbow mode. Not using it quite yet.

--- InfinityGoggles.ino
+++ InfinityGoggles.ino
@@ -61,6 +61,7 @@
         {nothing,       bpm},
         {nothing,       juggle},
         {nothing,       sinelon},
+        {nothing,       pulse},
         {lowKeyRainbow, rotateClockwise},
         {meteor,        rotateClockwise}
 };
@@ -136,7 +137,7 @@
 }

 void activateGestureModeSelect() {
-    uint8_t hueCounter = 1;
+    uint8_t hueCounter = 0;
     clearStrip();
     FastLED.show();
     int ledsPerSection = round(NUM_LEDS / ARRAY_SIZE(patterns));
@@ -243,8 +244,8 @@

 // a colored dot sweeping back and forth, with fading trails
 void sinelon() {
-    fadeToBlackBy(rightLense, NUM_LEDS, 20);
-    fadeToBlackBy(leftLense, NUM_LEDS, 20);
+    fadeToBlackBy(rightLense, NUM_LEDS, 10);
+    fadeToBlackBy(leftLense, NUM_LEDS, 10);
     int pos = beatsin16(13, 0, NUM_LEDS);
     rightLense[pos] += CHSV(hue, 200, 192);
     leftLense[pos] += CHSV(hue, 200, 192);
@@ -260,9 +261,9 @@
     fadeToBlackBy(rightLense, NUM_LEDS, 20);
     fadeToBlackBy(leftLense, NUM_LEDS, 20);
     byte dothue = 0;
-    for (int i = 0; i < 8; i++) {
-        rightLense[beatsin16(i + 7, 0, NUM_LEDS)] |= CHSV(dothue, 200, 180);
-        leftLense[beatsin16(i + 7, 0, NUM_LEDS)] |= CHSV(dothue, 200, 180);
+    for (int i = 0; i < 6; i++) {
+        rightLense[beatsin16(i + 5, 0, NUM_LEDS)] |= CHSV(dothue, 200, 180);
+        leftLense[beatsin16(i + 5, 0, NUM_LEDS)] |= CHSV(dothue, 200, 180);
         dothue += 32;
     }

@@ -349,6 +350,12 @@
 bool swap = true;
 bool direction = false;

+void pulse() {
+    pulse(rightLense, random16(NUM_LEDS), 2);
+    pulse(leftLense, random16(NUM_LEDS), 2);
+    delay(100);
+}
+
 void pulse(CRGB strip[], int center, int radius) {
     if (swap == true) {
         swap = false;
@@ -357,9 +364,8 @@
     if (direction == false) {
         fadeAmount = -fadeAmount;
     }
-    lsm.getEvent(&accel, &mag, &gyro, &temp);
     CRGB magHeadingBasedColor = convertHeadingToColor(fabs(round(mag.magnetic.z * 100)), 180);
-    int tempBrightness = brightness;
+    int tempBrightness = BRIGHTNESS;

     strip[center] = magHeadingBasedColor;
     strip[center].nscale8_video(brightness);
@@ -411,6 +417,12 @@
         leftLense[i] = CRGB(0, 0, 0);
         rightLense[i] = CRGB(0, 0, 0);
     }
+}
+
+void rainbow() {
+    fill_rainbow(leftLense, NUM_LEDS, hue, 11);
+    fill_rainbow(rightLense, NUM_LEDS, hue, 11);
+    FastLED.show();
 }

 void lowKeyRainbow() {


Re-wrote SabertoothDriver arduino file with sherry to make it more readable & functional

--- src/SabertoothDriverROS/SabertoothDriverROS.ino
+++ src/SabertoothDriverROS/SabertoothDriverROS.ino
@@ -1,5 +1,3 @@
-
-
 /*
  / This is a simple Arduino program that demonstrates the
  / "Packetized Serial" method of using the Sabertooth Motor
@@ -24,388 +22,361 @@
 #include <EncoderRR.h>
 #include "SabertoothDriverROS.h"

-std_msgs::String msg;
-command2ros::ManualCommand currentRotationMessage
-ros::Publisher pubResults("testing", &msg);
-ros::Publisher pubCurrentRotation("Current Rotation", &currentRotationMessage)// current rotation data for each wheel.
-                                                                              // This will publish when the angle is updated.
+
+//The encoders we will use to monitor the angle of each articulation joint
 EncoderFL efl;
 EncoderML eml;
 EncoderFR efr;
 EncoderMR emr;
 EncoderRL erl;
 EncoderRR err;
-// Have we done a software E-stop?
-// By default, we have (this way the robot won't move until we get
-// valid control input)
-bool emergencyStop = true;
-
-// Is the robot currently rotating?
-// If true, we should override motor drive velocity with rotation velocity.
-bool currentlyRotating = false;
-bool currentlyDriving = false;
-bool currentlyStopped = false;
-
-//Wheel is 0-6
-const int ARTICULATION_OFFSET = 6;
-
-//Articulation joints try to rotate to a certain point +- ARTICULATION_PLAY
-const int ARTICULATION_PLAY = 3;
-
-// How long should we drive for (milliseconds)?
-unsigned long driveTimeMillis = 0;
-//DriveUntil: Used when driving forwards.
-// Should be of the form millis() + driveTimeMillis
-unsigned long driveUntilTime = 0;
-
-
-// By default, the velocity is zero.
-char current_velocity = 0;
-Wheel_status targetWheelStatus[6];
-Wheel_status currentWheelStatus[6];
-
-void newManualCommandCallback(const command2ros::ManualCommand& nmc)
+
+
+//Control logic
+
+//All proccesses killed, don't ever leave this state.
+const int E_STOPPED = -1;
+
+//Not driving any motors
+const int STOPPED = 0;
+
+//Articulating the wheels
+const int ARTICULATING = 1;
+
+//Done articulating, drive
+const int START_DRIVING = 2;
+
+//Started articulating, drive
+const int IS_DRIVING = 3;
+
+//The state the robot is currently in
+int currentStatus = STOPPED;
+long driveStopTime = 0;
+
+//
+// Articualtion Constants
+//
+
+//We want to get articulation join within this range (degrees) from the target
+const int DELTA_RANGE = 3;
+
+//When we are passing around the direction in which to articulate the wheels, we pass this value
+const int ARTICULATION_DIRECTION_CLOCKWISE = -1;
+const int ARTICULATION_DIRECTION_NONE = 0;
+const int ARTICULATION_DIRECTION_COUNTER_CLOCKWISE = 1;
+
+//The speed
+const double ARTICULATION_DRIVE_SPEED = 6260.0 / 7521.0;
+
+//
+// ROS initialization
+//
+
+// This node handle represents this arduino.
+ros::NodeHandle sabertoothDriverNode;
+
+//
+// ROS Subsribers
+//
+
+//Inbound wheel command
+command2ros::ManualCommand wheelTarget;
+
+// Subscribers for intended drive velocity and direction
+void newManualCommandCallback(const command2ros::ManualCommand& newManualCommand)
 {
-  char* error = "received command";
-  msg.data = error;
-  pubResults.publish(&msg);
+
+  //TODO: Do we need to stop the robot at this point, Sherry and Matt H say no
+
+  //Set articulation target values
+  wheelTarget.fl_articulation_angle = newManualCommand.fl_articulation_angle;
+  wheelTarget.ml_articulation_angle = newManualCommand.ml_articulation_angle;
+  wheelTarget.rl_articulation_angle = newManualCommand.rl_articulation_angle;
+
+  wheelTarget.fr_articulation_angle = newManualCommand.fr_articulation_angle;
+  wheelTarget.mr_articulation_angle = newManualCommand.mr_articulation_angle;
+  wheelTarget.rr_articulation_angle = newManualCommand.rr_articulation_angle;
+
+  //Set wheel speeds
+  wheelTarget.fl_drive_speed = newManualCommand.fl_drive_speed;
+  wheelTarget.ml_drive_speed = newManualCommand.ml_drive_speed;
+  wheelTarget.rl_drive_speed = newManualCommand.rl_drive_speed;
+
+  wheelTarget.fr_drive_speed = newManualCommand.fr_drive_speed;
+  wheelTarget.mr_drive_speed = newManualCommand.mr_drive_speed;
+  wheelTarget.rr_drive_speed = newManualCommand.rr_drive_speed;
+
+  //Set E-Stop Command
+  wheelTarget.e_stop = newManualCommand.e_stop;
+  if (wheelTarget.e_stop == true) {
+    //TODO: Make this E_STOPPED for final product
+    currentStatus = STOPPED;
+    stopAllMotors(false);
+  }
+
+
+  //How long we will be driving from (in seconds)
+  wheelTarget.drive_duration = newManualCommand.drive_duration;
+}
+ros::Subscriber<command2ros::ManualCommand> commandSubscriber("ManualCommand", &newManualCommandCallback );
+
+//
+// ROS Publishers
+//
+
+//Publisher to present the rotation of each of the articulation joints
+command2ros::ManualCommand wheelStatus;
+ros::Publisher pubwheelStatus("current_rotation", &wheelStatus);// current rotation data for each wheel.
+                                                                              // This will publish when the angle is updated.
+//Publisher to print debug statements
+std_msgs::String debugMsg;
+ros::Publisher pubDebug("sabertooth_debugger", &debugMsg);
+
+
+
+
+
+
+
+void setupWheelStatus() {
+  wheelStatus.fl_articulation_angle = 0;
+  wheelStatus.fr_articulation_angle = 180;
+  wheelStatus.ml_articulation_angle = 0;
+  wheelStatus.mr_articulation_angle = 180;
+  wheelStatus.rl_articulation_angle = 0;
+  wheelStatus.rr_articulation_angle = 180;

-  // TODO: Update wheel status info. Determine if we need to rotate.
-  currentlyRotating = currentlyRotating || updateTargetWheelStatus(nmc);
-  if(currentlyRotating)
-  {
-    char* error = "currentlyRotating is TRUE.";
-    msg.data = error;
-    pubResults.publish(&msg);
-  }
-  else
-  {
-    char* error = "currentlyRotating is FALSE.";
-    msg.data = error;
-    pubResults.publish(&msg);
-  }
-  driveTimeMillis = (long)(nmc.drive_duration * 1000);
-  emergencyStop = nmc.e_stop;
+  wheelStatus.fl_drive_speed = 0;
+  wheelStatus.fr_drive_speed = 0;
+  wheelStatus.ml_drive_speed = 0;
+  wheelStatus.mr_drive_speed = 0;
+  wheelStatus.rl_drive_speed = 0;
+  wheelStatus.rr_drive_speed = 0;
+}
+
+/**
+ * Checks the current angle of each of the articulation joints against their target value
+ *    if the difference betwen any of them exceed the range given by DELTA_RANGE then we need
+ *    to articulate "return true"
+ */
+bool needsToArticulate() {
+
+  int delta;
+
+  //Check if the difference between target and actual articulation exceeds our given range
+  delta = wheelStatus.fl_articulation_angle - wheelTarget.fl_articulation_angle;  //FL
+  if (delta < -DELTA_RANGE || delta > DELTA_RANGE) { return true; }
+
+  delta = wheelStatus.ml_articulation_angle - wheelTarget.ml_articulation_angle;  //ML
+  if (delta < -DELTA_RANGE || delta > DELTA_RANGE) { return true; }
+
+  delta = wheelStatus.rl_articulation_angle - wheelTarget.rl_articulation_angle;  //RL
+  if (delta < -DELTA_RANGE || delta > DELTA_RANGE) { return true; }
+
+  delta = wheelStatus.fr_articulation_angle - wheelTarget.fr_articulation_angle;  //FR
+  if (delta < -DELTA_RANGE || delta > DELTA_RANGE) { return true; }
+
+  delta = wheelStatus.mr_articulation_angle - wheelTarget.mr_articulation_angle;  //MR
+  if (delta < -DELTA_RANGE || delta > DELTA_RANGE) { return true; }
+
+  delta = wheelStatus.rr_articulation_angle - wheelTarget.rr_articulation_angle;  //RR
+  if (delta < -DELTA_RANGE || delta > DELTA_RANGE) { return true; }
+
+  return false;
+}
+
+void articulateAllWheels() {
+
+  int delta;
+  int direction;
+
+  direction = getArticulationDirection(wheelStatus.fl_articulation_angle, wheelTarget.fl_articulation_angle); //FL
+  articulateWheel(FRONT_LEFT_DRIVE_MOTOR_ID, direction);
+
+  direction = getArticulationDirection(wheelStatus.ml_articulation_angle, wheelTarget.ml_articulation_angle); //ML
+  articulateWheel(MIDDLE_LEFT_DRIVE_MOTOR_ID, direction);
+
+  direction = getArticulationDirection(wheelStatus.rl_articulation_angle, wheelTarget.rl_articulation_angle); //RL
+  articulateWheel(REAR_LEFT_DRIVE_MOTOR_ID, direction);
+
+  direction = getArticulationDirection(wheelStatus.fr_articulation_angle, wheelTarget.fr_articulation_angle); //FR
+  articulateWheel(FRONT_RIGHT_DRIVE_MOTOR_ID, direction);
+
+  direction = getArticulationDirection(wheelStatus.mr_articulation_angle, wheelTarget.mr_articulation_angle); //MR
+  articulateWheel(MIDDLE_RIGHT_DRIVE_MOTOR_ID, direction);
+
+  direction = getArticulationDirection(wheelStatus.rr_articulation_angle, wheelTarget.rr_articulation_angle); //RR
+  articulateWheel(REAR_RIGHT_DRIVE_MOTOR_ID, direction);
+
+}
+
+/**
+ *
+ */
+int getArticulationDirection(int from, int to) {
+
+  int delta = from-to;
+  if (delta > -DELTA_RANGE && delta < DELTA_RANGE) {
+    return ARTICULATION_DIRECTION_NONE;
+  }
+
+  // Calculations are made by "normalizing" the data so that we are always starting from "0"
+  // and heading towards the target
+
+  // If the current articulation angle is between 0 and 180, then we want to shift the
+  // whole frame of reference clockwise
+
+  //Shift "to" and "from" so that "to" is the angle that we need to turn and "from" is 0
+  if(from > 0 && from <= 180){
+
+    // shift "to" by however much "from" shifted (counter clockwise)
+    to -= from;
+    if(to < 0){
+      to += 360;
+    }
+
+
+    // shift "from" to 0 degrees
+    from = 0;
+  }
+  else{
+    delta = 360 - from;
+
+    // shift "from" to 0 degrees
+    from = 0;
+
+    // shift "to" by however much "from" shifted (clockwise)
+    to += delta;
+    if(to > 360){
+      to -= 360;
+    }
+  }
+
+
+  //Now we compare the two distances and move in the diection that is fastest
+  int counterClockwiseDistance = to;
+  int clockwiseDistance = 360 - to;
+
+  if(counterClockwiseDistance > clockwiseDistance){
+    return ARTICULATION_DIRECTION_CLOCKWISE;
+  }
+  else{
+    return ARTICULATION_DIRECTION_COUNTER_CLOCKWISE;
+  }
+}
+
+/**
+ *
+ */
+void articulateWheel(int motorID, int direction) {
+
+  int articulationSpeed = 50*direction*ARTICULATION_DRIVE_SPEED;
+  int wheelSpeed = 50*direction;
+
+  //TODO: Make a constant
+  int articulationID = motorID + 6;
+
+  if (direction == ARTICULATION_DIRECTION_CLOCKWISE) {
+    driveClockwise(articulationID, articulationSpeed);
+    driveClockwise(motorID, wheelSpeed);
+  } else if (direction == ARTICULATION_DIRECTION_COUNTER_CLOCKWISE) {
+    driveCounterclockwise(articulationID, articulationSpeed);
+    driveCounterclockwise(motorID, wheelSpeed);
+  }
+
+}
+
+
+void driveAllMotors() {
+
+  //Set wheel speeds
+  driveMotor(FRONT_LEFT_DRIVE_MOTOR_ID, wheelTarget.fl_drive_speed*-1);
+  driveMotor(MIDDLE_LEFT_DRIVE_MOTOR_ID, wheelTarget.ml_drive_speed*-1);
+  driveMotor(REAR_LEFT_DRIVE_MOTOR_ID, wheelTarget.rl_drive_speed*-1);

-  if(emergencyStop == true)
-  {
-    char* error = "Setting E-Stop to TRUE.";
-    msg.data = error;
-    pubResults.publish(&msg);
-  }
-  else
-  {
-    char* error = "Setting E-Stop to FALSE.";
-    msg.data = error;
-    pubResults.publish(&msg);
+  driveMotor(FRONT_RIGHT_DRIVE_MOTOR_ID, wheelTarget.fr_drive_speed);
+  driveMotor(MIDDLE_RIGHT_DRIVE_MOTOR_ID, wheelTarget.mr_drive_speed);
+  driveMotor(REAR_RIGHT_DRIVE_MOTOR_ID, wheelTarget.rr_drive_speed);
+
+}
+
+void driveMotor(int motorID, int speed) {
+
+  if (speed < 0) {
+    speed = speed * -50;
+    driveClockwise(motorID, speed);
+  } else {
+    speed = speed * 50;
+    driveCounterclockwise(motorID, speed);
+  }
+
+}
+
+/**
+ * Stops all the motors (Articulation and Wheels) and chages the current state to reflect the stop
+ *
+ * Parameters:
+ *  EStop - Weather or not this is an E-Stop or a normal end of command stop
+ */
+void stopAllMotors(bool EStop) {
+
+  if (EStop == true) {
+    currentStatus = E_STOPPED;
+  } else {
+    currentStatus = STOPPED;
+  }
+
+  //Stop all motors
+  const int speed = 0;
+  for(int motorID = 0; motorID <= 11; ++motorID) {
+    driveClockwise(motorID, speed);
   }

 }

-
-// Function to update wheel status variables.
-// Returns TRUE if an orientation changed, FALSE otherwise.
-bool updateTargetWheelStatus(const command2ros::ManualCommand& nmc)
-{
-  bool retval = false;
-  // Check wheel orientations:
-  // Check if the front-left wheel orientation should be changed.
-  if(targetWheelStatus[FRONT_LEFT_DRIVE_MOTOR_ID].orientation != (int)nmc.fl_articulation_angle)
-  {
-    retval = true;
-    targetWheelStatus[FRONT_LEFT_DRIVE_MOTOR_ID].orientation = (int)nmc.fl_articulation_angle;
-  }
-
-  // Check if the middle-left wheel orientation should be changed.
-  if(targetWheelStatus[MIDDLE_LEFT_DRIVE_MOTOR_ID].orientation != (int)nmc.ml_articulation_angle)
-  {
-    retval = true;
-    targetWheelStatus[MIDDLE_LEFT_DRIVE_MOTOR_ID].orientation = (int)nmc.ml_articulation_angle;
-  }
-
-  // Check if the rear-left wheel orientation should be changed.
-  if(targetWheelStatus[REAR_LEFT_DRIVE_MOTOR_ID].orientation != (int)nmc.rl_articulation_angle)
-  {
-    retval = true;
-    targetWheelStatus[REAR_LEFT_DRIVE_MOTOR_ID].orientation = (int)nmc.rl_articulation_angle;
-  }
-
-  // Check if the front-right wheel orientation should be changed.
-  if(targetWheelStatus[FRONT_RIGHT_DRIVE_MOTOR_ID].orientation != (int)nmc.fr_articulation_angle)
-  {
-    retval = true;
-    targetWheelStatus[FRONT_RIGHT_DRIVE_MOTOR_ID].orientation = (int)nmc.fr_articulation_angle;
-  }
-
-  // Check if the middle-right wheel orientation should be changed.
-  if(targetWheelStatus[MIDDLE_RIGHT_DRIVE_MOTOR_ID].orientation != (int)nmc.mr_articulation_angle)
-  {
-    retval = true;
-    targetWheelStatus[MIDDLE_RIGHT_DRIVE_MOTOR_ID].orientation = (int)nmc.mr_articulation_angle;
-  }
-
-  // Check if the rear-right wheel orientation should change.
-  if(targetWheelStatus[REAR_RIGHT_DRIVE_MOTOR_ID].orientation != (int)nmc.rr_articulation_angle)
-  {
-    retval = true;
-    targetWheelStatus[REAR_RIGHT_DRIVE_MOTOR_ID].orientation = (int)nmc.rr_articulation_angle;
-  }
-
-  // Update target drive speeds.
-  targetWheelStatus[FRONT_LEFT_DRIVE_MOTOR_ID].velocity = (long)nmc.fl_drive_speed;
-  targetWheelStatus[FRONT_RIGHT_DRIVE_MOTOR_ID].velocity = (long)nmc.fr_drive_speed;
-  targetWheelStatus[MIDDLE_LEFT_DRIVE_MOTOR_ID].velocity = (long)nmc.ml_drive_speed;
-  targetWheelStatus[MIDDLE_RIGHT_DRIVE_MOTOR_ID].velocity = (long)nmc.mr_drive_speed;
-  targetWheelStatus[REAR_LEFT_DRIVE_MOTOR_ID].velocity = (long)nmc.rl_drive_speed;
-  targetWheelStatus[REAR_RIGHT_DRIVE_MOTOR_ID].velocity = (long)nmc.rr_drive_speed;
-  return retval;
-
-}
-
-
-
-// Function to drive the motors as specified in the ROS packet.
-// HANDY VALUES:
-// speed: unsigned char representing the velocity (0-127).
-void continueDriving(){
-  // Short-circuit on EStop!
-  if(emergencyStop)
-  {
-    if (!currentlyStopped) {
-      char* error = "E-Stopped all motors.";
-      msg.data = error;
-      pubResults.publish(&msg);
-
-    }
-    stopAllMotors();
-
-    // short-circuit
-    return;
-  }
-
-  if(currentlyRotating)
-  {
-    // TODO:Rotate
-    articulate();
-  }
-  else
-  {
-    // If we still intend to drive, do so.
-    if(millis() < driveUntilTime)
-    {
-      if(!currentlyDriving)
-      {
-        // TODO: Drive
-        driveAllMotors();
-        currentlyDriving = true;
-      }
-    }
-    else
-    {
-      currentlyDriving = false;
-      char* error = "done: Stopped all motors.";
-      msg.data = error;
-      pubResults.publish(&msg);
-
-      stopAllMotors();
-    }
-  }
-
-}
-
-bool currentlyMoving[6];
-// A function to articulate the wheels to face the desired orientation.
-void articulate()
-{
-  // For now, only articulate the FRONT LEFT wheel.
-  int delta = 0;
-  int motorIDX;
-
-  bool stillRotating = false;
-
-  for(motorIDX = 0; motorIDX <= 5; ++motorIDX)
-  {
-    // Which direction should we rotate?
-    delta  = targetWheelStatus[motorIDX].orientation - currentWheelStatus[motorIDX].orientation
-
-    //If we are close enough to the target then stop moving
-    if(delta < ARTICULATION_PLAY && delta > -ARTICULATION_PLAY)
-    {
-      driveCounterclockwise(0, motorIDX + ARTICULATION_OFFSET);
-      driveCounterclockwise(0, motorIDX);
-
-      char* error = "Stopping motor: X";
-      error[16] = '0'+motorIDX;
-      msg.data = error;
-      pubResults.publish(&msg);
-      // TODO: update this when we have multi-wheel support
-      currentlyMoving[motorIDX] = false;
-    }
-
-    //Rotate the shortest distance to face
-    else if (((delta > 0) && (delta < 180)) || ((delta + 360) < 180 ))
-    {
-      stillRotating = true;
-      //So we only send one drive command
-      if(!currentlyMoving[motorIDX])
-      {
-        char* error = "Starting motor: X";
-        currentlyStopped = false;
-        error[16] = '0'+motorIDX;
-        msg.data = error;
-        pubResults.publish(&msg);
-        driveCounterclockwise(ARTICULATION_SPEED, motorIDX + ARTICULATION_OFFSET);
-        driveCounterclockwise(ARTICULATION_DRIVE_SPEED, motorIDX);
-        currentlyMoving[motorIDX] = true;
-      }
-
-    }
-    //
-    else
-    {
-      if(!currentlyMoving[motorIDX])
-      {
-        char* error = "Starting motor: X";
-        currentlyStopped = false;
-        error[16] = '0'+motorIDX;
-        msg.data = error;
-        pubResults.publish(&msg);
-        driveClockwise(ARTICULATION_SPEED, motorIDX + ARTICULATION_OFFSET);
-        driveClockwise(ARTICULATION_DRIVE_SPEED, motorIDX);
-        stillRotating = true;
-        currentlyMoving[motorIDX] = true;
-      }
-    }
-  }
-  //Done rotating, drive until we get to a certain time
-  if(!stillRotating)
-  {
-    driveUntilTime = millis() + driveTimeMillis;
-    currentlyRotating = false;
-  }
-  else
-  {
-    currentlyRotating = true;
-  }
-
-}
-
-// A function that tells all motors to stop moving.
-void stopAllMotors()
-{
-  if (!currentlyStopped) {
-    currentlyStopped = true;
-    // send stop-driving commands to all motors
-    for(int i = 0; i < 12; ++i)
-    {
-      driveClockwise(0,i);
-    }
-    char* error = "Stopping all motors.";
-    msg.data = error;
-    pubResults.publish(&msg);
-  }
-}
-
-// A function to drive all motors at the desired rates.
-void driveAllMotors()
-{
-  // send driving commands to all motors
-  for(int i = 0; i < 6; ++i)
-  {
-    driveClockwise(targetWheelStatus[i].velocity, i);
-  }
-}
-
-// Function to drive a specific motor forward.
-// Packet format: Address Byte, Command Byte, Value Byte, Checksum.
-void driveClockwise(char speed, char motor){
+/**
+ * Drives a given motor at a given speed in a clockwise direction
+ *
+ * Paramteters:
+ *  motorID - the id of the motor to spin (0-11)
+ *  speed - the speed at which to spin the motor.
+ */
+void driveClockwise(int motorID, int speed){
+  // Packet format: Address Byte, Command Byte, Value Byte, Checksum.
   // Build the data packet:
   // Get the address and motor command ID from a predefined array.
-  unsigned char address = MOTOR_ADDRESS[motor];
-  unsigned char command = MOTOR_COMMAND[motor];
-  unsigned char checksum = (address + command + speed) & 0b01111111;
+  unsigned char address = MOTOR_ADDRESS[motorID];
+  unsigned char command = MOTOR_COMMAND[motorID];
+  unsigned char checksum = (address + command + ((char)speed)) & 0b01111111;
   // Write the packet.
   Serial3.write(address);
   Serial3.write(command);
-  Serial3.write(speed);
+  Serial3.write(((char)speed));
   Serial3.write(checksum);
+  //TODO: Move the delay time to a constant
   delayMicroseconds(1000);
 }

-// Function to drive a specific motor backwards.
-// Packet format: Address Byte, Command Byte, Value Byte, Checksum.
-void driveCounterclockwise(char speed, char motor){
-  unsigned char address = MOTOR_ADDRESS[motor];
-  unsigned char command = MOTOR_COMMAND[motor] + 1;
+/**
+ * Drives a given motor at a given speed in a clockwise direction
+ *
+ * Paramteters:
+ *  motorID - the id of the motor to spin (0-11)
+ *  speed - the speed at which to spin the motor.
+ */
+ void driveCounterclockwise(char motorID, char speed){
+  // Packet format: Address Byte, Command Byte, Value Byte, Checksum.
+  unsigned char address = MOTOR_ADDRESS[motorID];
+  unsigned char command = MOTOR_COMMAND[motorID] + 1;
   unsigned char checksum = (address + command + speed) & 0b01111111;
   Serial3.write(address);
   Serial3.write(command);
   Serial3.write(speed);
   Serial3.write(checksum);
+  //TODO: Move the delay time to a constant
   delayMicroseconds(1000);
 }

-// This node handle represents this arduino.
-ros::NodeHandle sabertoothDriverNode;
-// Subscribers for intended drive velocity and direction
-ros::Subscriber<command2ros::ManualCommand> mcSUB("ManualCommand", &newManualCommandCallback );
-
-/**
-* Gets the last known position of each articulation joint, and updates acordingly.
-*/
-void updateArticulationValues()
-{
-  int temp = EncoderFL::getPosition();
-  if (temp < 0)
-  {
-    temp += 400;
-  }
-  currentWheelStatus[FRONT_LEFT_DRIVE_MOTOR_ID].orientation = (int)((temp * 9L) / 10L);
-  currentRotationMessage.FRONT_LEFT_articulation_angle = currentWheelStatus[FRONT_LIGHT_DRIVE_MOTOR_ID].orientation;
-
-  temp = EncoderML::getPosition();
-  if (temp < 0)
-  {
-    temp += 400;
-  }
-  currentWheelStatus[MIDDLE_LEFT_DRIVE_MOTOR_ID].orientation = (int)((temp * 9L) / 10L);
-  currentRotationMessage.ml_articulation_angle = currentWheelStatus[MIDDLE_LEFT_DRIVE_MOTOR_ID].orientation;
-
-  temp = EncoderRL::getPosition();
-  if (temp < 0)
-  {
-    temp += 400;
-  }
-  currentWheelStatus[REAR_LEFT_DRIVE_MOTOR_ID].orientation = (int)((temp * 9L) / 10L);
-  currentRotationMessage.rl_articulation_angle = currentWheelStatus[REAR_LEFT_DRIVE_MOTOR_ID].orientation;
-
-  temp = EncoderFR::getPosition() - 200; // Subtract 200 b/c the right side is flipped.
-  if (temp < 0)
-  {
-    temp += 400;
-  }
-  currentWheelStatus[FRONT_RIGHT_DRIVE_MOTOR_ID].orientation = (int)((temp * 9L) / 10L);
-  currentRotationMessage.fr_articulation_angle = currentWheelStatus[FRONT_RIGHT_DRIVE_MOTOR_ID].orientation;
-
-  temp = EncoderMR::getPosition() - 200;
-  if (temp < 0)
-  {
-    temp += 400;
-  }
-  currentWheelStatus[MIDDLE_RIGHT_DRIVE_MOTOR_ID].orientation = (int)((temp * 9L) / 10L);
-  currentRotationMessage.mr_articulation_angle = currentWheelStatus[MIDDLE_RIGHT_DRIVE_MOTOR_ID].orientation;
-
-  temp = EncoderRR::getPosition() - 200;
-  if (temp < 0)
-  {
-    temp += 400;
-  }
-
-  currentWheelStatus[REAR_RIGHT_DRIVE_MOTOR_ID].orientation = (int)((temp * 9L) / 10L);
-  currentRotationMessage.rr_articulation_angle = currentWheelStatus[REAR_RIGHT_DRIVE_MOTOR_ID].orientation;
-
-  pubCurrentRotation.publish(&currentRotationMessage);
-}

 /**
 * Is called on starting the arduino.
@@ -413,7 +384,7 @@
 */
 void setup(){

-  // Communicate with the computer
+  // Setup the encoders
   EncoderFL::setupEncoderFL();
   EncoderML::setupEncoderML();
   EncoderRL::setupEncoderRL();
@@ -421,58 +392,78 @@
   EncoderMR::setupEncoderMR();
   EncoderRR::setupEncoderRR();

+  //Initialize the ROS Node
   sabertoothDriverNode.initNode();
-  sabertoothDriverNode.subscribe(mcSUB);
-
-  // Communicate with the Sabertooth
+  sabertoothDriverNode.subscribe(commandSubscriber);
+  sabertoothDriverNode.advertise(pubwheelStatus);
+  sabertoothDriverNode.advertise(pubDebug);
+
+  // Open communication with Saberteeth
   Serial3.begin(9600);

   // Initialize the message
-  currentRotationMessage.fl_articulation_angle = 0;
-  currentRotationMessage.fr_articulation_angle = 180;
-  currentRotationMessage.ml_articulation_angle = 0;
-  currentRotationMessage.mr_articulation_angle = 180;
-  currentRotationMessage.rl_articulation_angle = 0;
-  currentRotationMessage.rr_articulation_angle = 180;
-
-  currentRotationMessage.fl_drive_speed = 0;
-  currentRotationMessage.fr_drive_speed = 0;
-  currentRotationMessage.ml_drive_speed = 0;
-  currentRotationMessage.mr_drive_speed = 0;
-  currentRotationMessage.rl_drive_speed = 0;
-  currentRotationMessage.rr_drive_speed = 0;
-
-  // Initialize current wheel status: Assume we're in "closed" position
-  currentWheelStatus[FRONT_LEFT_DRIVE_MOTOR_ID].orientation = 0.0;     //.fl_articulation_angle = 0.0;
-  currentWheelStatus[FRONT_RIGHT_DRIVE_MOTOR_ID].orientation = 180.0;   //.fr_articulation_angle = 180.0;
-  currentWheelStatus[MIDDLE_LEFT_DRIVE_MOTOR_ID].orientation = 0.0;//.ml_articulation_angle = 0.0;
-  currentWheelStatus[MIDDLE_RIGHT_DRIVE_MOTOR_ID].orientation = 180.0;//.mr_articulation_angle = 180.0;
-  currentWheelStatus[REAR_LEFT_DRIVE_MOTOR_ID].orientation = 0.0;//.rl_articulation_angle = 0.0;
-  currentWheelStatus[REAR_RIGHT_DRIVE_MOTOR_ID].orientation = 180.0; //.rr_articulation_angle = 180.0;
-
-  currentWheelStatus[FRONT_LEFT_DRIVE_MOTOR_ID].velocity = 0.0;//.fl_drive_speed = 0.0;
-  currentWheelStatus[FRONT_RIGHT_DRIVE_MOTOR_ID].velocity = 0.0;//.fr_drive_speed = 0.0;
-  currentWheelStatus[MIDDLE_LEFT_DRIVE_MOTOR_ID].velocity = 0.0;//.ml_drive_speed = 0.0;
-  currentWheelStatus[MIDDLE_RIGHT_DRIVE_MOTOR_ID].velocity = 0.0;//.mr_drive_speed = 0.0;
-  currentWheelStatus[REAR_LEFT_DRIVE_MOTOR_ID].velocity = 0.0;//.rl_drive_speed = 0.0;
-  currentWheelStatus[REAR_RIGHT_DRIVE_MOTOR_ID].velocity = 0.0;//.rr_drive_speed = 0.0;
-
-  // Publish our stuff
-  sabertoothDriverNode.advertise(pubResults);
-  sabertoothDriverNode.advertise(pubCurrentRotation);
+  setupWheelStatus();
+
 }

 // This program does whatever ROS directs it to do.
 // So far, it drives the robot forwards and backwards.
 void loop(){
+
+
+  //We are E-Stopped, don't respond to future commands.
+  if (currentStatus == E_STOPPED) {
+    return;
+  }
+
+  //Currently stopped, don't do anything
+  if (currentStatus == STOPPED) {
+    //Do Nothing
+  }
+
+  if (currentStatus == ARTICULATING) {
+    if (needsToArticulate() == true) {
+       articulateAllWheels();
+    } else {
+       currentStatus = START_DRIVING;
+    }
+  }
+
+  if (currentStatus == START_DRIVING) {
+    //TODO: Check with MEs about possibility of losing correct articulation (by going over an obstacle or something)
+
+    //Check if we need to articulate
+    // if (needsToArticulate()) {
+    //   //Set state to articulate and don't drive
+    //   currentStatus = ARTICULATING;
+    //   return;
+    // }
+
+
+    //Set stop drive time
+    driveStopTime = millis() + wheelTarget.drive_duration*1000;
+
+    //Send drive start command
+    driveAllMotors();
+
+    currentStatus = IS_DRIVING;
+  }
+
+  if (currentStatus == IS_DRIVING) {
+
+    if (millis() >= driveStopTime) {
+      stopAllMotors(false);
+    }
+
+
+  }
+
+  //Sync with ROS
+  sabertoothDriverNode.spinOnce(); // Check for subscriber update/update timestamp
+
+  //Delay so we don't overload any serial buffers
   delayMicroseconds(100000);
-  sabertoothDriverNode.spinOnce(); // Check for subscriber update/update timestamp
-  updateArticulationValues();
-  continueDriving();
-  // TODO: Delete this?
-  //  msg.data = (long)currentWheelStatus[FRONT_LEFT_DRIVE_MOTOR_ID].orientation;
-  //  pubResults.publish(&msg);
-
-}
-
-
+
+}
+
+


fixes #832

fixes #832

--- libraries/SD/examples/listfiles/listfiles.ino
+++ libraries/SD/examples/listfiles/listfiles.ino
@@ -77,6 +77,7 @@
        Serial.print("\t\t");
        Serial.println(entry.size(), DEC);
      }
+     entry.close();
    }
 }


Minor changes AM

Minor changes AM

--- Plex_Bot/Plex_Bot.ino
+++ Plex_Bot/Plex_Bot.ino
@@ -7,13 +7,16 @@

 Adafruit_PWMServoDriver driver = Adafruit_PWMServoDriver(0x40);

+//Makes the servo line up better
 int offSetList[16] = { -6,7,-6,4,0,5,6,7,8,9,10,2,-2,-2,-9,-9 };

 //{footMin, footMax, shinMin, shingMax, ....., hipMin, hipMax}
 int hardLegLimits[5 * 2] = { -25, 15, -40, 80, -40, 80, -40, 80, -15, 70 };

+//Pointer to the two walking routines for each leg
 int *routine[2] = { rWalk, lWalk };

+//Useful for setting leg to center
 int zero[5] = { 0 };

 /*


chmod

chmod


Fixed calculation of uv intensity

Fixed calculation of uv intensity

--- files/template_home_feinstaub/template_home_feinstaub.ino
+++ files/template_home_feinstaub/template_home_feinstaub.ino
@@ -132,7 +132,7 @@
   sleep(1);
   if (Wire.available()) lsb = Wire.read();
   uvValue = (msb << 8) | lsb;
-  return uvValue * 5;
+  return uvValue * 5.625;
 }

 int printHexToStream(const uint8_t *data, uint8_t length, Print &stream) // prints 8-bit data in hex
@@ -252,4 +252,4 @@
     sleep(5000);
     asm volatile (" jmp 0");
   }
-}+}


space optimisation of print()

space optimisation of print()

Removed need for print(float f) by casting state info to int - saves
about 500 bytes

--- software/firmware/LogoBotWifiScribbler/LogoBotWifiScribbler.ino
+++ software/firmware/LogoBotWifiScribbler/LogoBotWifiScribbler.ino
@@ -95,11 +95,11 @@
 static void showStatus()
 {
   // Format: "X Y ang qSize"
-  Serial.print(bot.state.x);
+  Serial.print((int)bot.state.x);
   Serial.print(" ");
-  Serial.print(bot.state.y);
+  Serial.print((int)bot.state.y);
   Serial.print(" ");
-  Serial.print(bot.state.ang);
+  Serial.print((int)bot.state.ang);
   Serial.print(" ");
   Serial.println(cmdQ.pending());
 }


Better display of information. Add icons for WiFi

--- esp8266_dht11_oled.ino
+++ esp8266_dht11_oled.ino
@@ -9,20 +9,6 @@

 #include "wifi_creds.h"

-#define DHTPIN            D5         // Pin which is connected to the DHT sensor.
-#define DHTTYPE           DHT11      // DHT 11
-
-DHT_Unified dht(DHTPIN, DHTTYPE);
-U8G2_SSD1306_128X32_UNIVISION_F_HW_I2C u8g2(U8G2_R0);
-#define SCREEN_WIDTH 128
-#define SCREEN_HEIGHT 32
-
-static char sensor_buffer[] = "TT,YY";
-String postData = String("temperature=");
-
-HTTPClient http;
-String localIP;
-
 #define FINAL 0

 #if FINAL
@@ -33,30 +19,86 @@
 #define DEBUG_PRINT(...)	Serial.print(__VA_ARGS__)
 #endif

-unsigned long lastWiFIBegin = 0;
-static const unsigned long INTERVAL_BETWEEN_CONNECT_MS = 5000;
+static unsigned int const DHTPIN = D5;      // Pin which is connected to the DHT sensor.
+static unsigned int const DHTTYPE = DHT11;      // DHT 11

-void checkWiFi()
+DHT_Unified dht(DHTPIN, DHTTYPE);
+U8G2_SSD1306_128X32_UNIVISION_F_HW_I2C u8g2(U8G2_R0);
+#define SCREEN_WIDTH 128
+#define SCREEN_HEIGHT 32
+
+String postData = String("temperature=");
+
+HTTPClient http;
+
+static const uint8_t WIFI_FRAMES[5][8] = {
+	0x0,0x0,0x0,0x0,0x0,0x0,0x1,0x1,
+	0x0,0x0,0x0,0x0,0x4,0x4,0x5,0x5,
+	0x0,0x0,0x10,0x10,0x14,0x14,0x15,0x15,
+	0x40,0x40,0x50,0x50,0x54,0x54,0x55,0x55,
+	0x40,0x40,0x40,0x51,0x4a,0x44,0x4a,0x51,
+};
+
+class WiFiConnection
 {
-	if (WiFi.status() != WL_CONNECTED)
+public:
+
+	WiFiConnection() : lastWiFiBegin(0) {}
+
+	void checkWiFi()
 	{
-		DEBUG_PRINTLN("Connecting...");
-		if (lastWiFIBegin == 0 || (millis() - lastWiFIBegin > INTERVAL_BETWEEN_CONNECT_MS))
+		if (WiFi.status() != WL_CONNECTED)
 		{
-			WiFi.begin(WIFI_NAME, WIFI_PASS);
-			lastWiFIBegin = millis();
+			DEBUG_PRINTLN("Connecting...");
+			if (lastWiFiBegin == 0 || (millis() - lastWiFiBegin > INTERVAL_BETWEEN_CONNECT_MS))
+			{
+				WiFi.begin(WIFI_NAME, WIFI_PASS);
+				lastWiFiBegin = millis();
+			}
+			localIP = "No connection.";
+			Serial.println(WiFi.status());
 		}
-		localIP = "No connection.";
-		Serial.println(WiFi.status());
+		else
+		{
+			localIP = WiFi.localIP().toString();
+			long rssi = WiFi.RSSI();
+			Serial.print("RSSI:");
+			Serial.println(rssi);
+		}
 	}
-	else
+
+	const bool isConnected()
 	{
-		localIP = WiFi.localIP().toString();
-		long rssi = WiFi.RSSI();
-		Serial.print("RSSI:");
-		Serial.println(rssi);
+		return WiFi.status() == WL_CONNECTED;
 	}
-}
+
+	const String& getPublicIP()
+	{
+		return localIP;
+	}
+
+	const unsigned int getSignalStrength()
+	{
+		if (isConnected())
+		{
+			if (WiFi.RSSI() > -100)
+				return (100 + WiFi.RSSI()) * 5 / 100;
+			else
+				return 0;
+		}
+		else
+		{
+			return 0;
+		}
+	}
+
+private:
+	unsigned long lastWiFiBegin;
+	String localIP;
+	static unsigned long const INTERVAL_BETWEEN_CONNECT_MS = 5000;
+};
+
+WiFiConnection wifiConnection;

 void setup()
 {
@@ -70,14 +112,7 @@

 	dht.begin();
 	u8g2.begin();
-	checkWiFi();
-	/*
-	while (WiFi.status() != WL_CONNECTED)
-	{
-		Serial.print(".");
-		delay(500);
-	}
-	*/
+	wifiConnection.checkWiFi();
 }

 void button_pressed() {
@@ -85,45 +120,47 @@
 }

 void loop() {
-	checkWiFi();
-	/*
-	int button = digitalRead(D3);
-	Serial.print("Button: ");
-	Serial.println(button);
-	// Get temperature event and print its value.
+	wifiConnection.checkWiFi();
+
+	String displayData;
 	sensors_event_t event;
 	dht.temperature().getEvent(&event);
 	if (isnan(event.temperature)) {
-		Serial.println("Error reading temperature!");
+		DEBUG_PRINTLN("Error reading temperature!");
+		displayData += "NaN";
 	}
 	else {
-		Serial.print("Temperature: ");
-		Serial.print(event.temperature);
-		Serial.println(" *C");
-		sensor_buffer[0] = '0' + (int)event.temperature / 10;
-		sensor_buffer[1] = '0' + (int)event.temperature % 10;
+		DEBUG_PRINT("Temperature: ");
+		DEBUG_PRINT(event.temperature);
+		DEBUG_PRINTLN(" *C");
+		displayData += (int)event.temperature;
+		displayData += "°C";
 	}
+	displayData += " ";
 	// Get humidity event and print its value.
 	dht.humidity().getEvent(&event);
 	if (isnan(event.relative_humidity)) {
-		Serial.println("Error reading humidity!");
+		DEBUG_PRINTLN("Error reading humidity!");
+		displayData += "NaN";
 	}
 	else {
-		Serial.print("Humidity: ");
-		Serial.print(event.relative_humidity);
-		Serial.println("%");
-		sensor_buffer[3] = '0' + (int)event.relative_humidity / 10;
-		sensor_buffer[4] = '0' + (int)event.relative_humidity % 10;
+		DEBUG_PRINT("Humidity: ");
+		DEBUG_PRINT(event.relative_humidity);
+		DEBUG_PRINTLN("%");
+		displayData += (int)event.relative_humidity;
+		displayData += "%";
 	}
-	*/
+
 	u8g2.clearBuffer();
-	u8g2.setFont(u8g2_font_5x7_tr);
-	unsigned int size = u8g2.getStrWidth(localIP.c_str());
-	u8g2.drawStr(SCREEN_WIDTH - size, SCREEN_HEIGHT - 1, localIP.c_str());
+	u8g2.setFont(u8g2_font_5x7_tf);
+	unsigned int size = u8g2.getStrWidth(wifiConnection.getPublicIP().c_str());
+	u8g2.drawStr(0, SCREEN_HEIGHT, wifiConnection.getPublicIP().c_str());
+	u8g2.setFont(u8g2_font_9x15B_tf);
+	u8g2.drawUTF8(0, SCREEN_HEIGHT/2, displayData.c_str());
+	static int frame = 0;
+	u8g2.drawXBM(SCREEN_WIDTH - 8, SCREEN_HEIGHT - 8, 8, 8, WIFI_FRAMES[frame]);
+	frame = (frame + 1) % 5;
 	u8g2.sendBuffer();
-
-	//u8g2.drawBitmap(0,10, 2, 16, )
-	   //u8g2.drawStr(64,24,humidity_buffer);

 	/*
 	  http.begin("http:///");


Arduino - work in progress on ETR69's CR95HF module

--- LBridge_Arduino_V09/LBridge_Arduino_V09.ino
+++ LBridge_Arduino_V09/LBridge_Arduino_V09.ino
@@ -48,7 +48,7 @@
 #define N_RELEASE_VERSION

 #define LB_DATETIME "180207_2208"
-#define LB_VERSION "V09"        // version number
+#define LB_VERSION "V0.9"        // version number
 #define LB_MINOR_VERSION ".01"  // indicates minor version

 #ifdef RELEASE_VERSION
@@ -482,6 +482,8 @@

 void Inventory_Command() {
   unsigned long ct;
+  int cnt = 0;
+
   digitalWrite(SSPin, LOW);
   SPI.transfer(0x00);  // SPI control byte to send command to CR95HF
   SPI.transfer(0x04);  // Send Receive CR95HF command
@@ -506,29 +508,37 @@
   digitalWrite(SSPin, HIGH);
   delay(1);

-  digitalWrite(SSPin, LOW);
-  SPI.transfer(0x02);   // SPI control byte for read
-  RXBuffer[0] = SPI.transfer(0);  // response code
-  RXBuffer[1] = SPI.transfer(0);  // length of data
-  for (byte i = 0; i < RXBuffer[1]; i++)
-    RXBuffer[i + 2] = SPI.transfer(0); // data
-  digitalWrite(SSPin, HIGH);
-  delay(1);
-
-  if (RXBuffer[0] == 128)  // is response code good?
-  {
-#ifdef DB_PROCESSING
-    print_state(F("Sensor in range ... OK"));
-#endif
-    NFCReady = 2;
-    sensor_oor = 0;
-  }
-  else
-  {
-    print_state(F("Sensor out of range"));
-    NFCReady = 1;
-    nfc_inv_cmd_errors++;
-    sensor_oor = 1;
+//  print_state(F("poll Inventory_Command successfull"));
+
+  while ( cnt++ < 10 ) {
+    digitalWrite(SSPin, LOW);
+    SPI.transfer(0x02);   // SPI control byte for read
+    RXBuffer[0] = SPI.transfer(0);  // response code
+    RXBuffer[1] = SPI.transfer(0);  // length of data
+    for (byte i = 0; i < RXBuffer[1]; i++)
+      RXBuffer[i + 2] = SPI.transfer(0); // data
+    digitalWrite(SSPin, HIGH);
+    delay(1);
+
+    if (RXBuffer[0] == 128)  // is response code good?
+    {
+  #ifdef DB_PROCESSING
+      print_state(F("Sensor in range ... OK"));
+  #endif
+      NFCReady = 2;
+      sensor_oor = 0;
+      break;
+    }
+    else
+    {
+      print_state(F("Sensor out of range"));
+      NFCReady = 1;
+      nfc_inv_cmd_errors++;
+      sensor_oor = 1;
+    }
+
+    delay(100);
+
   }
 }