small error fix

small error fix

--- arduino/Led_Strip/Steppers.ino
+++ arduino/Led_Strip/Steppers.ino
@@ -23,6 +23,7 @@
   for(int i = 0; i < AMOUNTSTEPPERS; i++){
   pinMode(steppers[i].directionPort, OUTPUT);
   pinMode(steppers[i].PWMPort, OUTPUT);
+  pinMode(steppers[i].caliPort, INPUT);
   calib(i);
   }
 }


Print downlink ack status in ttn-otaa example too

When a packet is received that has the ack flag set, print a notice
about that.

--- examples/ttn-otaa/ttn-otaa.ino
+++ examples/ttn-otaa/ttn-otaa.ino
@@ -104,6 +104,8 @@
             break;
         case EV_TXCOMPLETE:
             Serial.println(F("EV_TXCOMPLETE (includes waiting for RX windows)"));
+            if (LMIC.txrxFlags & TXRX_ACK)
+              Serial.println(F("Received ack"));
             if (LMIC.dataLen) {
               Serial.println(F("Received "));
               Serial.println(LMIC.dataLen);


Fixed Things...?

Fixed Things...?

--- scr_arduino/scr_arduino.ino
+++ scr_arduino/scr_arduino.ino
@@ -67,7 +67,7 @@
 Encoder L_DCMotorEncoder(LEFT_ENCODER_A_PIN, LEFT_ENCODER_B_PIN);
 Encoder R_DCMotorEncoder(RIGHT_ENCODER_A_PIN, RIGHT_ENCODER_B_PIN);

-Timer g_TimerEncoder;
+long last_encoder_time = micros();  // not the best way to get this done... should check existence in Update Wheel Function
 float L_EncoderVelocity = 0; // Radians per second
 long L_LastEncoderValue = 0;
 double L_EncoderDelta = 0;
@@ -110,6 +110,10 @@
   long L_encoderValue = L_DCMotorEncoder.read();
   long R_encoderValue = R_DCMotorEncoder.read();

+  long now = micros();
+
+  long dt = (now - last_encoder_time);
+
   // Get position in revolutions (includes multiple revolutions)
 // Serial.println(encoderValue);
   L_EncoderDelta = (double)(L_encoderValue - L_LastEncoderValue) * (1-beta) + L_LastEncoderDelta * beta;
@@ -117,8 +121,8 @@
 // Serial.println(g_EncoderAngle);

   // Get velocity in ticks/sec
-  L_EncoderVelocity = 1000.0*(double)(L_EncoderDelta)/(double)ENCODER_TIME_DELAY_MS;
-  R_EncoderVelocity = 1000.0*(double)(R_EncoderDelta)/(double)ENCODER_TIME_DELAY_MS;
+  L_EncoderVelocity = 1000000.0*(double)(L_EncoderDelta)/dt;
+  R_EncoderVelocity = 1000000.0*(double)(R_EncoderDelta)/dt;

   // Convert to revolutions/sec
   L_EncoderVelocity /= (double)TICKS_PER_REVOLUTION;
@@ -131,6 +135,8 @@
   // Set last known encoder ticks
   L_LastEncoderValue = L_encoderValue;
   R_LastEncoderValue = R_encoderValue;
+
+  last_encoder_time = now;

   L_LastEncoderDelta = L_EncoderDelta;
   R_LastEncoderDelta = R_EncoderDelta;


Update LIDARLite_PWM_GetDistance_ContinuousRead.ino

Big thank you to Steve G. for catching this... changed line 24 digitalWrite to pin 2 not 3

--- Arduino/LIDARLite_PWM_GetDistance_ContinuousRead/LIDARLite_PWM_GetDistance_ContinuousRead.ino
+++ Arduino/LIDARLite_PWM_GetDistance_ContinuousRead/LIDARLite_PWM_GetDistance_ContinuousRead.ino
@@ -21,7 +21,7 @@
   Serial.begin(9600); // Start serial communications
   pinMode(2, OUTPUT); // Set pin 2 as trigger pin
   pinMode(3, INPUT); // Set pin 3 as monitor pin
-  digitalWrite(3, LOW); // Set trigger LOW for continuous read
+  digitalWrite(2, LOW); // Set trigger LOW for continuous read
 }

 void loop()


Needs to have /config at the end

Needs to have /config at the end

--- Arilux_AL-LC0X.ino
+++ Arilux_AL-LC0X.ino
@@ -763,7 +763,7 @@
 #endif

 #ifdef HOME_ASSISTANT_MQTT_DISCOVERY
-  sprintf(HOME_ASSISTANT_MQTT_DISCOVERY_TOPIC,"%s/light/ARILUX_%s_%s_%s",HOME_ASSISTANT_MQTT_DISCOVERY_PREFIX,DEVICE_MODEL,arilux.getColorString(),chipid);
+  sprintf(HOME_ASSISTANT_MQTT_DISCOVERY_TOPIC,"%s/light/ARILUX_%s_%s_%s/config",HOME_ASSISTANT_MQTT_DISCOVERY_PREFIX,DEVICE_MODEL,arilux.getColorString(),chipid);
 #endif

 #ifdef JSON


Increases status screen display time, slightly optimises time keeping (xadow-watch)

--- xadow-watch.ino
+++ xadow-watch.ino
@@ -51,6 +51,7 @@
   menu_init();
   status_init();
   watch_init(now);
+  batlog_init(0x33);

   status_show();

@@ -68,9 +69,6 @@
   oled.drawString("then press WAKE", 0, LINE_HEIGHT, FONT_SIZE, COLOR_RED);
   while(digitalRead(10) == HIGH);;

-  now = 0;
-  checkpoint = millis();
-
   power_twi_disable();
   power_usart1_disable();
   power_usb_disable();
@@ -78,6 +76,9 @@
   power_timer1_disable();
   power_timer2_disable();
   power_timer3_disable();
+
+  now = 0;
+  checkpoint = millis();
 }

 void loop() {
@@ -108,7 +109,7 @@
   switch(nextscreen) {
     case SCREEN_STATUS:
       status_show();
-      delay(1000);
+      delay(3000);
       break;

     case SCREEN_WATCH:
@@ -117,6 +118,6 @@
   }

   uint16_t t = millis();
-  now += millis() - checkpoint + 200;
+  now += t - checkpoint + 200;
   checkpoint = t;
 }


[WIP] Converted display to SSD1306 over I2C

--- xadow-watch.ino
+++ xadow-watch.ino
@@ -3,18 +3,10 @@
 #include <EEPROM.h>

 #include "SGL.h"
-#include "SSD1331.h"
+#include "SSD1306.h"

 #include "defs.h"
 #include "xadow.h"
-
-// These are arduino pin definitions. Note that on leonardo
-// boards MOSI and sclk cannot be directly used, and hence
-// D16 and D15 is a thing only with the Xadow board
-#define cs                  (A5)
-#define dc                  (3)
-#define mosi                (16)
-#define sclk                (15)

 #define LEFT_GUTTER_SIZE    (FONT_X)
 #define FONT_SIZE           (1)
@@ -24,7 +16,10 @@

 #define REDRAW_FLAG         (0x80)

-SSD1331 oled = SSD1331(cs, dc, mosi, sclk);
+#define SCREEN_WIDTH        (128)
+#define SCREEN_HEIGHT       (64)
+
+SSD1306 oled = SSD1306(0x3C);

 char *test_menu[] = {"hello", "world", "here be", "dragons", NULL};

@@ -53,9 +48,13 @@

   Xadow.init();

+  buzzer_init();
   oled.init();
-  oled.fillScreen(COLOR_BLACK);
+
   animation_boot();
+
+  buzzer_jingle(0);
+  return;
   oled.fillScreen(COLOR_BLACK);

   button_init();
@@ -89,7 +88,8 @@
 }

 void loop() {
-  power_spi_disable();
+  return;
+  power_twi_disable();

   if (Xadow.getChrgState() == NOCHARGE) {
     // when on battery power disable usb and uart and
@@ -109,7 +109,7 @@
     delay(SLEEP_TIME_MS);
   }

-  power_spi_enable();
+  power_twi_enable();
   uint8_t changes = watch_tick(now);

   if (changes >= 3) buzzer_sound(BUZZER_SOUND_CHIME);


poking examples

poking examples

--- examples/Cylon/Cylon.ino
+++ examples/Cylon/Cylon.ino
@@ -6,7 +6,7 @@
 // For led chips like Neopixels, which have a data line, ground, and power, you just
 // need to define DATA_PIN.  For led chipsets that are SPI based (four wires - data, clock,
 // ground, and power), like the LPD8806, define both DATA_PIN and CLOCK_PIN
-#define DATA_PIN 6
+#define DATA_PIN 4
 #define CLOCK_PIN 13

 // Define the array of leds


Update basic.ino

Update basic.ino

--- examples/basic/basic.ino
+++ examples/basic/basic.ino
@@ -14,11 +14,17 @@
   tm.begin();
 }

+
 void loop() {
   randomLed2();
   uint32_t pbutton = tm.getButtons();
   if (pbutton != 0) {
-    Serial.println(pbutton,DEC);
+    for (byte i=24; i>=0; i--){
+      if (bitRead(pbutton,i) == 1){
+        Serial.println(i+1,DEC);
+        break;
+      }
+    }
   }
 }

@@ -57,6 +63,3 @@
   tm.setLed(led,val,true);
   delay(5);
 }
-
-
-


Bugfix

Bugfix

--- Arduino/RoverRemoteControlJoystick/RoverRemoteControlJoystick.ino
+++ Arduino/RoverRemoteControlJoystick/RoverRemoteControlJoystick.ino
@@ -246,7 +246,6 @@
               Serial.println("Steer right!");
               sendCommand('D');
             }
-            sendCommand('A');
             break;

        case STOP:


Fixed a typo in the winch code.

Fixed a typo in the winch code.

--- src/SabertoothDriverROS/SabertoothDriverROS.ino
+++ src/SabertoothDriverROS/SabertoothDriverROS.ino
@@ -194,7 +194,7 @@

 void newWinchCommandCallback(const std_msgs::Int16& newWinchCommand){
   if(newWinchCommand.data != 0){
-    currentStatus = START_ROTATING_WINCH;
+    currentWinchStatus = START_ROTATING_WINCH;
     winchRotationTime = newWinchCommand.data;
   }
 }
@@ -852,7 +852,7 @@
   if(currentWinchStatus == START_ROTATING_WINCH){
     winchStopTime = millis() + (long)abs(winchRotationTime);
     driveWinch();
-    currentWinchStatus = ROTATING_CONVEYOR;
+    currentWinchStatus = ROTATING_WINCH;
   }

   if(currentWinchStatus == ROTATING_WINCH){


Update code for status LEDs

Update code for status LEDs

--- mainWrite.ino
+++ mainWrite.ino
@@ -11,16 +11,16 @@
 uint16_t matrixValue;
 uint8_t matrixNibbles[4];

-uint8_t writeArduinoState; //TODO uint8_t?
+uint8_t writeArduinoState;

 boolean isSubmitButtonPressed;
-uint8_t buttonState = 0;         // variable for reading the pushbutton status
+uint8_t buttonState = 0;
 uint8_t lastButtonState;
 const uint8_t ledPin = 99;
 const uint8_t LED_UID = 3;
 const uint8_t LED_MSG = 4;
 const uint8_t LED_ERR = 13;
-const uint8_t bigButtonPin = 2;     // the number of the pushbutton pin
+const uint8_t bigButtonPin = 2;

 uint16_t validUserIDs[3][2] = {
   {4680, 3},
@@ -136,8 +136,8 @@
       break;
     case STATE_MSG:
       Serial.println("STATE_MSG");
-      digitalWrite(LED_UID, HIGH);
-      digitalWrite(LED_MSG, LOW);
+      digitalWrite(LED_UID, LOW);
+      digitalWrite(LED_MSG, HIGH);
       break;
     case STATE_ACK:
       Serial.println("STATE_ACK");
@@ -155,8 +155,11 @@
   Serial.begin(9600);
   Serial.println("Trellis Setup");

-  // initlize trellis board
+
+  // initlize trellis board & LEDs
   trellis.begin(0x70);
+  pinMode(LED_UID, OUTPUT);
+  pinMode(LED_MSG, OUTPUT);

   // light up trellis board sequentially to indicate initalization
   for (uint8_t i=0; i<16; i++) {
@@ -214,18 +217,18 @@
         }
       break;
     case STATE_ACK: // Waiting for acknowledge
-      // if(isMessageReceived) {
+//      if(isMessageReceived) {
         // parseCRAP();
-        // if(isMessageSuccessful) {
-          // move to state 1
-        // } else {
+//        if(isMessageSuccessful) {
+          updateReadArduinoStatus(STATE_UID);
+//        } else {
           // light error led
           // flash 4x4 a few times
           updateReadArduinoStatus(STATE_UID);
           // move to state 1
-        // }
-      // }
-       break;
+//        }
+//      }
+      break;
     default:
       break;
   }


Small fix?

Small fix?

--- Kleineswetter/Kleineswetter.ino
+++ Kleineswetter/Kleineswetter.ino
@@ -35,7 +35,7 @@

 unsigned long connectInvainThreshold = 8 * 60 * 1000L; // some minutes
 unsigned int connectTryThreshold = 7000;
-unsigned int shiftStartSeconds = 6;
+int shiftStartSeconds = 6;

 void setup() {
   unsigned long systemStart = millis();


Movement of cards to the top row.

Movement of cards to the top row.

--- src/solitaire/solitaire.ino
+++ src/solitaire/solitaire.ino
@@ -171,17 +171,17 @@
     }
   }
   if (gb.buttons.pressed(BTN_LEFT)) {
-    if (activeLocation != stock && activeLocation != tableau1) {
+    if (activeLocation != talon && activeLocation != tableau1) {
       activeLocation = activeLocation - 1;
     }
   }
   if (gb.buttons.pressed(BTN_DOWN)) {
-    if (activeLocation < foundation1) activeLocation = activeLocation + 6;
-    else if (activeLocation <= foundation4) activeLocation = activeLocation + 7;
+    if (activeLocation == talon) activeLocation = tableau2;
+    else if (activeLocation <= foundation3) activeLocation = activeLocation + 7;
   }
   if (gb.buttons.pressed(BTN_UP)) {
-    if (activeLocation > tableau2) activeLocation = activeLocation - 7;
-    else if (activeLocation >= tableau1) activeLocation = activeLocation - 6;
+    if (activeLocation >= tableau4) activeLocation = activeLocation - 7;
+    else if (activeLocation >= tableau1) activeLocation = talon;
   }
   if (gb.buttons.pressed(BTN_A)) {
     switch (activeLocation) {
@@ -493,7 +493,22 @@

 void drawMovingPile() {
   drawPile(&moving);
+  Pile* pile = getActiveLocationPile();
+  moving.x = updatePosition(moving.x, pile->x);
+  moving.y = updatePosition(moving.y, pile->y + 2 * pile->getCardCount() + 2);
+}
+
+Pile* getActiveLocationPile() {
   switch (activeLocation) {
+    case stock:
+      return &stockDeck;
+    case talon:
+      return &talonDeck;
+    case foundation1:
+    case foundation2:
+    case foundation3:
+    case foundation4:
+      return &foundations[activeLocation - foundation1];
     case tableau1:
     case tableau2:
     case tableau3:
@@ -501,9 +516,7 @@
     case tableau5:
     case tableau6:
     case tableau7:
-      moving.x = updatePosition(moving.x, tableau[activeLocation - tableau1].x);
-      moving.y = updatePosition(moving.y, tableau[activeLocation - tableau1].y + 2 * tableau[activeLocation - tableau1].getCardCount() + 2);
-      break;
+      return &tableau[activeLocation - tableau1];
   }
 }


Version 2.3 - 2nd June 2018

Version 2.3 - 2nd June 2018

Includes Lisp Library feature

--- ulisp.ino
+++ ulisp.ino
@@ -1,5 +1,5 @@
-/* uLisp AVR Version 2.2 - www.ulisp.com
-   David Johnson-Davies - www.technoblogy.com - 19th May 2018
+/* uLisp AVR Version 2.3 - www.ulisp.com
+   David Johnson-Davies - www.technoblogy.com - 2nd June 2018

    Licensed under the MIT license: https://opensource.org/licenses/MIT
 */
@@ -12,9 +12,11 @@
 #define serialmonitor
 // #define printgcs
 // #define sdcardsupport
+// #define lisplibrary

 // Includes

+// #include "LispLibrary.h"
 #include <avr/sleep.h>
 #include <setjmp.h>
 #include <SPI.h>
@@ -41,7 +43,7 @@
 #define push(x, y)         ((y) = cons((x),(y)))
 #define pop(y)             ((y) = cdr(y))

-#define numberp(x)         ((x) != NULL && (x)->type == NUMBER)
+#define integerp(x)        ((x) != NULL && (x)->type == NUMBER)
 #define symbolp(x)         ((x) != NULL && (x)->type == SYMBOL)
 #define stringp(x)         ((x) != NULL && (x)->type == STRING)
 #define characterp(x)      ((x) != NULL && (x)->type == CHARACTER)
@@ -66,16 +68,16 @@
 enum function { SYMBOLS, NIL, TEE, NOTHING, AMPREST, LAMBDA, LET, LETSTAR, CLOSURE, SPECIAL_FORMS, QUOTE,
 DEFUN, DEFVAR, SETQ, LOOP, PUSH, POP, INCF, DECF, SETF, DOLIST, DOTIMES, TRACE, UNTRACE, FORMILLIS,
 WITHSERIAL, WITHI2C, WITHSPI, WITHSDCARD, TAIL_FORMS, PROGN, RETURN, IF, COND, WHEN, UNLESS, AND, OR,
-FUNCTIONS, NOT, NULLFN, CONS, ATOM, LISTP, CONSP, NUMBERP, SYMBOLP, STREAMP, EQ, CAR, FIRST, CDR, REST,
-CAAR, CADR, SECOND, CDAR, CDDR, CAAAR, CAADR, CADAR, CADDR, THIRD, CDAAR, CDADR, CDDAR, CDDDR, LENGTH,
-LIST, REVERSE, NTH, ASSOC, MEMBER, APPLY, FUNCALL, APPEND, MAPC, MAPCAR, ADD, SUBTRACT, MULTIPLY, DIVIDE,
-MOD, ONEPLUS, ONEMINUS, ABS, RANDOM, MAX, MIN, NUMEQ, LESS, LESSEQ, GREATER, GREATEREQ, NOTEQ, PLUSP,
-MINUSP, ZEROP, ODDP, EVENP, CHAR, CHARCODE, CODECHAR, CHARACTERP, STRINGP, STRINGEQ, STRINGLESS,
+FUNCTIONS, NOT, NULLFN, CONS, ATOM, LISTP, CONSP, SYMBOLP, STREAMP, EQ, CAR, FIRST, CDR, REST, CAAR, CADR,
+SECOND, CDAR, CDDR, CAAAR, CAADR, CADAR, CADDR, THIRD, CDAAR, CDADR, CDDAR, CDDDR, LENGTH, LIST, REVERSE,
+NTH, ASSOC, MEMBER, APPLY, FUNCALL, APPEND, MAPC, MAPCAR, ADD, SUBTRACT, MULTIPLY, DIVIDE, MOD, ONEPLUS,
+ONEMINUS, ABS, RANDOM, MAXFN, MINFN, NOTEQ, NUMEQ, LESS, LESSEQ, GREATER, GREATEREQ, PLUSP, MINUSP, ZEROP,
+ODDP, EVENP, INTEGERP, NUMBERP, CHAR, CHARCODE, CODECHAR, CHARACTERP, STRINGP, STRINGEQ, STRINGLESS,
 STRINGGREATER, SORT, STRINGFN, CONCATENATE, SUBSEQ, READFROMSTRING, PRINCTOSTRING, PRIN1TOSTRING, LOGAND,
 LOGIOR, LOGXOR, LOGNOT, ASH, LOGBITP, EVAL, GLOBALS, LOCALS, MAKUNBOUND, BREAK, READ, PRIN1, PRINT, PRINC,
 TERPRI, READBYTE, READLINE, WRITEBYTE, WRITESTRING, WRITELINE, RESTARTI2C, GC, ROOM, SAVEIMAGE, LOADIMAGE,
 CLS, PINMODE, DIGITALREAD, DIGITALWRITE, ANALOGREAD, ANALOGWRITE, DELAY, MILLIS, SLEEP, NOTE, EDIT,
-PPRINT, ENDFUNCTIONS };
+PPRINT, PPRINTALL, ENDFUNCTIONS };

 // Typedefs

@@ -167,7 +169,6 @@
 char *lookupbuiltin (symbol_t name);
 intptr_t lookupfn (symbol_t name);
 int builtin (char* n);
-void Display (char c);

 // Set up workspace

@@ -529,17 +530,17 @@
 }

 int integer (object *obj){
-  if (!numberp(obj)) error(PSTR("Not a number"));
+  if (!integerp(obj)) error2(obj, PSTR("is not an integer"));
   return obj->integer;
 }

 int fromchar (object *obj){
-  if (!characterp(obj)) error(PSTR("Not a character"));
+  if (!characterp(obj)) error2(obj, PSTR("is not a character"));
   return obj->integer;
 }

 int istream (object *obj){
-  if (!streamp(obj)) error(PSTR("Not a stream"));
+  if (!streamp(obj)) error2(obj, PSTR("is not a stream"));
   return obj->integer;
 }

@@ -552,7 +553,7 @@
   if ((arg1 == nil) || (arg2 == nil)) return false;  // Not both values
   if (arg1->cdr != arg2->cdr) return false;  // Different values
   if (symbolp(arg1) && symbolp(arg2)) return true;  // Same symbol
-  if (numberp(arg1) && numberp(arg2)) return true;  // Same number
+  if (integerp(arg1) && integerp(arg2)) return true;  // Same integer
   if (characterp(arg1) && characterp(arg2)) return true;  // Same character
   return false;
 }
@@ -675,7 +676,7 @@
   symbol_t varname = var->name;
   object *pair = value(varname, env);
   if (pair == NULL) pair = value(varname, GlobalEnv);
-  if (pair == NULL) error2(var,PSTR("unknown variable"));
+  if (pair == NULL) error2(var, PSTR("unknown variable"));
   return pair;
 }

@@ -1161,6 +1162,8 @@
   pop(*loc);
   return result;
 }
+
+// Special forms incf/decf

 object *sp_incf (object *args, object *env) {
   object **loc = place(first(args), env);
@@ -1320,7 +1323,7 @@
   I2CCount = 0;
   if (params != NULL) {
     object *rw = eval(first(params), env);
-    if (numberp(rw)) I2CCount = integer(rw);
+    if (integerp(rw)) I2CCount = integer(rw);
     read = (rw != NULL);
   }
   I2Cinit(1); // Pullups
@@ -1494,12 +1497,6 @@
   return consp(first(args)) ? tee : nil;
 }

-object *fn_numberp (object *args, object *env) {
-  (void) env;
-  object *arg = first(args);
-  return numberp(arg) ? tee : nil;
-}
-
 object *fn_symbolp (object *args, object *env) {
   (void) env;
   object *arg = first(args);
@@ -1869,7 +1866,7 @@
   return number(random(arg));
 }

-object *fn_max (object *args, object *env) {
+object *fn_maxfn (object *args, object *env) {
   (void) env;
   int result = integer(first(args));
   args = cdr(args);
@@ -1881,7 +1878,7 @@
   return number(result);
 }

-object *fn_min (object *args, object *env) {
+object *fn_minfn (object *args, object *env) {
   (void) env;
   int result = integer(first(args));
   args = cdr(args);
@@ -1894,71 +1891,6 @@
 }

 // Arithmetic comparisons
-
-object *fn_numeq (object *args, object *env) {
-  (void) env;
-  int arg1 = integer(first(args));
-  args = cdr(args);
-  while (args != NULL) {
-    int arg2 = integer(first(args));
-    if (!(arg1 == arg2)) return nil;
-    arg1 = arg2;
-    args = cdr(args);
-  }
-  return tee;
-}
-
-object *fn_less (object *args, object *env) {
-  (void) env;
-  int arg1 = integer(first(args));
-  args = cdr(args);
-  while (args != NULL) {
-    int arg2 = integer(first(args));
-    if (!(arg1 < arg2)) return nil;
-    arg1 = arg2;
-    args = cdr(args);
-  }
-  return tee;
-}
-
-object *fn_lesseq (object *args, object *env) {
-  (void) env;
-  int arg1 = integer(first(args));
-  args = cdr(args);
-  while (args != NULL) {
-    int arg2 = integer(first(args));
-    if (!(arg1 <= arg2)) return nil;
-    arg1 = arg2;
-    args = cdr(args);
-  }
-  return tee;
-}
-
-object *fn_greater (object *args, object *env) {
-  (void) env;
-  int arg1 = integer(first(args));
-  args = cdr(args);
-  while (args != NULL) {
-    int arg2 = integer(first(args));
-    if (!(arg1 > arg2)) return nil;
-    arg1 = arg2;
-    args = cdr(args);
-  }
-  return tee;
-}
-
-object *fn_greatereq (object *args, object *env) {
-  (void) env;
-  int arg1 = integer(first(args));
-  args = cdr(args);
-  while (args != NULL) {
-    int arg2 = integer(first(args));
-    if (!(arg1 >= arg2)) return nil;
-    arg1 = arg2;
-    args = cdr(args);
-  }
-  return tee;
-}

 object *fn_noteq (object *args, object *env) {
   (void) env;
@@ -1976,6 +1908,71 @@
   return tee;
 }

+object *fn_numeq (object *args, object *env) {
+  (void) env;
+  int arg1 = integer(first(args));
+  args = cdr(args);
+  while (args != NULL) {
+    int arg2 = integer(first(args));
+    if (!(arg1 == arg2)) return nil;
+    arg1 = arg2;
+    args = cdr(args);
+  }
+  return tee;
+}
+
+object *fn_less (object *args, object *env) {
+  (void) env;
+  int arg1 = integer(first(args));
+  args = cdr(args);
+  while (args != NULL) {
+    int arg2 = integer(first(args));
+    if (!(arg1 < arg2)) return nil;
+    arg1 = arg2;
+    args = cdr(args);
+  }
+  return tee;
+}
+
+object *fn_lesseq (object *args, object *env) {
+  (void) env;
+  int arg1 = integer(first(args));
+  args = cdr(args);
+  while (args != NULL) {
+    int arg2 = integer(first(args));
+    if (!(arg1 <= arg2)) return nil;
+    arg1 = arg2;
+    args = cdr(args);
+  }
+  return tee;
+}
+
+object *fn_greater (object *args, object *env) {
+  (void) env;
+  int arg1 = integer(first(args));
+  args = cdr(args);
+  while (args != NULL) {
+    int arg2 = integer(first(args));
+    if (!(arg1 > arg2)) return nil;
+    arg1 = arg2;
+    args = cdr(args);
+  }
+  return tee;
+}
+
+object *fn_greatereq (object *args, object *env) {
+  (void) env;
+  int arg1 = integer(first(args));
+  args = cdr(args);
+  while (args != NULL) {
+    int arg2 = integer(first(args));
+    if (!(arg1 >= arg2)) return nil;
+    arg1 = arg2;
+    args = cdr(args);
+  }
+  return tee;
+}
+
 object *fn_plusp (object *args, object *env) {
   (void) env;
   int arg = integer(first(args));
@@ -1993,22 +1990,29 @@
 object *fn_zerop (object *args, object *env) {
   (void) env;
   int arg = integer(first(args));
-  if (arg == 0) return tee;
-  else return nil;
+  return (arg == 0) ? tee : nil;
 }

 object *fn_oddp (object *args, object *env) {
   (void) env;
-  int arg = integer(first(args));
-  if ((arg & 1) == 1) return tee;
-  else return nil;
+  return ((integer(first(args)) & 1) == 1) ? tee : nil;
 }

 object *fn_evenp (object *args, object *env) {
   (void) env;
-  int arg = integer(first(args));
-  if ((arg & 1) == 0) return tee;
-  else return nil;
+  return ((integer(first(args)) & 1) == 0) ? tee : nil;
+}
+
+// Number functions
+
+object *fn_integerp (object *args, object *env) {
+  (void) env;
+  return integerp(first(args)) ? tee : nil;
+}
+
+object *fn_numberp (object *args, object *env) {
+  (void) env;
+  return integerp(first(args)) ? tee : nil;
 }

 // Characters
@@ -2131,7 +2135,7 @@
   (void) env;
   object *arg = first(args);
   symbol_t name = arg->name;
-  if (name != STRINGFN) error(PSTR("Only string result supported"));
+  if (name != STRINGFN) error(PSTR("'concatenate' only supports strings"));
   args = cdr(args);
   object *result = myalloc();
   result->type = STRING;
@@ -2409,7 +2413,7 @@
   I2CCount = 0;
   if (args != NULL) {
     object *rw = first(args);
-    if (numberp(rw)) I2CCount = integer(rw);
+    if (integerp(rw)) I2CCount = integer(rw);
     read = (rw != NULL);
   }
   int address = stream & 0xFF;
@@ -2458,7 +2462,7 @@
   (void) env;
   int pin = integer(first(args));
   object *mode = second(args);
-  if (numberp(mode)) pinMode(pin, mode->integer);
+  if (integerp(mode)) pinMode(pin, mode->integer);
   else pinMode(pin, (mode != nil));
   return nil;
 }
@@ -2473,7 +2477,7 @@
   (void) env;
   int pin = integer(first(args));
   object *mode = second(args);
-  if (numberp(mode)) digitalWrite(pin, mode->integer);
+  if (integerp(mode)) digitalWrite(pin, mode->integer);
   else digitalWrite(pin, (mode != nil));
   return mode;
 }
@@ -2560,21 +2564,16 @@
 const int PPINDENT = 2;
 const int PPWIDTH = 80;

+void pcount (char c) {
+  LastPrint = c;
+  if (c == '\n') GlobalStringIndex++;
+  GlobalStringIndex++;
+}
+
 int atomwidth (object *obj) {
-  if (obj == NULL) return 3;
-  if (numberp(obj)) {
-    int w = 1;
-    int n = obj->integer;
-    if (n < 0) { n = -n; w++; }
-    while (n >= 10) { n = n/10; w++; }
-    return w;
-  }
-  if (stringp(obj)) return stringlength(obj);
-  if (characterp(obj)) return 3; // Interim solution
-  int w = 0;
-  char *s = name(obj);
-  while (*s++) w++;
-  return w;
+  GlobalStringIndex = 0;
+  printobject(obj, pcount);
+  return GlobalStringIndex;
 }

 boolean quoted (object *obj) {
@@ -2597,7 +2596,10 @@
 }

 void superprint (object *form, int lm, pfun_t pfun) {
-  if (atom(form)) printobject(form, pfun);
+  if (atom(form)) {
+    if (form->name == NOTHING) pstring(name(form), pfun);
+    else printobject(form, pfun);
+  }
   else if (quoted(form)) { pfun('\''); superprint(car(cdr(form)), lm + 1, pfun); }
   else if (subwidth(form, PPWIDTH - lm) >= 0) supersub(form, lm + PPINDENT, 0, pfun);
   else supersub(form, lm + PPINDENT, 1, pfun);
@@ -2635,6 +2637,25 @@
   pfun_t pfun = pstreamfun(cdr(args));
   pln(pfun);
   superprint(obj, 0, pfun);
+  return symbol(NOTHING);
+}
+
+object *fn_pprintall (object *args, object *env) {
+  (void) args, (void) env;
+  object *globals = GlobalEnv;
+  while (globals != NULL) {
+    object *pair = first(globals);
+    object *var = car(pair);
+    object *val = cdr(pair);
+    object *head = car(val);
+    object *function = cdr(val);
+    if (head->name == LAMBDA) {
+      pln(pserial);
+      superprint(cons(symbol(DEFUN), cons(var, function)), 0, pserial);
+      pln(pserial);
+    }
+    globals = cdr(globals);
+  }
   return symbol(NOTHING);
 }

@@ -2687,114 +2708,116 @@
 const char string42[] PROGMEM = "atom";
 const char string43[] PROGMEM = "listp";
 const char string44[] PROGMEM = "consp";
-const char string45[] PROGMEM = "numberp";
-const char string46[] PROGMEM = "symbolp";
-const char string47[] PROGMEM = "streamp";
-const char string48[] PROGMEM = "eq";
-const char string49[] PROGMEM = "car";
-const char string50[] PROGMEM = "first";
-const char string51[] PROGMEM = "cdr";
-const char string52[] PROGMEM = "rest";
-const char string53[] PROGMEM = "caar";
-const char string54[] PROGMEM = "cadr";
-const char string55[] PROGMEM = "second";
-const char string56[] PROGMEM = "cdar";
-const char string57[] PROGMEM = "cddr";
-const char string58[] PROGMEM = "caaar";
-const char string59[] PROGMEM = "caadr";
-const char string60[] PROGMEM = "cadar";
-const char string61[] PROGMEM = "caddr";
-const char string62[] PROGMEM = "third";
-const char string63[] PROGMEM = "cdaar";
-const char string64[] PROGMEM = "cdadr";
-const char string65[] PROGMEM = "cddar";
-const char string66[] PROGMEM = "cdddr";
-const char string67[] PROGMEM = "length";
-const char string68[] PROGMEM = "list";
-const char string69[] PROGMEM = "reverse";
-const char string70[] PROGMEM = "nth";
-const char string71[] PROGMEM = "assoc";
-const char string72[] PROGMEM = "member";
-const char string73[] PROGMEM = "apply";
-const char string74[] PROGMEM = "funcall";
-const char string75[] PROGMEM = "append";
-const char string76[] PROGMEM = "mapc";
-const char string77[] PROGMEM = "mapcar";
-const char string78[] PROGMEM = "+";
-const char string79[] PROGMEM = "-";
-const char string80[] PROGMEM = "*";
-const char string81[] PROGMEM = "/";
-const char string82[] PROGMEM = "mod";
-const char string83[] PROGMEM = "1+";
-const char string84[] PROGMEM = "1-";
-const char string85[] PROGMEM = "abs";
-const char string86[] PROGMEM = "random";
-const char string87[] PROGMEM = "max";
-const char string88[] PROGMEM = "min";
+const char string45[] PROGMEM = "symbolp";
+const char string46[] PROGMEM = "streamp";
+const char string47[] PROGMEM = "eq";
+const char string48[] PROGMEM = "car";
+const char string49[] PROGMEM = "first";
+const char string50[] PROGMEM = "cdr";
+const char string51[] PROGMEM = "rest";
+const char string52[] PROGMEM = "caar";
+const char string53[] PROGMEM = "cadr";
+const char string54[] PROGMEM = "second";
+const char string55[] PROGMEM = "cdar";
+const char string56[] PROGMEM = "cddr";
+const char string57[] PROGMEM = "caaar";
+const char string58[] PROGMEM = "caadr";
+const char string59[] PROGMEM = "cadar";
+const char string60[] PROGMEM = "caddr";
+const char string61[] PROGMEM = "third";
+const char string62[] PROGMEM = "cdaar";
+const char string63[] PROGMEM = "cdadr";
+const char string64[] PROGMEM = "cddar";
+const char string65[] PROGMEM = "cdddr";
+const char string66[] PROGMEM = "length";
+const char string67[] PROGMEM = "list";
+const char string68[] PROGMEM = "reverse";
+const char string69[] PROGMEM = "nth";
+const char string70[] PROGMEM = "assoc";
+const char string71[] PROGMEM = "member";
+const char string72[] PROGMEM = "apply";
+const char string73[] PROGMEM = "funcall";
+const char string74[] PROGMEM = "append";
+const char string75[] PROGMEM = "mapc";
+const char string76[] PROGMEM = "mapcar";
+const char string77[] PROGMEM = "+";
+const char string78[] PROGMEM = "-";
+const char string79[] PROGMEM = "*";
+const char string80[] PROGMEM = "/";
+const char string81[] PROGMEM = "mod";
+const char string82[] PROGMEM = "1+";
+const char string83[] PROGMEM = "1-";
+const char string84[] PROGMEM = "abs";
+const char string85[] PROGMEM = "random";
+const char string86[] PROGMEM = "max";
+const char string87[] PROGMEM = "min";
+const char string88[] PROGMEM = "/=";
 const char string89[] PROGMEM = "=";
 const char string90[] PROGMEM = "<";
 const char string91[] PROGMEM = "<=";
 const char string92[] PROGMEM = ">";
 const char string93[] PROGMEM = ">=";
-const char string94[] PROGMEM = "/=";
-const char string95[] PROGMEM = "plusp";
-const char string96[] PROGMEM = "minusp";
-const char string97[] PROGMEM = "zerop";
-const char string98[] PROGMEM = "oddp";
-const char string99[] PROGMEM = "evenp";
-const char string100[] PROGMEM = "char";
-const char string101[] PROGMEM = "char-code";
-const char string102[] PROGMEM = "code-char";
-const char string103[] PROGMEM = "characterp";
-const char string104[] PROGMEM = "stringp";
-const char string105[] PROGMEM = "string=";
-const char string106[] PROGMEM = "string<";
-const char string107[] PROGMEM = "string>";
-const char string108[] PROGMEM = "sort";
-const char string109[] PROGMEM = "string";
-const char string110[] PROGMEM = "concatenate";
-const char string111[] PROGMEM = "subseq";
-const char string112[] PROGMEM = "read-from-string";
-const char string113[] PROGMEM = "princ-to-string";
-const char string114[] PROGMEM = "prin1-to-string";
-const char string115[] PROGMEM = "logand";
-const char string116[] PROGMEM = "logior";
-const char string117[] PROGMEM = "logxor";
-const char string118[] PROGMEM = "lognot";
-const char string119[] PROGMEM = "ash";
-const char string120[] PROGMEM = "logbitp";
-const char string121[] PROGMEM = "eval";
-const char string122[] PROGMEM = "globals";
-const char string123[] PROGMEM = "locals";
-const char string124[] PROGMEM = "makunbound";
-const char string125[] PROGMEM = "break";
-const char string126[] PROGMEM = "read";
-const char string127[] PROGMEM = "prin1";
-const char string128[] PROGMEM = "print";
-const char string129[] PROGMEM = "princ";
-const char string130[] PROGMEM = "terpri";
-const char string131[] PROGMEM = "read-byte";
-const char string132[] PROGMEM = "read-line";
-const char string133[] PROGMEM = "write-byte";
-const char string134[] PROGMEM = "write-string";
-const char string135[] PROGMEM = "write-line";
-const char string136[] PROGMEM = "restart-i2c";
-const char string137[] PROGMEM = "gc";
-const char string138[] PROGMEM = "room";
-const char string139[] PROGMEM = "save-image";
-const char string140[] PROGMEM = "load-image";
-const char string141[] PROGMEM = "cls";
-const char string142[] PROGMEM = "pinmode";
-const char string143[] PROGMEM = "digitalread";
-const char string144[] PROGMEM = "digitalwrite";
-const char string145[] PROGMEM = "analogread";
-const char string146[] PROGMEM = "analogwrite";
-const char string147[] PROGMEM = "delay";
-const char string148[] PROGMEM = "millis";
-const char string149[] PROGMEM = "sleep";
-const char string150[] PROGMEM = "note";
-const char string151[] PROGMEM = "edit";
-const char string152[] PROGMEM = "pprint";
+const char string94[] PROGMEM = "plusp";
+const char string95[] PROGMEM = "minusp";
+const char string96[] PROGMEM = "zerop";
+const char string97[] PROGMEM = "oddp";
+const char string98[] PROGMEM = "evenp";
+const char string99[] PROGMEM = "integerp";
+const char string100[] PROGMEM = "numberp";
+const char string101[] PROGMEM = "char";
+const char string102[] PROGMEM = "char-code";
+const char string103[] PROGMEM = "code-char";
+const char string104[] PROGMEM = "characterp";
+const char string105[] PROGMEM = "stringp";
+const char string106[] PROGMEM = "string=";
+const char string107[] PROGMEM = "string<";
+const char string108[] PROGMEM = "string>";
+const char string109[] PROGMEM = "sort";
+const char string110[] PROGMEM = "string";
+const char string111[] PROGMEM = "concatenate";
+const char string112[] PROGMEM = "subseq";
+const char string113[] PROGMEM = "read-from-string";
+const char string114[] PROGMEM = "princ-to-string";
+const char string115[] PROGMEM = "prin1-to-string";
+const char string116[] PROGMEM = "logand";
+const char string117[] PROGMEM = "logior";
+const char string118[] PROGMEM = "logxor";
+const char string119[] PROGMEM = "lognot";
+const char string120[] PROGMEM = "ash";
+const char string121[] PROGMEM = "logbitp";
+const char string122[] PROGMEM = "eval";
+const char string123[] PROGMEM = "globals";
+const char string124[] PROGMEM = "locals";
+const char string125[] PROGMEM = "makunbound";
+const char string126[] PROGMEM = "break";
+const char string127[] PROGMEM = "read";
+const char string128[] PROGMEM = "prin1";
+const char string129[] PROGMEM = "print";
+const char string130[] PROGMEM = "princ";
+const char string131[] PROGMEM = "terpri";
+const char string132[] PROGMEM = "read-byte";
+const char string133[] PROGMEM = "read-line";
+const char string134[] PROGMEM = "write-byte";
+const char string135[] PROGMEM = "write-string";
+const char string136[] PROGMEM = "write-line";
+const char string137[] PROGMEM = "restart-i2c";
+const char string138[] PROGMEM = "gc";
+const char string139[] PROGMEM = "room";
+const char string140[] PROGMEM = "save-image";
+const char string141[] PROGMEM = "load-image";
+const char string142[] PROGMEM = "cls";
+const char string143[] PROGMEM = "pinmode";
+const char string144[] PROGMEM = "digitalread";
+const char string145[] PROGMEM = "digitalwrite";
+const char string146[] PROGMEM = "analogread";
+const char string147[] PROGMEM = "analogwrite";
+const char string148[] PROGMEM = "delay";
+const char string149[] PROGMEM = "millis";
+const char string150[] PROGMEM = "sleep";
+const char string151[] PROGMEM = "note";
+const char string152[] PROGMEM = "edit";
+const char string153[] PROGMEM = "pprint";
+const char string154[] PROGMEM = "pprintall";

 const tbl_entry_t lookup_table[] PROGMEM = {
   { string0, NULL, NIL, NIL },
@@ -2842,114 +2865,116 @@
   { string42, fn_atom, 1, 1 },
   { string43, fn_listp, 1, 1 },
   { string44, fn_consp, 1, 1 },
-  { string45, fn_numberp, 1, 1 },
-  { string46, fn_symbolp, 1, 1 },
-  { string47, fn_streamp, 1, 1 },
-  { string48, fn_eq, 2, 2 },
+  { string45, fn_symbolp, 1, 1 },
+  { string46, fn_streamp, 1, 1 },
+  { string47, fn_eq, 2, 2 },
+  { string48, fn_car, 1, 1 },
   { string49, fn_car, 1, 1 },
-  { string50, fn_car, 1, 1 },
+  { string50, fn_cdr, 1, 1 },
   { string51, fn_cdr, 1, 1 },
-  { string52, fn_cdr, 1, 1 },
-  { string53, fn_caar, 1, 1 },
+  { string52, fn_caar, 1, 1 },
+  { string53, fn_cadr, 1, 1 },
   { string54, fn_cadr, 1, 1 },
-  { string55, fn_cadr, 1, 1 },
-  { string56, fn_cdar, 1, 1 },
-  { string57, fn_cddr, 1, 1 },
-  { string58, fn_caaar, 1, 1 },
-  { string59, fn_caadr, 1, 1 },
-  { string60, fn_cadar, 1, 1 },
+  { string55, fn_cdar, 1, 1 },
+  { string56, fn_cddr, 1, 1 },
+  { string57, fn_caaar, 1, 1 },
+  { string58, fn_caadr, 1, 1 },
+  { string59, fn_cadar, 1, 1 },
+  { string60, fn_caddr, 1, 1 },
   { string61, fn_caddr, 1, 1 },
-  { string62, fn_caddr, 1, 1 },
-  { string63, fn_cdaar, 1, 1 },
-  { string64, fn_cdadr, 1, 1 },
-  { string65, fn_cddar, 1, 1 },
-  { string66, fn_cdddr, 1, 1 },
-  { string67, fn_length, 1, 1 },
-  { string68, fn_list, 0, 127 },
-  { string69, fn_reverse, 1, 1 },
-  { string70, fn_nth, 2, 2 },
-  { string71, fn_assoc, 2, 2 },
-  { string72, fn_member, 2, 2 },
-  { string73, fn_apply, 2, 127 },
-  { string74, fn_funcall, 1, 127 },
-  { string75, fn_append, 0, 127 },
-  { string76, fn_mapc, 2, 3 },
-  { string77, fn_mapcar, 2, 3 },
-  { string78, fn_add, 0, 127 },
-  { string79, fn_subtract, 1, 127 },
-  { string80, fn_multiply, 0, 127 },
-  { string81, fn_divide, 2, 127 },
-  { string82, fn_mod, 2, 2 },
-  { string83, fn_oneplus, 1, 1 },
-  { string84, fn_oneminus, 1, 1 },
-  { string85, fn_abs, 1, 1 },
-  { string86, fn_random, 1, 1 },
-  { string87, fn_max, 1, 127 },
-  { string88, fn_min, 1, 127 },
+  { string62, fn_cdaar, 1, 1 },
+  { string63, fn_cdadr, 1, 1 },
+  { string64, fn_cddar, 1, 1 },
+  { string65, fn_cdddr, 1, 1 },
+  { string66, fn_length, 1, 1 },
+  { string67, fn_list, 0, 127 },
+  { string68, fn_reverse, 1, 1 },
+  { string69, fn_nth, 2, 2 },
+  { string70, fn_assoc, 2, 2 },
+  { string71, fn_member, 2, 2 },
+  { string72, fn_apply, 2, 127 },
+  { string73, fn_funcall, 1, 127 },
+  { string74, fn_append, 0, 127 },
+  { string75, fn_mapc, 2, 3 },
+  { string76, fn_mapcar, 2, 3 },
+  { string77, fn_add, 0, 127 },
+  { string78, fn_subtract, 1, 127 },
+  { string79, fn_multiply, 0, 127 },
+  { string80, fn_divide, 1, 127 },
+  { string81, fn_mod, 2, 2 },
+  { string82, fn_oneplus, 1, 1 },
+  { string83, fn_oneminus, 1, 1 },
+  { string84, fn_abs, 1, 1 },
+  { string85, fn_random, 1, 1 },
+  { string86, fn_maxfn, 1, 127 },
+  { string87, fn_minfn, 1, 127 },
+  { string88, fn_noteq, 1, 127 },
   { string89, fn_numeq, 1, 127 },
   { string90, fn_less, 1, 127 },
   { string91, fn_lesseq, 1, 127 },
   { string92, fn_greater, 1, 127 },
   { string93, fn_greatereq, 1, 127 },
-  { string94, fn_noteq, 1, 127 },
-  { string95, fn_plusp, 1, 1 },
-  { string96, fn_minusp, 1, 1 },
-  { string97, fn_zerop, 1, 1 },
-  { string98, fn_oddp, 1, 1 },
-  { string99, fn_evenp, 1, 1 },
-  { string100, fn_char, 2, 2 },
-  { string101, fn_charcode, 1, 1 },
-  { string102, fn_codechar, 1, 1 },
-  { string103, fn_characterp, 1, 1 },
-  { string104, fn_stringp, 1, 1 },
-  { string105, fn_stringeq, 2, 2 },
-  { string106, fn_stringless, 2, 2 },
-  { string107, fn_stringgreater, 2, 2 },
-  { string108, fn_sort, 2, 2 },
-  { string109, fn_stringfn, 1, 1 },
-  { string110, fn_concatenate, 1, 127 },
-  { string111, fn_subseq, 2, 3 },
-  { string112, fn_readfromstring, 1, 1 },
-  { string113, fn_princtostring, 1, 1 },
-  { string114, fn_prin1tostring, 1, 1 },
-  { string115, fn_logand, 0, 127 },
-  { string116, fn_logior, 0, 127 },
-  { string117, fn_logxor, 0, 127 },
-  { string118, fn_lognot, 1, 1 },
-  { string119, fn_ash, 2, 2 },
-  { string120, fn_logbitp, 2, 2 },
-  { string121, fn_eval, 1, 1 },
-  { string122, fn_globals, 0, 0 },
-  { string123, fn_locals, 0, 0 },
-  { string124, fn_makunbound, 1, 1 },
-  { string125, fn_break, 0, 0 },
-  { string126, fn_read, 0, 1 },
-  { string127, fn_prin1, 1, 2 },
-  { string128, fn_print, 1, 2 },
-  { string129, fn_princ, 1, 2 },
-  { string130, fn_terpri, 0, 1 },
-  { string131, fn_readbyte, 0, 2 },
-  { string132, fn_readline, 0, 1 },
-  { string133, fn_writebyte, 1, 2 },
-  { string134, fn_writestring, 1, 2 },
-  { string135, fn_writeline, 1, 2 },
-  { string136, fn_restarti2c, 1, 2 },
-  { string137, fn_gc, 0, 0 },
-  { string138, fn_room, 0, 0 },
-  { string139, fn_saveimage, 0, 1 },
-  { string140, fn_loadimage, 0, 1 },
-  { string141, fn_cls, 0, 0 },
-  { string142, fn_pinmode, 2, 2 },
-  { string143, fn_digitalread, 1, 1 },
-  { string144, fn_digitalwrite, 2, 2 },
-  { string145, fn_analogread, 1, 1 },
-  { string146, fn_analogwrite, 2, 2 },
-  { string147, fn_delay, 1, 1 },
-  { string148, fn_millis, 0, 0 },
-  { string149, fn_sleep, 1, 1 },
-  { string150, fn_note, 0, 3 },
-  { string151, fn_edit, 1, 1 },
-  { string152, fn_pprint, 1, 2 },
+  { string94, fn_plusp, 1, 1 },
+  { string95, fn_minusp, 1, 1 },
+  { string96, fn_zerop, 1, 1 },
+  { string97, fn_oddp, 1, 1 },
+  { string98, fn_evenp, 1, 1 },
+  { string99, fn_integerp, 1, 1 },
+  { string100, fn_numberp, 1, 1 },
+  { string101, fn_char, 2, 2 },
+  { string102, fn_charcode, 1, 1 },
+  { string103, fn_codechar, 1, 1 },
+  { string104, fn_characterp, 1, 1 },
+  { string105, fn_stringp, 1, 1 },
+  { string106, fn_stringeq, 2, 2 },
+  { string107, fn_stringless, 2, 2 },
+  { string108, fn_stringgreater, 2, 2 },
+  { string109, fn_sort, 2, 2 },
+  { string110, fn_stringfn, 1, 1 },
+  { string111, fn_concatenate, 1, 127 },
+  { string112, fn_subseq, 2, 3 },
+  { string113, fn_readfromstring, 1, 1 },
+  { string114, fn_princtostring, 1, 1 },
+  { string115, fn_prin1tostring, 1, 1 },
+  { string116, fn_logand, 0, 127 },
+  { string117, fn_logior, 0, 127 },
+  { string118, fn_logxor, 0, 127 },
+  { string119, fn_lognot, 1, 1 },
+  { string120, fn_ash, 2, 2 },
+  { string121, fn_logbitp, 2, 2 },
+  { string122, fn_eval, 1, 1 },
+  { string123, fn_globals, 0, 0 },
+  { string124, fn_locals, 0, 0 },
+  { string125, fn_makunbound, 1, 1 },
+  { string126, fn_break, 0, 0 },
+  { string127, fn_read, 0, 1 },
+  { string128, fn_prin1, 1, 2 },
+  { string129, fn_print, 1, 2 },
+  { string130, fn_princ, 1, 2 },
+  { string131, fn_terpri, 0, 1 },
+  { string132, fn_readbyte, 0, 2 },
+  { string133, fn_readline, 0, 1 },
+  { string134, fn_writebyte, 1, 2 },
+  { string135, fn_writestring, 1, 2 },
+  { string136, fn_writeline, 1, 2 },
+  { string137, fn_restarti2c, 1, 2 },
+  { string138, fn_gc, 0, 0 },
+  { string139, fn_room, 0, 0 },
+  { string140, fn_saveimage, 0, 1 },
+  { string141, fn_loadimage, 0, 1 },
+  { string142, fn_cls, 0, 0 },
+  { string143, fn_pinmode, 2, 2 },
+  { string144, fn_digitalread, 1, 1 },
+  { string145, fn_digitalwrite, 2, 2 },
+  { string146, fn_analogread, 1, 1 },
+  { string147, fn_analogwrite, 2, 2 },
+  { string148, fn_delay, 1, 1 },
+  { string149, fn_millis, 0, 0 },
+  { string150, fn_sleep, 1, 1 },
+  { string151, fn_note, 0, 3 },
+  { string152, fn_edit, 1, 1 },
+  { string153, fn_pprint, 1, 2 },
+  { string154, fn_pprintall, 0, 0 },
 };

 // Table lookup functions
@@ -3034,7 +3059,7 @@

   if (form == NULL) return nil;

-  if (numberp(form) || characterp(form) || stringp(form)) return form;
+  if (integerp(form) || characterp(form) || stringp(form)) return form;

   if (symbolp(form)) {
     symbol_t name = form->name;
@@ -3085,7 +3110,7 @@
         object *pair = first(env);
         if (pair != NULL) {
           object *val = cdr(pair);
-          if (numberp(val)) val = number(val->integer);
+          if (integerp(val)) val = number(val->integer);
           push(cons(car(pair), val), envcopy);
         }
         env = cdr(env);
@@ -3165,6 +3190,10 @@

 // Print functions

+inline int maxbuffer (char *buffer) {
+  return SYMBOLTABLESIZE-(buffer-SymbolTable)-1;
+}
+
 void pserial (char c) {
   LastPrint = c;
   if (c == '\n') Serial.write('\r');
@@ -3255,7 +3284,7 @@
       printobject(form, pfun);
     }
     pfun(')');
-  } else if (numberp(form)) {
+  } else if (integerp(form)) {
     pint(integer(form), pfun);
   } else if (symbolp(form)) {
     if (form->name != NOTHING) pstring(name(form), pfun);
@@ -3278,6 +3307,27 @@

 // Read functions

+#if defined(lisplibrary)
+int glibrary () {
+  if (LastChar) {
+    char temp = LastChar;
+    LastChar = 0;
+    return temp;
+  }
+  char c = pgm_read_byte(&LispLibrary[GlobalStringIndex++]);
+  return (c != 0) ? c : -1; // -1?
+}
+
+void loadfromlibrary (object *env) {
+  GlobalStringIndex = 0;
+  object *line = read(glibrary);
+  while (line != NULL) {
+    eval(line, env);
+    line = read(glibrary);
+  }
+}
+#endif
+
 int gserial () {
   if (LastChar) {
     char temp = LastChar;
@@ -3299,7 +3349,6 @@
     ch = '(';
   }
   if (ch == '\n') ch = gfun();
-  if (ch == EOF) exit(0);
   if (ch == -1) return nil;
   if (ch == ')') return (object *)KET;
   if (ch == '(') return (object *)BRA;
@@ -3312,7 +3361,7 @@
   // Parse symbol, character, or number
   int index = 0, base = 10, sign = 1;
   char *buffer = SymbolTop;
-  int bufmax = SYMBOLTABLESIZE-(buffer-SymbolTable)-1; // Max index
+  int bufmax = maxbuffer(buffer); // Max index
   unsigned int result = 0;
   if (ch == '+') {
     buffer[index++] = ch;
@@ -3327,6 +3376,7 @@
     else if (ch == 'B') base = 2;
     else if (ch == 'O') base = 8;
     else if (ch == 'X') base = 16;
+    else if (ch == 0x07); // Ignore '
     else error(PSTR("Illegal character after #"));
     ch = gfun();
   }
@@ -3408,10 +3458,11 @@

 void setup () {
   Serial.begin(9600);
+  while (!Serial);
   initworkspace();
   initenv();
   initsleep();
-  pfstring(PSTR("uLisp 2.2 "), pserial); pln(pserial);
+  pfstring(PSTR("uLisp 2.3 "), pserial); pln(pserial);
 }

 // Read/Evaluate/Print loop
@@ -3457,5 +3508,8 @@
   #if defined(sdcardsupport)
   SDpfile.close(); SDgfile.close();
   #endif
+  #if defined(lisplibrary)
+  loadfromlibrary(NULL);
+  #endif
   repl(NULL);
 }


Optimized Particle publishing processing.

--- thermometer.ino
+++ thermometer.ino
@@ -41,9 +41,9 @@
 #define PHOTON_PUBLISH_DEBUG        // Uncomment to publish debug events to Particle
 #define PHOTON_PUBLISH_VALUE        // Uncomment to publish regular events to Particle

-#define THINGSPEAK_CLOUD            // Comment to totally ignore ThingSpeak Cloud
-
-#define BLYNK_CLOUD                 // Comment to totally ignore Blynk Cloud
+// #define THINGSPEAK_CLOUD            // Comment to totally ignore ThingSpeak Cloud
+
+// #define BLYNK_CLOUD                 // Comment to totally ignore Blynk Cloud
 #define BLYNK_NOTIFY_TEMP           // Uncomment to send Blynk push notifications
 #define BLYNK_SIGNAL_TEMP           // Uncomment to use Blynk LED signalling

@@ -65,12 +65,13 @@
 // STARTUP(WiFi.selectAntenna(ANT_EXTERNAL));
 STARTUP(WiFi.selectAntenna(ANT_INTERNAL));
 STARTUP(System.enableFeature(FEATURE_RETAINED_MEMORY));
-SYSTEM_THREAD(ENABLED);
-SYSTEM_MODE(AUTOMATIC);
+// SYSTEM_THREAD(ENABLED);
+// SYSTEM_MODE(AUTOMATIC);
+
 //-------------------------------------------------------------------------
 // Temperature sensing and publishing to Particle, ThinkSpeak, and Blynk
 //-------------------------------------------------------------------------
-#define SKETCH "THERMOMETER 1.0.0"
+#define SKETCH "THERMOMETER 2.0.0"
 #include "credentials.h"

 const unsigned int TIMEOUT_WATCHDOG = 10000;    // Watchdog timeout in milliseconds
@@ -90,6 +91,7 @@
 //-------------------------------------------------------------------------
 #ifdef PARTICLE_CLOUD
 const unsigned int PERIOD_PUBLISH_PARTICLE = 15000;
+const unsigned char PARTICLE_BATCH_LIMIT = 4;
 #endif

 //-------------------------------------------------------------------------
@@ -293,24 +295,115 @@
     if (millis() - tsPublish >= PERIOD_PUBLISH_PARTICLE)
     {
         tsPublish = millis();
-        // Publish boot events
-        if (stateAfterBoot)
-        {
-            bool sent;
-            sent = Particle.publish("Boot_Cnt_Run", String::format("%d/%d/%s", bootCount, bootRunPeriod, bootFwVersion));
-#ifdef PHOTON_PUBLISH_DEBUG
-            if (sent)
+        unsigned char batchMsgs = 0;
+        batchMsgs = publishParticleInits(batchMsgs);     // Publish boot events
+        batchMsgs = publishParticleValues(batchMsgs);    // Publish value events
+    }
+}
+
+unsigned char publishParticleInits(unsigned char sentBatchMsgs)
+{
+    static unsigned char events[5];
+    unsigned char batchMsgs = sentBatchMsgs;    // Messages sent in the current burst
+    if (stateAfterBoot)
+    {
+        boolean publishSuccess = false;
+        unsigned char sentMsgs = 0;     // Messages sent totally so far
+        for (unsigned char i = 0; i < sizeof(events)/sizeof(events[0]); i++)
+        {
+            if (batchMsgs >= PARTICLE_BATCH_LIMIT) break;
+            if (events[i])
             {
-                sent = Particle.publish("Sketch", String(SKETCH));
+                sentMsgs++;
+                continue;      // No processing with already sent messages
             }
-#endif
-            if (sent) stateAfterBoot = false;
-        }
-        // Publish values
-        Particle.publish("RSSI", String::format("%3d", rssiValue));
-        Particle.publish("Temperature/Trend", String::format("%4.1f/%4.3f", tempValue, tempTrend));
-    }
-}
+
+            switch(i)
+            {
+                case 0:
+                    publishSuccess = Particle.publish("Boot_Cnt_Run", String::format("%d/%d/%s", bootCount, bootRunPeriod, bootFwVersion));
+                    break;
+
+                case 1:
+                    publishSuccess = Particle.publish("Sketch", String(SKETCH));
+                    break;
+
+                case 2:
+                    publishSuccess = Particle.publish("Library", String(WATCHDOGS_VERSION));
+                    break;
+
+                case 3:
+                    publishSuccess = Particle.publish("Library", String(EXPONENTIALFILTER_VERSION));
+                    break;
+
+                case 4:
+                    publishSuccess = Particle.publish("Library", String(SMOOTHSENSORDATA_VERSION));
+                    break;
+            }
+            if (publishSuccess)
+            {
+                events[i] = true;
+                batchMsgs++;
+                sentMsgs++;
+                if (sentMsgs >= sizeof(events)/sizeof(events[0]))
+                {
+                    stateAfterBoot = false; // All initial messages have been sent
+                }
+            }
+            else
+            {
+                break;          // Break publishing due to cloud disconnection
+            }
+        }
+    }
+    return batchMsgs;
+}
+
+unsigned char publishParticleValues(unsigned char sentBatchMsgs)
+{
+    static unsigned char events[2];
+    unsigned char batchMsgs = sentBatchMsgs; // Messages sent in the current burst
+    unsigned char sentMsgs = 0;              // Messages sent totally so far
+    boolean publishSuccess = false;
+    for (unsigned char i = 0; i < sizeof(events)/sizeof(events[0]); i++)
+    {
+        if (batchMsgs >= PARTICLE_BATCH_LIMIT) break;
+        if (events[i])
+        {
+            sentMsgs++;
+            continue;      // No processing with already sent messages
+        }
+        switch(i)
+        {
+            case 0:
+                publishSuccess = Particle.publish("RSSI", String::format("%3d", rssiValue));
+                break;
+
+            case 1:
+                publishSuccess = Particle.publish("Temperature/Trend", String::format("%4.1f/%4.3f", tempValue, tempTrend));
+                break;
+        }
+        if (publishSuccess)
+        {
+            events[i] = true;
+            batchMsgs++;
+            sentMsgs++;
+        }
+        else
+        {
+            break;          // Break publishing due to cloud disconnection
+        }
+    }
+    if (sentMsgs >= sizeof(events)/sizeof(events[0]))
+    {
+        for (unsigned char i = 0; i < sizeof(events)/sizeof(events[0]); i++)
+        {
+            events[i] = false;
+        }
+    }
+    return batchMsgs;
+}
+
 #endif


rm robot.ino

rm robot.ino

--- robot.ino
+++ robot.ino
@@ -1,915 +0,0 @@
-/*
- * On the Arduino Mega we have 6 timers and 15 PWM outputs:
- * Pins 4 and 13: controlled by timer0 (used by clock)
- * Pins 11 and 12: controlled by timer1 (Servo library)
- * Pins 9 and10: controlled by timer2 (NewPing)
- * Pin 2, 3 and 5: controlled by timer 3 (free)
- * Pin 6, 7 and 8: controlled by timer 4 (free)
- * Pin 46, 45 and 44:: controlled by timer 5 (free)
- *
- * PWM pins 2-13, 44-46. Free 9: 2,3,5,6,7,8,44,45,46
- *
- * IMU MPU9150 uses library from https://github.com/richards-tech/RTIMULib-Arduino.git
- */
-
-#include <NewPing.h>
-#include <Servo.h>
-#include <RunningMedian.h>
-#include <Wire.h>
-#include "I2Cdev.h"
-#include "RTIMUSettings.h"
-#include "RTIMU.h"
-#include "CalLib.h"
-#include <EEPROM.h>
-#include <ros.h>
-#include "geometry_msgs/Twist.h"
-#include "robik/GenericStatus.h"
-#include "robik/GenericControl.h"
-#include "robik/ArmControl.h"
-#include "robik.h"
-#include "robot_config.h"
-#include "robik_api.h"
-
-
-ros::NodeHandle nh;
-
-void genericMessageListener(const robik::GenericControl& msg);
-void armMessageListener(const robik::ArmControl& msg);
-void velocityMessageListener(const geometry_msgs::Twist& msg);
-ros::Subscriber<robik::GenericControl> sub_generic_control("robik_generic_control", &genericMessageListener);
-ros::Subscriber<robik::ArmControl> sub_arm_control("robik_arm_control", &armMessageListener);
-ros::Subscriber<geometry_msgs::Twist> sub_velocity_control("robik_velocity_control", &velocityMessageListener);
-
-robik::GenericStatus status_msg;
-ros::Publisher pub_status("robik_status", &status_msg);
-
-unsigned long range_timer50;
-int motor_left_dir = 1; //direction for motor expected to go. 1 means forward
-int motor_right_dir = 1;
-double req_motor_left = 0; //store required wheel velocity
-double req_motor_right = 0;
-double old_motor_left = 0; //store to compare if speed changed since last cmd_vel
-double old_motor_right = 0;
-long odom_ticks_left_since_cmd = 0;
-long odom_ticks_right_since_cmd = 0;
-unsigned long millis_since_cmd = 0;
-
-//sonar.h
-unsigned long pingTimer;
-NewPing sonar(PIN_USOUND_TRIG, PIN_USOUND_ECHO, MAX_DISTANCE);
-void echoCheck();
-
-//IMU
-RTIMU *imu;                                           // the IMU object
-RTIMUSettings settings;                               // the settings object
-void read_IMU();
-void clean_IMU();
-
-//DISPLAY_INTERVAL sets the rate at which results are displayed
-#define DISPLAY_INTERVAL  300                         // interval between pose displays
-
-unsigned long lastDisplay;
-unsigned long lastRate;
-int sampleCount = 0;
-
-//determine the sign of a value
-template <typename type>
-type sign(type value) {
-	return type((value>0)-(value<0));
-}
-
-template <typename type>
-type signNZ(type value) {
-	return (value < 0) ? -1 : 1;
-}
-
-//arm.h
-void armPreInit();
-void armSetJointState(uint32_t clamp, uint32_t roll, uint32_t elbow, uint32_t shoulder, uint32_t yaw, uint32_t time_to_complete);
-unsigned long servoSenseTimer;
-FastRunningMedian<unsigned int,32, 0> servoSenseMedian_yaw;
-FastRunningMedian<unsigned int,32, 0> servoSenseMedian_shoulder;
-FastRunningMedian<unsigned int,32, 0> servoSenseMedian_elbow;
-FastRunningMedian<unsigned int,32, 0> servoSenseMedian_roll;
-FastRunningMedian<unsigned int,32, 0> servoSenseMedian_clamp;
-uint32_t shoulder2 = 512;
-estimated_pos_t estimated_clamp_pos;
-void refreshMotorJoints();
-
-//move.h
-void intr_left_encoder();
-void intr_right_encoder();
-void velocity_control_VelTh(float vel, float th);
-void velocity_control_LR();
-int motor_vel_to_pwm(double vel);
-
-//parking.h
-#define PARK_BACKSPEED -0.17 //-0.1 necouva
-#define PARK_TURNSPEED 0.9 //calibrated, do not put less, it won't work unless excitation energy will be provided
-#define PARK_BACKTURNSPEED_SLOW_RATIO 0.2
-#define PARKPHASE_NOPARKING 0
-#define PARKPHASE_NAVIGATING 1
-#define PARKPHASE_ROTATING 2
-#define PARKPHASE_BACKWARD 3
-#define PARKPHASE_PARKED 4
-	//0: normal operation;
-	//1: navigating to base;
-	//2: rotating to find laser;
-	//3: going backwards;
-	//4: final stage going backwards
-int state_parking = 0;
-int park_last_spotted = 0; //0: no value; 1: inner; -1: outer
-
-void park_rotating(bool inner, bool outer);
-void park_backward(bool inner, bool outer);
-void park_parked(bool inner, bool outer);
-void handle_parking();
-
-/*------------------Utils----------------------------*/
-#define LOG_STR_LEN 150
-char charBuf[LOG_STR_LEN + 1];
-char strtmp[LOG_STR_LEN + 1];
-
-void init_variables() {
-
-	estimated_clamp_pos.orig_time.sec = 0;
-	estimated_clamp_pos.orig_time.nsec = 0;
-	estimated_clamp_pos.time_to_complete = 0;
-	estimated_clamp_pos.orig_pos = 0;
-	estimated_clamp_pos.target_pos = 0;
-	estimated_clamp_pos.corrected = false;
-
-	status_msg.status_code = (int32_t *) malloc(20 * sizeof(int32_t));
-}
-
-void log_string(String str) {
-
-	unsigned int str_length = str.length();
-
-	if (str_length > 0) {
-
-		if (str_length > LOG_STR_LEN)
-			str_length = LOG_STR_LEN;
-
-		str.toCharArray(charBuf, str_length + 1);
-		status_msg.log_message = &charBuf[0];
-		status_msg.log_count++;
-
-	}
-
-}
-
-void log_chars(char *str) {
-
-	if (strlen(str) > 0) {
-		strcpy(charBuf, str);
-		status_msg.log_message = &charBuf[0];
-		status_msg.log_count++;
-
-	}
-
-}
-
-void status_msg_clean () {
-	charBuf[0] = '\0';
-	status_msg.log_count = 0;
-	status_msg.status_code_length = 0;
-	status_msg.status_param_length = 0;
-	clean_IMU();
-}
-
-void add_status_code (int32_t code) {
-	if (status_msg.status_code_length < 20) {
-		status_msg.status_code[status_msg.status_code_length] = code;
-		status_msg.status_code_length++;
-	}
-}
-
-void jetsonPower() {
-
-	if (digitalRead(PIN_JETSONPOWER) == LOW) {
-		digitalWrite(PIN_PETRPOWER, LOW); // Jetson was powered down (was up before). Poweroff whole robot
-	}
-}
-
-
-/*------------------State----------------------------*/
-#include "robik_state.h"
-
-/*--------------------SETUP()------------------------*/
-void setup() {
-
-	//Power
-	pinMode(PIN_PETRPOWER, OUTPUT);   // this pin needs to be enabled to keep power on
-	digitalWrite(PIN_PETRPOWER, HIGH); // turn on power
-	pinMode(PIN_JETSONPOWER, INPUT);  // read power-on status of Jetson
-	pinMode(PIN_CHARGER_BUTTON, OUTPUT);  // read power-on status of Jetson
-
-	init_variables();
-
-	//Arm
-	Serial3.begin(9600);
-	delay(100);
-	servoSenseTimer = millis();
-	pinMode(PIN_ARM_FORE_CLAMP, INPUT);
-	pinMode(PIN_ARM_BACK_CLAMP, INPUT);
-	digitalWrite(PIN_ARM_FORE_CLAMP, HIGH); // turn on pullup resistors
-	digitalWrite(PIN_ARM_BACK_CLAMP, HIGH); // turn on pullup resistors
-
-	//LED
-	pinMode(PIN_LED, OUTPUT);
-	digitalWrite(PIN_LED, LOW);
-
-	//power arm
-	pinMode(PIN_RELAY, OUTPUT);
-	digitalWrite(PIN_RELAY, HIGH);
-	armPreInit();
-	digitalWrite(PIN_RELAY, LOW);  //turn off arm (as standby)
-
-	//IMU init
-	int errcode;
-	Wire.begin();
-	imu = RTIMU::createIMU(&settings);  // create the imu object
-
-	if ((errcode = imu->IMUInit()) < 0) {
-		//#Serial.print("Failed to init IMU: "); Serial.println(errcode);
-	}
-
-	imu->getCalibrationValid();
-
-	//ROS node
-	nh.getHardware()->setBaud(115200); //57600 115200
-	nh.initNode();
-	nh.subscribe(sub_generic_control);
-	nh.subscribe(sub_arm_control);
-	nh.subscribe(sub_velocity_control);
-	nh.advertise(pub_status);
-
-	while(!nh.connected()) {
-		jetsonPower();
-		nh.spinOnce();
-	}
-	nh.loginfo("Arduino connected");
-
-	range_timer50 = 11;
-
-	//sonar
-	pingTimer = millis();
-
-	//odometry encoders
-	pinMode(PIN_ODOM_LEFT, INPUT);
-	digitalWrite(PIN_ODOM_LEFT, HIGH);       // turn on pullup resistor
-	pinMode(PIN_ODOM_RIGHT, INPUT);
-	digitalWrite(PIN_ODOM_RIGHT, HIGH);       // turn on pullup resistor
-	attachInterrupt(INT_ODOM_LEFT, intr_left_encoder, CHANGE);
-	attachInterrupt(INT_ODOM_RIGHT, intr_right_encoder, CHANGE);
-
-	//bumber
-	pinMode(PIN_BUMPER, INPUT);
-	digitalWrite(PIN_BUMPER, HIGH); // turn on pullup resistors
-
-	//motion detector
-	pinMode(PIN_MOTION_DETECTOR, INPUT);
-
-	//velocity
-	pinMode(PIN_MOTOR_LEFT_ENA, OUTPUT); //Motor left
-	pinMode(PIN_MOTOR_LEFT_0, OUTPUT);
-	pinMode(PIN_MOTOR_LEFT_1, OUTPUT);
-	pinMode(PIN_MOTOR_RIGHT_ENA, OUTPUT); //Motor right
-	pinMode(PIN_MOTOR_RIGHT_0, OUTPUT);
-	pinMode(PIN_MOTOR_RIGHT_1, OUTPUT);
-
-	//menu control
-	pinMode(PIN_MENU_MENU, INPUT);
-	digitalWrite(PIN_MENU_MENU, HIGH); // turn on pullup resistors
-	pinMode(PIN_MENU_OK, INPUT);
-	digitalWrite(PIN_MENU_OK, HIGH); // turn on pullup resistors
-	pinMode(PIN_MENU_CANCEL, INPUT);
-	digitalWrite(PIN_MENU_CANCEL, HIGH); // turn on pullup resistors
-	pinMode(PIN_MENU_UP, INPUT);
-	digitalWrite(PIN_MENU_UP, HIGH); // turn on pullup resistors
-	pinMode(PIN_MENU_DOWN, INPUT);
-	digitalWrite(PIN_MENU_DOWN, HIGH); // turn on pullup resistors
-
-}
-
-//Return time difference in milliseconds
-uint32_t ros_time_diff(ros::Time before, ros::Time after) {
-	uint32_t sec = after.sec - before.sec;
-	uint32_t nsec = after.nsec - before.nsec;
-
-	if (nsec < 0) {
-		nsec = 1000 + nsec;
-	}
-
-	return sec * 1000 + nsec / 1000000;
-}
-
-//-1: t2<t1
-// 0: t2==t1
-// 1: t2>t1
-int ros_time_cmp(ros::Time t1, ros::Time t2) {
-	if (t1.sec == t2.sec && t1.nsec == t2.nsec)
-		return 0;
-
-	if (ros_time_diff(t1, t2) > 0)
-		return 1;
-	else
-		return -1;
-}
-
-void checkClampAndStop() {
-//time_to_complete [ms]
-	ros::Time now = nh.now();
-	uint32_t milliseconds_since_start = ros_time_diff(estimated_clamp_pos.orig_time, now);
-
-	if ( (estimated_clamp_pos.orig_pos < estimated_clamp_pos.target_pos) &&
-	     (milliseconds_since_start < (estimated_clamp_pos.time_to_complete + 700)) ) { //clamp is being moved, try hard for extra 700
-		double progress = milliseconds_since_start / estimated_clamp_pos.time_to_complete;
-		progress *= 0.9; //bring it back a bit to compensate late detection
-		if (progress > 1)
-			progress = 1;
-
-		uint32_t estimatedClampPos = estimated_clamp_pos.orig_pos + (uint32_t)(progress * (estimated_clamp_pos.target_pos - estimated_clamp_pos.orig_pos));
-		sprintf(strtmp, "SSS %lu | TTC %lu | ORG %lu | TRG %lu | PRG %lu | ECP %lu", milliseconds_since_start, estimated_clamp_pos.time_to_complete, estimated_clamp_pos.orig_pos, estimated_clamp_pos.target_pos, (uint32_t)(progress*100), estimatedClampPos); log_chars(strtmp);
-		armSetJointState(estimatedClampPos, 0, 0, 0, 0, 80); //80ms
-		estimated_clamp_pos.corrected = true; //prevent iteration for stopping clamp again
-	}
-	else
-		sprintf(strtmp, "SSS %lu | TTC %lu | ORG %lu | TRG %lu ", milliseconds_since_start, estimated_clamp_pos.time_to_complete, estimated_clamp_pos.orig_pos, estimated_clamp_pos.target_pos); log_chars(strtmp);
-}
-
-unsigned int menu_lag = 0;
-uint8_t menu_controls(){
-  uint8_t res = 0;
-
-  if (menu_lag > 3) {
-		menu_lag = 0;
-		if (digitalRead(PIN_MENU_MENU) == LOW)
-			res |= MASK_MENU_MENU;
-		if (digitalRead(PIN_MENU_OK) == LOW)
-			res |= MASK_MENU_OK;
-		if (digitalRead(PIN_MENU_CANCEL) == LOW)
-			res |= MASK_MENU_CANCEL;
-		if (digitalRead(PIN_MENU_UP) == LOW)
-			res |= MASK_MENU_UP;
-		if (digitalRead(PIN_MENU_DOWN) == LOW)
-			res |= MASK_MENU_DOWN;
-  }
-  else {
-		menu_lag++;
-  }
-  return res;
-}
-
-
-/*---------------------- LOOP() ---------------------*/
-void loop() {
-
-	//sent status message
-	if ( (millis()-range_timer50) > 50){   //[ms] 50ms == 20Hz
-
-		status_msg.header.stamp = nh.now();
-
-		//bumper
-		status_msg.bumper_front = bumperFront;
-		bumperFrontPublished = true;
-
-		//menu controls
-		status_msg.menu_controls = menu_controls();
-
-		//odom
-		status_msg.odom_ticksLeft = motor_left_dir * odom_ticks_left;
-		status_msg.odom_ticksRight = motor_right_dir * odom_ticks_right;
-		odom_ticks_left = 0;
-		odom_ticks_right = 0;
-		unsigned long now = millis();
-		status_msg.odom_millisSinceLastUpdate = now - odomMillisSinceLastUpdate;
-		odomMillisSinceLastUpdate = now;
-
-		//wheels are expected to move but robot is stopped
-		velocity_control_LR();
-//add_status_code(req_motor_left);
-//add_status_code(old_motor_left);
-//add_status_code(odom_ticks_left_since_cmd);
-//add_status_code(millis_since_cmd);
-
-
-		//sonar
-		status_msg.ultrasound_Back = ultrasoundBack;
-
-		//arm
-		status_msg.arm_yaw = servoSenseMedian_yaw.getMedian();
-		status_msg.arm_shoulder = servoSenseMedian_shoulder.getMedian();
-		status_msg.arm_elbow = servoSenseMedian_elbow.getMedian();
-		status_msg.arm_roll = servoSenseMedian_roll.getMedian();
-		status_msg.arm_clamp = servoSenseMedian_clamp.getMedian();
-
-		status_msg.arm_fore_clamp = digitalRead(PIN_ARM_FORE_CLAMP) == LOW ? true : false;
-		status_msg.arm_back_clamp = digitalRead(PIN_ARM_BACK_CLAMP) == LOW ? true : false;
-		if (estimated_clamp_pos.corrected == false && (status_msg.arm_fore_clamp == true || status_msg.arm_back_clamp == true))
-			checkClampAndStop();
-
-		//motion detector
-		status_msg.motion_detector = motionDetector;
-		motionDetectorPublished = true;
-
-
-		//parking photo sensor
-		status_msg.parkSens_inner = analogRead(PIN_PARK_SENS_INNER);
-		status_msg.parkSens_outer = analogRead(PIN_PARK_SENS_OUTER);
-
-
-		//IMU
-		status_msg.imu_sample_count = sampleCount;
-		status_msg.imu_gyro_bias_valid = imu->IMUGyroBiasValid();  //do not move IMU if false
-
-		refreshMotorJoints();
-
-		//publish
-		range_timer50 = millis();
-		pub_status.publish(&status_msg);
-		status_msg_clean();
-
-	}
-
-	//thigs to be done frequently
-	read_IMU();
-	handle_parking();
-	jetsonPower();
-
-	nh.spinOnce();
-
-	//sonar
-	if (millis() >= pingTimer) {         // Is it this sensor's time to ping?
-		pingTimer += PING_INTERVAL;
-		sonar.ping_timer(echoCheck); // Do the ping (processing continues, interrupt will call echoCheck to look for echo).
-	}
-
-	//bumper
-	if (digitalRead(PIN_BUMPER) == LOW) {
-		bumperFrontPublished = false;
-		bumperFront = true;
-	}
-	else {
-		if (bumperFrontPublished == true) bumperFront = false;
-	}
-
-	//motion detector
-	if (digitalRead(PIN_MOTION_DETECTOR) == HIGH) {
-		motionDetectorPublished = false;
-		motionDetector = true;
-	}
-	else {
-		if (motionDetectorPublished == true) motionDetector = false;
-	}
-
-	//servo sense
-	if (millis() >= servoSenseTimer) {         // Is it this sensor's time to ping?
-		servoSenseTimer += SERVO_SENSE_INTERVAL;
-                servoSenseMedian_yaw.addValue(analogRead(PIN_SERVO_SENSE_YAW));
-                servoSenseMedian_shoulder.addValue(analogRead(PIN_SERVO_SENSE_SHOULDER));
-                servoSenseMedian_elbow.addValue(analogRead(PIN_SERVO_SENSE_ELBOW));
-                servoSenseMedian_roll.addValue(analogRead(PIN_SERVO_SENSE_ROLL));
-                servoSenseMedian_clamp.addValue(analogRead(PIN_SERVO_SENSE_CLAMP));
-	}
-
-} //end of loop
-
-
-void clean_IMU() {
-	for (int i = 0; i < 3; i++) {
-		status_msg.imu_angular_velocity_v3_x[i] = 0;
-		status_msg.imu_angular_velocity_v3_y[i] = 0;
-		status_msg.imu_angular_velocity_v3_z[i] = 0;
-
-		status_msg.imu_linear_acceleration_v3_x[i] = 0;
-		status_msg.imu_linear_acceleration_v3_y[i] = 0;
-		status_msg.imu_linear_acceleration_v3_z[i] = 0;
-
-		status_msg.imu_compass_v3_x[i] = 0;
-		status_msg.imu_compass_v3_y[i] = 0;
-		status_msg.imu_compass_v3_z[i] = 0;
-	}
-	sampleCount = 0;
-}
-
-void read_IMU() {
-
-	if (sampleCount < 3 && imu->IMURead()) { // get the latest data if ready yet
-		RTVector3 gyro_v3 = (RTVector3&)imu->getGyro();
-		status_msg.imu_angular_velocity_v3_x[sampleCount] = gyro_v3.x();
-		status_msg.imu_angular_velocity_v3_y[sampleCount] = gyro_v3.y();
-		status_msg.imu_angular_velocity_v3_z[sampleCount] = gyro_v3.z();
-
-		RTVector3 accel_v3 = (RTVector3&)imu->getAccel();
-		status_msg.imu_linear_acceleration_v3_x[sampleCount] = accel_v3.x();
-		status_msg.imu_linear_acceleration_v3_y[sampleCount] = accel_v3.y();
-		status_msg.imu_linear_acceleration_v3_z[sampleCount] = accel_v3.z();
-
-		RTVector3 compass_v3 = (RTVector3&)imu->getCompass();
-		status_msg.imu_compass_v3_x[sampleCount] = compass_v3.x();
-		status_msg.imu_compass_v3_y[sampleCount] = compass_v3.y();
-		status_msg.imu_compass_v3_z[sampleCount] = compass_v3.z();
-
-		sampleCount++;
-	}
-
-}
-
-void park_rotating(bool inner, bool outer) {
-
-	if (inner || outer) {
-		//stop rotation
-		velocity_control_VelTh(PARK_BACKSPEED, 0);
-		state_parking = PARKPHASE_BACKWARD;
-		park_last_spotted = 1; //if beam is lost, turn reverse first to find it
-		// ROS_INFO("Laser beam found. Switching to PARKPHASE_BACKWARD");
-	}
-
-}
-
-void park_backward(bool inner, bool outer) {
-
-	//adjust speed
-	float parking_speed = PARK_BACKSPEED; //default ful speed
-	if ( ultrasoundBack < 10 ) { // getting as close as 10cm, slow down
-		// ROS_DEBUG("Jsem blizko 20cm. Brzdim na pulku");
-		parking_speed = PARK_BACKSPEED / 2;
-	}
-	if ( ultrasoundBack < 2 ) {  // <2cm stop
-		parking_speed = 0;
-		state_parking = PARKPHASE_PARKED;
-	}
-
-	float dir = 0;  // >0 toci se doleva
-	if (inner && outer)
-		dir = 0;
-	else if (inner == true && outer == false) {
-		dir = +1 * PARK_BACKTURNSPEED_SLOW_RATIO;
-		park_last_spotted = 1;
-	}
-	else if (inner == false && outer == true) {
-		dir = -1 * PARK_BACKTURNSPEED_SLOW_RATIO;
-		park_last_spotted = -1;
-	}
-	else if (inner == false && outer == false && park_last_spotted == 1) {
-		dir = +1 * 1.3 * PARK_BACKTURNSPEED_SLOW_RATIO;
-	}
-	else if (inner == false && outer == false && park_last_spotted == -1) {
-		dir = -1 * 1.3 * PARK_BACKTURNSPEED_SLOW_RATIO;
-	}
-	else if (inner == false && outer == false && park_last_spotted == 0) {
-		dir = 1;
-		parking_speed = 0;
-		state_parking = 0;
-		// ROS_ERROR("Parking laser beam lost. Switching to manual parking.");
-	}
-
-	velocity_control_VelTh(parking_speed, dir * PARK_TURNSPEED);
-	// ROS_INFO("B I:%d O:%d US: %f lst: %d | spd: %f tw %f",	inner, outer, laserscan_msg.ranges[0], park_last_spotted, parking_speed, dir * PARK_TURNSPEED);
-
-}
-
-void park_parked(bool inner, bool outer) {
-	velocity_control_VelTh(0, 0);
-	// ROS_INFO("Parked");
-	state_parking = PARKPHASE_NOPARKING;
-}
-
-void handle_parking() {
-
-	if (state_parking > PARKPHASE_NOPARKING) {
-
-		int ps_inner = analogRead(PIN_PARK_SENS_INNER);
-		int ps_outer = analogRead(PIN_PARK_SENS_OUTER);
-		bool inner = false;
-		bool outer = false;
-		if (ps_inner < 320)
-			inner = true;
-		if (ps_outer < 280)
-			outer = true;
-
-		//ROS_INFO("PS status: { inner: %d, outer: %d }", inner, outer);
-
-		switch (state_parking) {
-			case PARKPHASE_ROTATING:
-				park_rotating(inner, outer);
-				break;
-			case PARKPHASE_BACKWARD:
-				park_backward(inner, outer);
-				break;
-			case PARKPHASE_PARKED:
-				park_parked(inner, outer);
-				break;
-		}
-	}
-
-}
-
-/*------------------------ Listeners -------------------*/
-
-void echoCheck() { // If ping received, set the sensor distance to array.
-
-	 if (sonar.check_timer())
-		 ultrasoundBack = sonar.ping_result / US_ROUNDTRIP_CM;
-}
-
-void genop_head_pose(const robik::GenericControl& msg) {
-
-	//head
-		//int val = map(msg.gen_param2, -1000, 1000, 50, 165);
-		//servoHeadPitch.write(val);
-
-}
-
-void setParkingPhase(const robik::GenericControl& msg) {
-	state_parking = msg.gen_param1;
-	if (msg.gen_param1 == PARKPHASE_ROTATING) {
-		velocity_control_VelTh(0, 1 * PARK_TURNSPEED);
-	}
-}
-
-void setLED(const robik::GenericControl& msg) {
-	digitalWrite(PIN_LED, (msg.gen_param1 == 1) ? HIGH : LOW);
-}
-
-void setDPin(const robik::GenericControl& msg) {
-	digitalWrite(msg.gen_param1, (msg.gen_param2 == 1) ? HIGH : LOW);
-}
-
-void setArmPower(const robik::GenericControl& msg) {
-	digitalWrite(PIN_RELAY, (msg.gen_param1 == 1) ? HIGH : LOW);
-}
-
-void genericMessageListener(const robik::GenericControl& msg) {
-
-	if (msg.gen_operation == OPERATION_HEAD_POSE) {
-		genop_head_pose(msg);
-	}
-	if (msg.gen_operation == OPERATION_SET_PARKING_PHASE) {
-		setParkingPhase(msg);
-	}
-	if (msg.gen_operation == OPERATION_SET_LED) {
-		setLED(msg);
-	}
-	if (msg.gen_operation == OPERATION_SET_DPIN) {
-		setDPin(msg);
-	}
-	if (msg.gen_operation == OPERATION_SET_ARMPOWER) {
-		setArmPower(msg);
-	}
-
-}
-
-void armPreInit() {
-	Serial3.print("#10P1320T500\r\n"); delay(500);
-	Serial3.print("#3P2250T500\r\n"); delay(500);
-	Serial3.print("#4P1740T500\r\n"); delay(500);
-	Serial3.print("#5P1091T500\r\n"); delay(500);
-	Serial3.print("#1P1908T500\r\n"); delay(500);
-	Serial3.print("#10P1280T500\r\n"); delay(500);
-
-	Serial3.print("#11P700T1\r\n");  //set initial speed for shoulder 2
-	shoulder2 = 512; //set initial position for shoulder2
-}
-
-
-void armMessageListener(const robik::ArmControl& msg) {
-
-	armSetJointState(msg.arm_clamp, msg.arm_roll, msg.arm_elbow, msg.arm_shoulder, msg.arm_yaw, msg.time_to_complete);
-
-}
-
-//replace by constrain arduino funtion after logging will no longer be needed
-uint32_t check_limits(uint32_t val, uint32_t min, uint32_t max) {
-
-    if (min > max) {
-	uint32_t t = min;
-	min = max;
-	max = t;
-    }
-
-    if (val < min) {
-	val = min;
-	log_string("ArmSrvoMinX:" + String(min, DEC));  //nevim jestli funguje
-    }
-    if (val > max) {
-	val = max;
-	log_string("ArmSrvoMaxX:" + String(max, DEC));
-    }
-
-    return val;
-}
-/*
-Postup ladeni
-- zapoj potenciometr serva do analogu a zapis min a max hodnotu
-- odladit smer otaceni, pak odstranit 100ms stop  //konfiguruj PWM stejne jako ohm
-- vyzkouset jestli shoulder2 se hybe stejne jako shoulder1
-*/
-
-//TODO pridej parametr effort
-void setMotorJoint(int pin0, int pin1, int pinsense, int target_ohm) {
-
-	int current_ohm = analogRead(pinsense);
-	add_status_code(current_ohm);
-
-	int a0 = LOW;  //stop implicitly
-	int a1 = LOW;
-	if (current_ohm > target_ohm) {
-		a0 = HIGH;
-	}
-	else if (current_ohm < target_ohm) {
-		a1 = HIGH;
-	}
-	/*digitalWrite(pin0, a0);*/
-	/*digitalWrite(pin1, a1);*/
-
-	//delay(100);   //zastav motor pro 100ms dokud neodladis smer
-	/*digitalWrite(pin0, LOW);*/
-	/*digitalWrite(pin1, LOW);*/
-}
-
-void refreshMotorJoints() {
-//TODO rozeber servo a predelej to ne lm298n
-  //setMotorJoint(PIN_MOTOR_SHOULDER_0, PIN_MOTOR_SHOULDER_1, PIN_SERVO_SENSE_SHOULDER2, shoulder2);
-  //add more joints here
-}
-
-
-void armSetJointState(uint32_t clamp, uint32_t roll, uint32_t elbow, uint32_t shoulder, uint32_t yaw, uint32_t time_to_complete) {
-	String arm_clamp = "";
-	String arm_roll = "";
-	String arm_elbow = "";
-	String arm_shoulder = "";
-	String arm_yaw = "";
-	String time_to_complete_str = String(time_to_complete, DEC);
-	if (clamp != 0) {
-		arm_clamp = "#5P" + String(check_limits(clamp, ARM_MIN_CLAMP, ARM_MAX_CLAMP), DEC);
-		estimated_clamp_pos.orig_time = nh.now();
-		estimated_clamp_pos.time_to_complete = time_to_complete;
-		estimated_clamp_pos.orig_pos = estimated_clamp_pos.target_pos;
-		estimated_clamp_pos.target_pos = check_limits(clamp, ARM_MIN_CLAMP, ARM_MAX_CLAMP);
-		estimated_clamp_pos.corrected = false;
-	}
-
-	if (roll != 0) {
-		arm_roll = "#4P" + String(check_limits(roll, ARM_MIN_ROLL, ARM_MAX_ROLL), DEC);
-	}
-
-	if (elbow != 0) {
-		arm_elbow = "#3P" + String(check_limits(elbow, ARM_MIN_ELBOW, ARM_MAX_ELBOW), DEC);
-	}
-
-	if (shoulder != 0) {
-		arm_shoulder = "#10P" + String(check_limits(shoulder, ARM_MIN_SHOULDER, ARM_MAX_SHOULDER), DEC);
-		shoulder2 = 512; //TODO map from shoulder
-// otestuje servo   setMotorJoint(PIN_MOTOR_SHOULDER_0, PIN_MOTOR_SHOULDER_1, PIN_SERVO_SENSE_SHOULDER2, constrain(shoulder2, ARM_RES_MIN_SHOULDER2, ARM_RES_MAX_SHOULDER2));
-	}
-
-	if (yaw != 0) {
-		arm_yaw = "#1P" + String(check_limits(yaw, ARM_MIN_YAW, ARM_MAX_YAW), DEC);
-	}
-
-	String cmd = String(arm_clamp + arm_roll + arm_elbow + arm_shoulder + arm_yaw + "T" + time_to_complete_str + "\r\n");
-	log_string(cmd);
-	Serial3.print(cmd);
-
-}
-
-void velocityMessageListener(const geometry_msgs::Twist& msg) {
-	velocity_control_VelTh(msg.linear.x, msg.angular.z);
-}
-
-// vel: linear velocity [m/sec]
-// theta: angular velocity [rad/sec]
-void velocity_control_VelTh(float vel, float th) {
-
-	//limit to standard max values on input
-	vel = constrain(vel, -MAX_VEL, MAX_VEL);
-	th = constrain(th, -MAX_TH, MAX_TH);
-
-	//combine speed and rotation and set as required speed for boot wheels
-	req_motor_left  = vel - ((AXIAL_LENGTH_MM/1000 / 2) * th);
-	req_motor_right = vel + ((AXIAL_LENGTH_MM/1000 / 2) * th);
-
-	//limit combined speed to maximum available
-	double coef = 1;
-	if (abs(req_motor_left) > MAX_VEL)
-		coef = req_motor_left / MAX_VEL;
-	if (abs(req_motor_right) > MAX_VEL)
-		coef = req_motor_right / MAX_VEL;
-
-	req_motor_left = req_motor_left / abs(coef);
-	req_motor_right = req_motor_right / abs(coef);
-
-	velocity_control_LR();
-
-}
-
-
-int motor_vel_to_pwm(double vel) {
-	return vel / MAX_VEL * MOTOR_MAX;
-}
-
-int is_equal_3decplaces(double a, double b) {
-		long long ai = a * 1000;
-		long long bi = b * 1000;
-		return ai == bi;
-}
-
-//input is global variable req_motor_left and req_motor_right, [meter/sec], max speed is normalized
-void velocity_control_LR() {
-
-	//if velocity command has changed
-	if ( (!is_equal_3decplaces(req_motor_left, old_motor_left)) ||
-	     (!is_equal_3decplaces(req_motor_right, old_motor_right))
-	    ||
-	     (req_motor_left == 0 && req_motor_right == 0)
-	) {
-		//remove correction, use only estimated speeds for new velocity command
-		old_motor_left = req_motor_left;
-		old_motor_right = req_motor_right;
-		millis_since_cmd = millis();
-		odom_ticks_left_since_cmd = 0;
-		odom_ticks_right_since_cmd = 0;
-	}
-
-	//number of ticks that the robot is behind. Can be also < 0 which means the robot is ahead of required
-	long millis_now = millis();
-	long correction_motor_left  = long( (double)(millis_now - millis_since_cmd)/1000.0 * req_motor_left  * 1000 / TICK_LENGTH_MM ) - odom_ticks_left_since_cmd;
-	long correction_motor_right = long( (double)(millis_now - millis_since_cmd)/1000.0 * req_motor_right * 1000 / TICK_LENGTH_MM ) - odom_ticks_right_since_cmd;
-
-	//include correction <0 - ow; MOTOR_MAX + ow> where ow is an overflow
-	int corrected_motor_left  = motor_vel_to_pwm(abs(req_motor_left))  + constrain(correction_motor_left  * 5, -MOTOR_MAX/5, MOTOR_MAX/5);
-	int corrected_motor_right = motor_vel_to_pwm(abs(req_motor_right)) + constrain(correction_motor_right * 5, -MOTOR_MAX/5, MOTOR_MAX/5);
-	add_status_code(corrected_motor_left);
-	add_status_code(corrected_motor_right);
-
-	//remap if speed of any wheel overflows 0 or MOTOR_MAX
-	if (corrected_motor_left < MOTOR_MIN)
-		corrected_motor_left = 0;
-	if (corrected_motor_right < MOTOR_MIN)
-		corrected_motor_right = 0;
-
-	int ow_left = 0;
-	int ow_right = 0;
-	int ow = 0;
-	if (corrected_motor_left > MOTOR_MAX)
-		ow_left = corrected_motor_left - MOTOR_MAX;
-	if (corrected_motor_right > MOTOR_MAX)
-		ow_right = corrected_motor_right - MOTOR_MAX;
-
-	//subtract common overflow for both wheels
-	ow = max(ow_left, ow_right);
-	corrected_motor_left -= ow;
-	corrected_motor_right -= ow;
-
-	//derive direction from original speed
-	//left
-	int a0 = LOW;  //stop implicitly
-	int a1 = LOW;
-	if (req_motor_left < 0) {
-		a0 = HIGH;
-		motor_left_dir = -1;
-	}
-	if (req_motor_left > 0) {
-		a1 = HIGH;
-		motor_left_dir = 1;
-	}
-	digitalWrite(PIN_MOTOR_LEFT_0, a0);
-	digitalWrite(PIN_MOTOR_LEFT_1, a1);
-	analogWrite(PIN_MOTOR_LEFT_ENA, constrain(corrected_motor_left, 0, MOTOR_MAX));
-	add_status_code(correction_motor_left);
-	add_status_code(corrected_motor_left);
-
-	//right
-	a0 = LOW;
-	a1 = LOW;
-	if (req_motor_right < 0) {
-		a0 = HIGH;
-		motor_right_dir = -1;
-	}
-	if (req_motor_right > 0) {
-		a1 = HIGH;
-		motor_right_dir = 1;
-	}
-	digitalWrite(PIN_MOTOR_RIGHT_0, a0);
-	digitalWrite(PIN_MOTOR_RIGHT_1, a1);
-	analogWrite(PIN_MOTOR_RIGHT_ENA, constrain(corrected_motor_right, 0, MOTOR_MAX));
-	add_status_code(correction_motor_right);
-	add_status_code(corrected_motor_right);
-}
-
-//odometry encoders
-void intr_left_encoder() {
-
-	odom_ticks_left++;
-	odom_ticks_left_since_cmd++;
-}
-
-void intr_right_encoder() {
-
-	odom_ticks_right++;
-	odom_ticks_right_since_cmd++;
-}


1.2.1

1.2.1

Bug correction for RGB blade

--- LightSaberOS.ino
+++ LightSaberOS.ino
@@ -1094,90 +1094,6 @@
 			break;
 #endif //LEDSTRINGS

-/*LUXEON*/
-#if defined LUXEON
-			case 2: // BLADE MAIN COLOR
-			confMenuStart(storage.mainColor, 9, dfplayer);
-			confParseValue(storage.mainColor, 0, COLORS - 1, 1, dfplayer);
-			if (modification) {
-				modification = 0;
-				storage.mainColor = value;
-				getColor(currentColor, storage.mainColor);
-				lightOn(ledPins, currentColor);
-#ifdef LS_INFO
-				Serial.print(storage.mainColor);
-				Serial.print("\tR:");
-				Serial.print(currentColor[0]);
-				Serial.print("\tG:");
-				Serial.print(currentColor[1]);
-				Serial.print(" \tB:");
-				Serial.println(currentColor[2]);
-#endif //LS_INFO
-			}
-			break;
-			case 3: //BLADE CLASH COLOR
-			confMenuStart(storage.clashColor, 10, dfplayer);
-
-			confParseValue(storage.clashColor, 0, COLORS - 1, 1, dfplayer);
-
-			if (modification) {
-
-				modification = 0;
-				storage.clashColor = value;
-				getColor(currentColor, storage.clashColor);
-				lightOn(ledPins, currentColor);
-#ifdef LS_INFO
-				Serial.print(storage.clashColor);
-				Serial.print("\tR:");
-				Serial.print(currentColor[0]);
-				Serial.print("\tG:");
-				Serial.print(currentColor[1]);
-				Serial.print(" \tB:");
-				Serial.println(currentColor[2]);
-#endif //LS_INFO
-			}
-
-			break;
-			case 4: //SAVE TO SOUNDFONT ?
-			confMenuStart(0, 11, dfplayer);
-
-			if (modification > 0) {
-				//Yes
-				//We save color values to this Soundfount
-				Serial.println(F("Yes"));
-				dfplayer.playPhysicalTrack(12);
-				storage.soundFontColorPreset[storage.soundFont][0] =
-				storage.mainColor;
-				storage.soundFontColorPreset[storage.soundFont][1] =
-				storage.clashColor;
-				menu++;
-				changeMenu = true;
-				enterMenu = true;
-				delay(500);
-				modification = 0;
-			} else if (modification < 0) {
-				//No
-				// we do nothing and leave this menu
-				Serial.println(F("No"));
-				dfplayer.playPhysicalTrack(13);
-				menu++;
-				changeMenu = true;
-				enterMenu = true;
-				delay(20);
-				modification = 0;
-			}
-			break;
-#endif
-/*LUXEON*/
-
-
-
-
-
-
-
-
-

@@ -1354,7 +1270,12 @@
 //				EEPROM.update(i, 0);
 //				//			 }
 //			}
+#if defined LUXEON
+			lightOff(ledPins);
+#else
 			lightOff();
+#endif
+
 			dfplayer.playPhysicalTrack(3);
 			browsing = false;
 			enterMenu = false;


Remove heartbeat control LED

Remove heartbeat control LED

--- heartbeat_with_pulse.ino
+++ heartbeat_with_pulse.ino
@@ -22,9 +22,6 @@
 #define ACCELY_PIN 2
 #define ACCELZ_PIN 3
 #define PULSE_PIN 5
-#define CTRL_LED_RED_PIN 9
-#define CTRL_LED_GREEN_PIN 4
-#define CTRL_LED_BLUE_PIN 3
 #define BUTTON_PIN 4

 // ui
@@ -79,10 +76,6 @@
   FastLED.addLeds<NEOPIXEL, LED_PIN_CH2>(leds[1], NUM_LEDS_CH2);
   Serial.begin(BAUD_RATE);
   interruptSetup();
-
-  pinMode(CTRL_LED_RED_PIN, OUTPUT);
-  pinMode(CTRL_LED_GREEN_PIN, OUTPUT);
-  pinMode(CTRL_LED_BLUE_PIN, OUTPUT);
   pinMode(BUTTON_PIN, INPUT_PULLUP);

   modeButton.attach(BUTTON_PIN);
@@ -234,9 +227,6 @@
   if (pulseFound == true && pulseBpm <= maxBpm) {
 //    serialOutputWhenBeatHappens();
     bpm = pulseBpm;
-    digitalWrite(CTRL_LED_RED_PIN, HIGH);
-    digitalWrite(CTRL_LED_GREEN_PIN, LOW);
-    digitalWrite(CTRL_LED_BLUE_PIN, HIGH);
   } else {
     bpm = staticBpm;
   }
@@ -352,11 +342,6 @@
 void loop() {
   // TODO Fix button noise
 //  updateModeFromButton();
-
-  // default to all off (high)
-  digitalWrite(CTRL_LED_GREEN_PIN, HIGH);
-  digitalWrite(CTRL_LED_BLUE_PIN, HIGH);
-  digitalWrite(CTRL_LED_RED_PIN, HIGH);

   // Activate effect based on mode
   if(mode == 1) {


Add text to clarify that a message is test message

--- ARTF_Sensors/examples/MoistureSensorGSM/MoistureSensorGSM.ino
+++ ARTF_Sensors/examples/MoistureSensorGSM/MoistureSensorGSM.ino
@@ -86,7 +86,7 @@
   // 0. Send a test text message with with moisture reading and time
   // ---------------------------------------------------------------
   int roundedMoisture = takeMoistureReadings();
-  String textMessage = String(SENSOR_TYPE) + " " + String(roundedMoisture);
+  String textMessage = "Test: " + String(SENSOR_TYPE) + " " + String(roundedMoisture);

   digitalWrite(MOSFET_GSM_PIN, HIGH);
   delay(1500);


final upload...

final upload...

--- code/Sender_/Sender_.ino
+++ code/Sender_/Sender_.ino
@@ -6,8 +6,8 @@

 MCP342X myADC;

-#define NODE_ID         3
-#define NETWORK_ID      101
+#define NODE_ID         6
+#define NETWORK_ID      100
 #define GATEWAY_ID      0
 #define FREQUENCY       RF69_433MHZ
 #define KEY             "p6ZNvTmGfdY2hUXb" //has to be same 16 characters/bytes on all nodes, not more not less!


Watchdog wake from sleep if not dark

Watchdog wake from sleep if not dark

--- tracker/tracker.ino
+++ tracker/tracker.ino
@@ -69,7 +69,7 @@

 #define LED_DEBUG 13
-#define PIN_TX 12
+#define PIN_TX 6          // PD6
 #define PIN_PERIF_POWER 7 // PD7

 #define PIN_LDR_LEFT  2 // PC2
@@ -85,6 +85,9 @@

 #define DARK_THRESHOLD 500 // too dark below this

+#define SLEEP_FLAG_DARK 0  // Gone to sleep because it is dark
+#define SLEEP_FLAG_IDLE 1  // Gone to sleep because nothing to do
+
 // When the last move was attempted.
 unsigned long move_last = 0;
 // The move delay.
@@ -100,6 +103,7 @@
 byte ledState = HIGH;
 byte msgId = 0;

+byte sleep_flag;

 Servo servo_horz;

@@ -123,8 +127,8 @@
     pinMode(LED_DEBUG, OUTPUT);
     pinMode(PIN_TX, OUTPUT);

-    pinMode(PIN_PERIF_POWER);
-    digitalWrite(PIN_PERIF_POWER, HIGH); // high = 0n (NPN)
+    pinMode(PIN_PERIF_POWER, OUTPUT);
+    digitalWrite(PIN_PERIF_POWER, LOW); // low = 0n (PNP)

     pinMode(SERVO_PIN_HORZ_POWER, OUTPUT);
     digitalWrite(SERVO_PIN_HORZ_POWER, LOW); // low = 0n (PNP)
@@ -152,12 +156,13 @@
         tkr_move();

         if (light_level < DARK_THRESHOLD) {
-          //goToSleep();
+          //goToSleep(SLEEP_FLAG_DARK);
           //TODO connect PIN_LDR_LEFT to INT0
         }

     }

+    // Transmit after interval
     if (now - tx_last >= tx_interval) {
         tx_last = now;

@@ -175,6 +180,9 @@
         vw_send((uint8_t *)msg, strlen(msg));
         vw_wait_tx(); // Wait until the whole message is gone
         ++msgId;
+
+        // Watchdog will wake us up in 8 seconds time.
+        goToSleep(SLEEP_FLAG_IDLE);
     }
 }

@@ -225,10 +233,11 @@
     return 0;
 }

-
+/**
+ * Watchdog for while awake to ensure things are ticking over.
+ */
 void watchdog_setup()
 {
-
   // Clear any previous watchdog interupt
   MCUSR = 0;

@@ -238,23 +247,27 @@
   wdt_reset();
 }

-void goToSleep()
-{
+void goToSleep(byte flag)
+{
+  sleep_flag = flag;
+
   // Power down the periferals
-  digitalWrite(PIN_PERIF_POWER, LOW);
+  digitalWrite(PIN_PERIF_POWER, HIGH);

   cli();

-  // Turn off watchdog
-  wdt_disable();

   // disable ADC
   ADCSRA = 0;

   set_sleep_mode(SLEEP_MODE_PWR_DOWN);

-  // Attach the external interupt to the LDR.
-  attachInterrupt(0, wakeUp, HIGH);
+  if (sleep_flag == SLEEP_FLAG_DARK) {
+    // Turn off watchdog
+    wdt_disable();
+    // Attach the external interupt to the LDR.
+    attachInterrupt(0, wakeUp, HIGH);
+  }

   power_all_disable();  // power off ADC, Timer 0 and 1, serial interface
   sleep_enable();
@@ -267,18 +280,20 @@

   sleep_disable();
-  //MCUSR = 0; // clear the reset register
-
-  watchdog_setup(); // watchdog back on
-
-  detachInterrupt(0); // No wakeup interupt while awake
+  MCUSR = 0; // clear the reset register
+
+
+  if (sleep_flag == SLEEP_FLAG_DARK) {
+    detachInterrupt(0); // No wakeup interupt while awake
+    watchdog_setup(); // watchdog back on
+  }

   power_all_enable();    // power everything back on

   ADCSRA = (1 << ADEN); // ADC back on

   // Power up the periferals
-  digitalWrite(PIN_PERIF_POWER, HIGH); // high = 0n (NPN)
+  digitalWrite(PIN_PERIF_POWER, LOW); // (PNP)
 }

 /**


Removed virticle movement

Removed virticle movement

--- tracker/tracker.ino
+++ tracker/tracker.ino
@@ -23,21 +23,16 @@
 #define LED_DEBUG 13
 #define PIN_TX 7

-#define PIN_LDR_UP    0 // PC0
-#define PIN_LDR_DOWN  1 // PC1
 #define PIN_LDR_LEFT  2 // PC2
 #define PIN_LDR_RIGHT 3 // PC3
-#define THRESHOLD_LDR_VIRT 50
 #define THRESHOLD_LDR_HORZ 50
 #define MOVE_INTERVAL_MILLIS 60

 #define SERVO_INCREMENT 5 // How much to move each loop
 #define SERVO_PIN_HORZ 8 // PB1
-#define SERVO_PIN_VIRT 9 // PB0
 #define SERVO_HORZ_MAX 2800
 #define SERVO_HORZ_MIN 700
-#define SERVO_VIRT_MAX 1900
-#define SERVO_VIRT_MIN 1200
+

 #define I2C_SLAVE_ADDR 100

@@ -55,12 +50,9 @@
 byte msgId = 0;

-Servo servo_virt;
 Servo servo_horz;

 // Store the current servo positions.
-// Not using sero.read() as that is degrees.
-int servo_virt_pos = 1500;
 int servo_horz_pos = 1700;

 void setup()
@@ -72,10 +64,8 @@
     pinMode(LED_DEBUG, OUTPUT);
     pinMode(PIN_TX, OUTPUT);

-    servo_virt.attach(SERVO_PIN_VIRT, SERVO_VIRT_MIN, SERVO_VIRT_MAX);
     servo_horz.attach(SERVO_PIN_HORZ, SERVO_HORZ_MIN, SERVO_HORZ_MAX);

-    servo_virt.writeMicroseconds(servo_virt_pos);
     servo_horz.writeMicroseconds(servo_horz_pos);

     // Join the i2c bus as master
@@ -119,31 +109,8 @@

 void tkr_move()
 {
-    int diff_virt = tkr_diff_virt();
     int diff_horz = tkr_diff_horz();
     //Serial.println();
-
-    if (diff_virt) {
-      // move up or down
-
-      if (diff_virt > 0) {
-        servo_virt_pos += SERVO_INCREMENT;
-        if (servo_virt_pos > SERVO_VIRT_MAX) servo_virt_pos = SERVO_VIRT_MAX;
-      } else {
-        servo_virt_pos -= SERVO_INCREMENT;
-        if (servo_virt_pos < SERVO_VIRT_MIN) servo_virt_pos = SERVO_VIRT_MIN;
-      }
-
-      Serial.print("V > ");
-      Serial.print(diff_virt);
-      Serial.print('-');
-      Serial.println(servo_virt_pos);
-
-      // move a degree each loop until target is reached.
-      //servo_virt.attach(SERVO_PIN_VIRT);
-      servo_virt.writeMicroseconds(servo_virt_pos);
-
-    }

     if (diff_horz) {
       // move left or right.
@@ -161,32 +128,11 @@
       Serial.print("-");
       Serial.println(servo_horz_pos);

-      // move a degree each loop until target is reached.
-      //servo_horz.attach(SERVO_PIN_HORZ);
+      // move a bit each loop until target is reached.
       servo_horz.writeMicroseconds(servo_horz_pos);

     }

-}
-
-int tkr_diff_virt()
-{
-    int up = analogRead(PIN_LDR_UP);
-    int down = analogRead(PIN_LDR_DOWN);
-
-    /*
-    Serial.print(up);
-    Serial.print(":");
-    Serial.print(down);
-    Serial.print(":");
-    */
-
-    int diff = up - down;
-    if (abs(diff) > THRESHOLD_LDR_VIRT) {
-      return diff;
-    }
-
-    return 0;
 }

 int tkr_diff_horz()


Remove delay in shutting down the car

Remove delay in shutting down the car

--- VehicleController/VehicleController.ino
+++ VehicleController/VehicleController.ino
@@ -172,7 +172,6 @@

         if (digitalRead(PIN_START_CAR) == HIGH) {
             digitalWriteFast(PIN_CLOSE_AIR, LOW);
-            delay(10);
             digitalWriteFast(PIN_SHUTDOWN_CTRL, LOW);
             msTimer.end();


Cleanup.

Cleanup.

--- examples/CosaSampleSet/CosaSampleSet.ino
+++ examples/CosaSampleSet/CosaSampleSet.ino
@@ -31,12 +31,12 @@
 #include "Cosa/Watchdog.h"

 // Analog input pins
-
 AnalogPin levelPin(0);
 AnalogPin basePin(1);
 AnalogPin powerPin(2);
 AnalogPin tempVCC(8);

+// Analog pin vector for pin set. Note: use program memory
 const PROGMEM AnalogPin* pins[] = {
   &levelPin,
   &basePin,


more friendly variable name

more friendly variable name

--- examples/Widgets/TimeInput/TimeInput.ino
+++ examples/Widgets/TimeInput/TimeInput.ino
@@ -36,14 +36,14 @@
 }

 BLYNK_WRITE(V1) {
-  TimeInputParam time_input(param);
+  TimeInputParam timeInput(param);

   // Process start time

-  switch (time_input.getStartMode())
+  switch (timeInput.getStartMode())
   {
     case TimeInputParam::TIME_SPECIFIED:
-      Serial.println(String("Start: ") + time_input.getStartHour() + ":" + time_input.getStartMinute() + ":" + time_input.getStartSecond());
+      Serial.println(String("Start: ") + timeInput.getStartHour() + ":" + timeInput.getStartMinute() + ":" + timeInput.getStartSecond());
       break;
     case TimeInputParam::TIME_SUNRISE:
       Serial.println("Start at sunrize");
@@ -59,10 +59,10 @@

   // Process stop time

-  switch (time_input.getStopMode())
+  switch (timeInput.getStopMode())
   {
     case TimeInputParam::TIME_SPECIFIED:
-      Serial.println(String("Stop: ") + time_input.getStopHour() + ":" + time_input.getStopMinute() + ":" + time_input.getStopSecond());
+      Serial.println(String("Stop: ") + timeInput.getStopHour() + ":" + timeInput.getStopMinute() + ":" + timeInput.getStopSecond());
       break;
     case TimeInputParam::TIME_SUNRISE:
       Serial.println("Stop at sunrise");
@@ -77,12 +77,12 @@

   // Process timezone (in seconds, it is already added to start/stop time)

-  Serial.println(String("Time zone: ") + time_input.getTZ());
+  Serial.println(String("Time zone: ") + timeInput.getTZ());

   // Process weekdays

   for (int i = 1; i <= 7; i++) {
-    if (time_input.isWeekdaySelected(i)) {
+    if (timeInput.isWeekdaySelected(i)) {
       Serial.println(String("Day ") + i + " is selected");
     }
   }


adapt to teensy

adapt to teensy

--- pug.ino
+++ pug.ino
@@ -1,18 +1,21 @@
 #include <Servo.h>

-#define rightMotorPin1 12
-#define rightMotorPin2 11
-
-#define leftMotorPin1 9
-#define leftMotorPin2 10
-
-#define ultraTrig 14
-#define ultraEcho 15
-
-#define starterServoPin 18
+#define rightMotorPin1 6
+#define rightMotorPin2 5
+
+#define leftMotorPin1 3
+#define leftMotorPin2 4
+
+IntervalTimer motorTimer;
+
+#define ultraTrig 20
+#define ultraEcho 21
+
+#define starterServoPin 19

 #define rotateTime 600

+IntervalTimer ultraTimer;
 int ultraBuffer[10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
 int ultraCompteur;
 int ultraSum;
@@ -42,18 +45,25 @@
   delayMicroseconds(10);
   digitalWrite(ultraTrig, LOW);
   lecture_echo = pulseIn(ultraEcho, HIGH);
-  cm = lecture_echo/29;
+  cm = lecture_echo / 29;

   return cm;
 }

-void ultraCheck(int ultraBuffer[10]) {
-  long cm = 0;
-  for (int i = 0; i < 10; i++) {
-    cm += ultraBuffer[i] / 10;
-  }
-
-  Serial.println(cm);
+void ultraCheck(void) {
+  int i = ultraCompteur % 10;
+  int first_i = (ultraCompteur + 1) % 10;
+  ultraBuffer[i] = ultraMesure();
+  ultraSum += ultraBuffer[i];
+  ultraSum -= ultraBuffer[first_i];
+  Serial.println(ultraSum / 10);
+
+  if (ultraSum / 10 < 20) {
+    digitalWrite(13, HIGH);
+  } else {
+    digitalWrite(13, LOW);
+  }
+  ultraCompteur++;
 }

 void stopRight() {
@@ -122,7 +132,7 @@
     delay(50);
   }
   starterServo.detach();
-  initializeTimer1();
+  //  initializeTimer1();
 }

 void prepareToStart () {
@@ -133,29 +143,18 @@
 /**
  * Interrupt service routine
  **/
-ISR(TIMER1_OVF_vect)
+void manageMotors()
 {
   loopCounter = (loopCounter + 1) % 200;
-  TCNT1 = 64286; // preload timer
-
+
+  if (rightSpeed == 100) stopRight();
+  else if (loopCounter >= rightSpeed)forwardRight();
+  else backwardRight();
+
   if (leftSpeed == 100) stopLeft();
   else if (loopCounter >= leftSpeed)forwardLeft();
   else backwardLeft();

-  if (rightSpeed == 100) stopRight();
-  else if (loopCounter >= rightSpeed)forwardRight();
-  else backwardRight();
-}
-
-void initializeTimer1 () {
-  noInterrupts();           // disable all interrupts
-  TCCR1A = 0;
-  TCCR1B = 0;
-
-  TCNT1 = 64286;            // preload timer
-  TCCR1B |= (1 << CS10);    // 0 prescaler
-  TIMSK1 |= (1 << TOIE1);   // enable timer overflow interrupt
-  interrupts();             // enable all interrupts
 }

 void setup() {
@@ -167,38 +166,27 @@
   pinMode(rightMotorPin1, OUTPUT);
   pinMode(rightMotorPin2, OUTPUT);

+  motorTimer.begin(manageMotors, 30);
+
   // Ultrason
+  ultraTimer.begin(ultraCheck, 50000);
   pinMode(ultraTrig, OUTPUT);
   digitalWrite(ultraTrig, LOW);
   pinMode(ultraEcho, INPUT);
   ultraCompteur = 1;
   ultraSum = 0;
-
+
   //DebugLed
-  pinMode(13,OUTPUT);
+  pinMode(13, OUTPUT);

   // Serial
   Serial.begin(9600);

-  initializeTimer1();
+  //  initializeTimer1();
 }

 void loop() {
   // TODO Script to win here ;)
-  int i = ultraCompteur % 10;
-  int first_i = (ultraCompteur + 1) % 10;
-  ultraBuffer[i] = ultraMesure();
-  ultraSum += ultraBuffer[i];
-  ultraSum -= ultraBuffer[first_i];
-
-if (ultraSum/10 < 20){
-  digitalWrite(13,HIGH);
-  } else {
-    digitalWrite(13,LOW);
-    }
-  ultraCompteur++;
-
-
 }

 // Serial
@@ -235,11 +223,6 @@
 {
   switch (serialInput[0])
   {
-    case 'i':
-    case 'I': // Initialize
-      initializeTimer1();
-      break;
-
     case 'r':
     case 'R': // Rotation
       if (serialInput[1] == 'L' || serialInput[1] == 'l') {
@@ -285,6 +268,19 @@
       stopMotors();
       break;

+    case 'd':
+    case 'D':
+      Serial.println("Right on");
+      setRightSpeed( 30);
+      break;
+
+    case 'l':
+    case 'L':
+      Serial.println("left on");
+      setLeftSpeed( 30);
+      break;
+
+
     default:
       Serial.print("Unkwnown command (");
       Serial.print(serialInput[0]);


Update servopin

Update servopin

--- pug.ino
+++ pug.ino
@@ -6,7 +6,7 @@
 #define leftMotorPin1 9
 #define leftMotorPin2 10

-#define starterServoPin 15
+#define starterServoPin 18

 #define rotateTime 750


Created SIPO Structure to make it easier to populate properties and send to the board

--- SIPO.ino
+++ SIPO.ino
@@ -7,14 +7,55 @@
 // Used to set each bit with a bitwise or
 unsigned char hex[8]= {0x01  ,0x02  ,0x04  ,0x08  ,0x10  ,0x20  ,0x40  ,0x80};

-void setup() {
-  SPI.begin();
-}
+/*

-void loop() {
-  NightRider();
-  //BitCounter(1000);
-  //Step(100); // Worked at 10
+P4
+P3
+P2
+P1
+4DIR
+4STEP
+4EN
+3DIR
+3STEP
+3EN
+2DIR
+2STEP
+2EN
+1DIR
+1STEP
+1EN
+
+*/
+
+typedef struct {
+  boolean P[4]; // Output Pins
+  boolean EN[4];       // Enable Pins
+  boolean STEP[4];     // Step Pins
+  boolean DIR[4];      // Direction Pins
+} sipo_t;
+
+sipo_t SIPO;
+
+void SendSIPO()
+{
+    // Populate SIPO
+  for(int K=0;K<4;K++)
+  {
+    SIPO.P[K] = 1;
+  }
+
+
+  int byte1 = 0;
+  int byte2 = 0;
+
+  if(SIPO.P[0]) byte2 = byte2 | hex[8-1];
+  if(SIPO.P[1]) byte2 = byte2 | hex[7-1];
+  if(SIPO.P[2]) byte2 = byte2 | hex[6-1];
+  if(SIPO.P[3]) byte2 = byte2 | hex[5-1];
+
+  Send2Bytes(0,byte2);
+
 }

 void NightRider()
@@ -75,3 +116,17 @@
     delay(waitTime);
   }
 }
+
+
+void setup() {
+  SPI.begin();
+}
+
+void loop() {
+
+  //SendSIPO();
+  NightRider();
+  //BitCounter(1000);
+  //Step(100); // Worked at 10
+}
+


Handle unsigned long values in the URL (#142)

toInt() only handles long correct, not unsigned long. Use strtoul() instead.

--- examples/IRServer/IRServer.ino
+++ examples/IRServer/IRServer.ino
@@ -26,7 +26,7 @@
   for (uint8_t i=0; i<server.args(); i++){
     if(server.argName(i) == "code")
     {
-      unsigned long code = server.arg(i).toInt();
+      unsigned long code = strtoul(server.arg(i).c_str(), NULL, 10);
       irsend.sendNEC(code, 32);
     }
   }


Blinken solange Schranke geschlossen

Blinken solange Schranke geschlossen

--- examples/Schranke/Schranke.ino
+++ examples/Schranke/Schranke.ino
@@ -49,7 +49,6 @@
       Serial.println(SchrankePos);
       taskDelay(SchrankeInterval);
     }
-    blinken = false;
   } else { //öffnen
     while (SchrankePos > 0) {
       SchrankePos -= SchrankeStep;
@@ -57,6 +56,7 @@
       Serial.println(SchrankePos);
       taskDelay(SchrankeInterval);
     }
+    blinken = false;
   }
   SchrankeZu = SchrankePos >= PosZu;
   taskEnd();