correcting date/time format and removing unnecessary code

--- Arduino/I2CSensorsMicro/I2CSensorsMicro.ino
+++ Arduino/I2CSensorsMicro/I2CSensorsMicro.ino
@@ -48,6 +48,8 @@
   outputFile = SD.open("output.txt", FILE_WRITE);
   Serial.flush();
   Serial.println(F("\n\nDate\tTime\tDO\tORP\tpH\tEC\tTDS\tSAL\tSG"));
+  outputFile.println(F("\n\nDate\tTime\tDO\tORP\tpH\tEC\tTDS\tSAL\tSG"));
+
 }

@@ -89,66 +91,18 @@
   DateTime now = rtc.now();

   String date = String(now.month(), DEC) + "/" + String(now.day(), DEC) + "/" + String(now.year(), DEC) + "\t";
-  date +=  String(now.hour(), DEC) + ":" + String(now.minute()) + ":" + String(now.second());
+  if (now.hour() < 10)
+    date += "0";
+  date +=  String(now.hour(), DEC) + ":";
+  if (now.minute() < 10)
+    date += "0";
+  date += String(now.minute(), DEC) + ":";
+  if (now.second() < 10)
+    date += "0";
+  date += String(now.second(), DEC);
   return date;
 }

-
-/*
-
-*********************************************************
-*                                                       *
-*               Real Time Clock                         *
-*                                                       *
-*********************************************************
-
-For the real time clock we are going to use the code available from gravitech http://site.gravitech.us/MicroResearch/I2C/I2C-RTC/I2C-RTC-Arduino.pde
-This just displays the time. At some point we need to have set the time!
-
-*/
-
-
-
-String RTC_time() {
-  char tempchar [7];
-  byte i = 0;
-  Wire.beginTransmission(rtc_address);
-  Wire.write(0);
-  Wire.endTransmission();
-
-  Wire.requestFrom(rtc_address, 7);
-  while(Wire.available()) {
-    tempchar[i] = Wire.read(); // receive a byte as character
-    i++;
-  }
-
-  byte Second = tempchar[0];
-  byte Minute = tempchar[1];
-  byte Hour   = tempchar[2];
-  byte Day    = tempchar[3];
-  byte Date   = tempchar[4];
-  byte Month  = tempchar[5];
-  byte Year   = tempchar[6];
-
-  // Display time
-  String ret = "";
-  ret += String(Month, HEX) + "/" + String(Date, HEX) + "/";
-  if (Year<10)
-    ret += "0";
-  ret += String(Year, HEX) + "\t";
-  if (Hour < 10)
-    ret += "0";
-  ret += String(Hour, HEX) + ":";
-  if (Minute < 10)
-    ret += "0";
-  ret += String(Minute, HEX) + ":";
-  if (Second < 10)
-    ret += "0";
-  ret += String(Second, HEX);
-
-  return ret;
-
-}

 /*


Update for sensors

Update for sensors

2 pins only for line detection, remember to cut traces to middle four sensors.

--- MyCode/NQRoS/NQRoS.ino
+++ MyCode/NQRoS/NQRoS.ino
@@ -14,11 +14,12 @@
 #define LED 13
 Pushbutton button(ZUMO_BUTTON); // pushbutton on pin 12
 // Reflectance Sensor Settings
-#define NUM_SENSORS 6
+#define NUM_SENSORS 2
+byte pins[] = [4, 5];
 unsigned int sensor_values[NUM_SENSORS];
 // this might need to be tuned for different lighting conditions, surfaces, etc.
 #define QTR_THRESHOLD  1500 // microseconds
-ZumoReflectanceSensorArray sensors(QTR_NO_EMITTER_PIN);
+ZumoReflectanceSensorArray sensors(pins, 2, 2000, QTR_NO_EMITTER_PIN);

 // Motor Settings
 ZumoMotors motors;
@@ -103,8 +104,8 @@
   }
   else  // otherwise, go straight
   {
-    lVal = analogRead(4);   // reads the value of the sharp sensor
-    rVal = analogRead(5);
+    lVal = analogRead(0);   // reads the value of the sharp sensor
+    rVal = analogRead(1);
     lVoltage = lVal*(5/1023.0);
     rVoltage = rVal*(5/1023.0);
     lDist = 27.0570*pow(lVoltage,-1.1811);//pow(((val*(5/1023.0)*0.001221)/16.251),1.1765);


Tweaked the delays.

Tweaked the delays.

--- LED_Blink/LED_Blink.ino
+++ LED_Blink/LED_Blink.ino
@@ -7,8 +7,16 @@

 void loop(void)
 {
-  digitalWrite(13, HIGH);   // set the LED on
-  delay(1000);              // wait for a second
-  digitalWrite(13, LOW);    // set the LED off
-  delay(1000);              // wait for a second
+  digitalWrite(13, HIGH);
+  delay(50);
+  digitalWrite(13, LOW);
+  delay(50);
+  digitalWrite(13, HIGH);
+  delay(50);
+  digitalWrite(13, LOW);
+  delay(05);
+  digitalWrite(13, HIGH);
+  delay(50);
+  digitalWrite(13, LOW);
+  delay(1000);
 }


Battery alarm level canceled

Battery alarm level canceled

--- OTTO_todo/OTTO_todo.ino
+++ OTTO_todo/OTTO_todo.ino
@@ -1,1103 +1,978 @@
-
-//----------------------------------------------------------------
-//-- Zowi basic firmware v2 adapted to Otto
-//-- (c) BQ. Released under a GPL licencse
-//-- 04 December 2015
-//-- Authors:  Anita de Prado: ana.deprado@bq.com
-//--           Jose Alberca:   jose.alberca@bq.com
-//--           Javier Isabel:  javier.isabel@bq.com
-//--           Juan Gonzalez (obijuan): juan.gonzalez@bq.com
-//--           Irene Sanz : irene.sanz@bq.com
-//-----------------------------------------------------------------
-//-- Experiment with all the features that Otto has thanks to Zowi!
-//-----------------------------------------------------------------
-
-#include <Servo.h>
-#include <Oscillator.h>
-#include <EEPROM.h>
-#include <BatReader.h>
-#include <US.h>
-#include <LedMatrix.h>
-
-//-- Library to manage external interruptions
-#include <EnableInterrupt.h>
-
-//-- Library to manage serial commands
-#include <OttoSerialCommand.h>
-OttoSerialCommand SCmd;  //The SerialCommand object
-
-//-- Otto Library
-#include <Otto.h>
-Otto Otto;  //This is Otto!
-
-//---------------------------------------------------------
-//-- First step: Configure the pins where the servos are attached
-/*
-         ---------------
-        |     O   O     |
-        |---------------|
-YR 3==> |               | <== YL 2
-         ---------------
-            ||     ||
-            ||     ||
-RR 5==>   -----   ------  <== RL 4
-         |-----   ------|
-*/
-
-  #define PIN_YL 2 //servo[0]
-  #define PIN_YR 3 //servo[1]
-  #define PIN_RL 4 //servo[2]
-  #define PIN_RR 5 //servo[3]
-
-//---Otto Buttons
-#define PIN_SecondButton 6
-#define PIN_ThirdButton 7
-
-///////////////////////////////////////////////////////////////////
-//-- Global Variables -------------------------------------------//
-///////////////////////////////////////////////////////////////////
-
-const char programID[]="Otto_todo"; //Each program will have a ID
-
-const char name_fac='$'; //Factory name
-const char name_fir='#'; //First name
-
-//-- Movement parameters
-int T=1000;              //Initial duration of movement
-int moveId=0;            //Number of movement
-int moveSize=15;         //Asociated with the height of some movements
-
-//---------------------------------------------------------
-//-- Otto has 5 modes:
-//--    * MODE = 0: Otto is awaiting
-//--    * MODE = 1: Dancing mode!
-//--    * MODE = 2: Obstacle detector mode
-//--    * MODE = 3: Noise detector mode
-//--    * MODE = 4: OttoPAD or any Teleoperation mode (listening SerialPort).
-//---------------------------------------------------------
-volatile int MODE=0; //State of Otto in the principal state machine.
-
-volatile bool buttonPushed=false;  //Variable to remember when a button has been pushed
-volatile bool buttonAPushed=false; //Variable to remember when A button has been pushed
-volatile bool buttonBPushed=false; //Variable to remember when B button has been pushed
-
-unsigned long previousMillis=0;
-
-int randomDance=0;
-int randomSteps=0;
-
-bool obstacleDetected = false;
-
-
-///////////////////////////////////////////////////////////////////
-//-- Setup ------------------------------------------------------//
-///////////////////////////////////////////////////////////////////
-void setup(){
-
-  //Serial communication initialization
-  Serial.begin(115200);
-
-  pinMode(PIN_SecondButton,INPUT);
-  pinMode(PIN_ThirdButton,INPUT);
-
-  //Set the servo pins
-  Otto.init(PIN_YL,PIN_YR,PIN_RL,PIN_RR,true);
-
-  //Uncomment this to set the servo trims manually and save on EEPROM
-    //Otto.setTrims(TRIM_YL, TRIM_YR, TRIM_RL, TRIM_RR);
-    //Otto.saveTrimsOnEEPROM(); //Uncomment this only for one upload when you finaly set the trims.
-
-  //Set a random seed
-  randomSeed(analogRead(A6));
-
-  //Interrumptions
-  enableInterrupt(PIN_SecondButton, secondButtonPushed, RISING);
-  enableInterrupt(PIN_ThirdButton, thirdButtonPushed, RISING);
-
-  //Setup callbacks for SerialCommand commands
-  SCmd.addCommand("S", receiveStop);      //  sendAck & sendFinalAck
-  SCmd.addCommand("L", receiveLED);       //  sendAck & sendFinalAck
-  SCmd.addCommand("T", recieveBuzzer);    //  sendAck & sendFinalAck
-  SCmd.addCommand("M", receiveMovement);  //  sendAck & sendFinalAck
-  SCmd.addCommand("H", receiveGesture);   //  sendAck & sendFinalAck
-  SCmd.addCommand("K", receiveSing);      //  sendAck & sendFinalAck
-  SCmd.addCommand("C", receiveTrims);     //  sendAck & sendFinalAck
-  SCmd.addCommand("G", receiveServo);     //  sendAck & sendFinalAck
-  SCmd.addCommand("R", receiveName);      //  sendAck & sendFinalAck
-  SCmd.addCommand("E", requestName);
-  SCmd.addCommand("D", requestDistance);
-  SCmd.addCommand("N", requestNoise);
-  SCmd.addCommand("B", requestBattery);
-  SCmd.addCommand("I", requestProgramId);
-  SCmd.addDefaultHandler(receiveStop);
-
-
-
-  //Otto wake up!
-  Otto.sing(S_connection);
-  Otto.home();
-
-
-  //If Otto's name is '&' (factory name) means that is the first time this program is executed.
-  //This first time, Otto mustn't do anything. Just born at the factory!
-  //5 = EEPROM address that contains first name character
-  if (EEPROM.read(5)==name_fac){
-
-    EEPROM.put(5, name_fir); //From now, the name is '#'
-    EEPROM.put(6, '\0');
-    Otto.putMouth(culito);
-
-    while(true){
-       delay(1000);
-    }
-  }
-
-
-  //Send Otto name, programID & battery level.
-  requestName();
-  delay(50);
-  requestProgramId();
-  delay(50);
-  requestBattery();
-
-  //Checking battery
-  OttoLowBatteryAlarm();
-
-
- // Animation Uuuuuh - A little moment of initial surprise
- //-----
-  for(int i=0; i<2; i++){
-      for (int i=0;i<8;i++){
-        if(buttonPushed){break;}
-        Otto.putAnimationMouth(littleUuh,i);
-        delay(150);
-      }
-  }
- //-----
-
-
-  //Smile for a happy Otto :)
-  if(!buttonPushed){
-    Otto.putMouth(smile);
-    Otto.sing(S_happy);
-    delay(200);
-  }
-
-
-  //If Otto's name is '#' means that Otto hasn't been baptized
-  //In this case, Otto does a longer greeting
-  //5 = EEPROM address that contains first name character
-  if (EEPROM.read(5)==name_fir){
-
-    if(!buttonPushed){
-        Otto.jump(1,700);
-        delay(200);
-    }
-
-    if(!buttonPushed){
-        Otto.shakeLeg(1,T,1);
-    }
-
-    if(!buttonPushed){
-        Otto.putMouth(smallSurprise);
-        Otto.swing(2,800,20);
-        Otto.home();
-    }
-  }
-
-
-  if(!buttonPushed){
-    Otto.putMouth(happyOpen);
-  }
-
-  previousMillis = millis();
-
-}
-
-
-
-///////////////////////////////////////////////////////////////////
-//-- Principal Loop ---------------------------------------------//
-///////////////////////////////////////////////////////////////////
-void loop() {
-
-
-  if (Serial.available()>0 && MODE!=4){
-
-    MODE=4;
-    Otto.putMouth(happyOpen);
-
-    //Disable Pin Interruptions
-    disableInterrupt(PIN_SecondButton);
-    disableInterrupt(PIN_ThirdButton);
-
-    buttonPushed=false;
-  }
-
-
-  //First attemp to initial software
-  if (buttonPushed){
-
-    Otto.home();
-
-    delay(100); //Wait for all buttons
-    Otto.sing(S_buttonPushed);
-    delay(200); //Wait for all buttons
-
-    if      ( buttonAPushed && !buttonBPushed){ MODE=1; Otto.sing(S_mode1);}
-    else if (!buttonAPushed && buttonBPushed) { MODE=2; Otto.sing(S_mode2);}
-    else if ( buttonAPushed && buttonBPushed) { MODE=3; Otto.sing(S_mode3);} //else
-
-    Otto.putMouth(MODE);
-
-    int showTime = 2000;
-    while((showTime>0)){ //Wait to show the MODE number
-
-        showTime-=10;
-        delay(10);
-    }
-
-    Otto.putMouth(happyOpen);
-
-    buttonPushed=false;
-    buttonAPushed=false;
-    buttonBPushed=false;
-
-  }else{
-
-    switch (MODE) {
-
-      //-- MODE 0 - Otto is awaiting
-      //---------------------------------------------------------
-      case 0:
-
-        //Every 80 seconds in this mode, Otto falls asleep
-        if (millis()-previousMillis>=80000){
-            OttoSleeping_withInterrupts(); //ZZzzzzz...
-            previousMillis=millis();
-        }
-
-        break;
-
-
-      //-- MODE 1 - Dance Mode!
-      //---------------------------------------------------------
-      case 1:
-
-        randomDance=random(5,21); //5,20
-        if((randomDance>14)&&(randomDance<19)){
-            randomSteps=1;
-            T=1600;
-        }
-        else{
-            randomSteps=random(3,6); //3,5
-            T=1000;
-        }
-
-        Otto.putMouth(random(10,21));
-
-        for (int i=0;i<randomSteps;i++){
-            move(randomDance);
-            if(buttonPushed){break;}
-        }
-        break;
-
-
-      //-- MODE 2 - Obstacle detector mode
-      //---------------------------------------------------------
-      case 2:
-
-        if(obstacleDetected){
-
-            if(!buttonPushed){
-              Otto.putMouth(bigSurprise);
-              Otto.sing(S_surprise);
-              Otto.jump(5, 500);
-            }
-
-            if(!buttonPushed){
-              Otto.putMouth(confused);
-              Otto.sing(S_cuddly);
-            }
-
-            //Otto takes two steps back
-            for(int i=0;i<3;i++){
-              if(buttonPushed){break;}
-              Otto.walk(1,1300,-1);
-            }
-
-            delay(100);
-            obstacleDetector();
-            delay(100);
-
-
-           //If there are no obstacles and no button is pressed, Otto shows a smile
-           if((obstacleDetected==true)||(buttonPushed==true)){break;}
-           else{
-              Otto.putMouth(smile);
-              delay(50);
-              obstacleDetector();
-           }
-
-
-           //If there are no obstacles and no button is pressed, Otto shows turns left
-           for(int i=0; i<3; i++){
-              if((obstacleDetected==true)||(buttonPushed==true)){break;}
-              else{
-                  Otto.turn(1,1000,1);
-                  obstacleDetector();
-              }
-           }
-
-            //If there are no obstacles and no button is pressed, Otto is happy
-            if((obstacleDetected==true)||(buttonPushed==true)){break;}
-            else{
-                Otto.home();
-                Otto.putMouth(happyOpen);
-                Otto.sing(S_happy_short);
-                delay(200);
-            }
-
-
-        }else{
-
-            Otto.walk(1,1000,1); //Otto walk straight
-            obstacleDetector();
-        }
-
-        break;
-
-
-      //-- MODE 3 - Noise detector mode
-      //---------------------------------------------------------
-      case 3:
-        if (Otto.getNoise()>=650){ //740
-
-          delay(50);  //Wait for the possible 'lag' of the button interruptions.
-                      //Sometimes, the noise sensor detect the button before the interruption takes efect
-
-          if(!buttonPushed){
-
-            Otto.putMouth(bigSurprise);
-            Otto.sing(S_OhOoh);
-
-            if(buttonPushed){break;}
-            Otto.putMouth(random(10,21));
-            randomDance=random(5,21);
-            move(randomDance);
-            Otto.home();
-            delay(500); //Wait for possible noise of the servos while get home
-          }
-
-          if(!buttonPushed){Otto.putMouth(happyOpen);}
-        }
-        break;
-
-
-      //-- MODE 4 - OttoPAD or any Teleoperation mode (listening SerialPort)
-      //---------------------------------------------------------
-      case 4:
-
-        SCmd.readSerial();
-
-        //If Otto is moving yet
-        if (Otto.getRestState()==false){
-          move(moveId);
-        }
-
-        break;
-
-
-      default:
-          MODE=4;
-          break;
-    }
-
-  }
-
-}
-
-
-
-///////////////////////////////////////////////////////////////////
-//-- Functions --------------------------------------------------//
-///////////////////////////////////////////////////////////////////
-
-//-- Function executed when second button is pushed
-void secondButtonPushed(){
-
-    buttonAPushed=true;
-
-    if(!buttonPushed){
-        buttonPushed=true;
-        Otto.putMouth(smallSurprise);
-    }
-}
-
-//-- Function executed when third button is pushed
-void thirdButtonPushed(){
-
-    buttonBPushed=true;
-
-    if(!buttonPushed){
-        buttonPushed=true;
-        Otto.putMouth(smallSurprise);
-    }
-}
-
-
-//-- Function to read distance sensor & to actualize obstacleDetected variable
-void obstacleDetector(){
-
-   int distance = Otto.getDistance();
-
-        if(distance<15){
-          obstacleDetected = true;
-        }else{
-          obstacleDetected = false;
-        }
-}
-
-
-//-- Function to receive Stop command.
-void receiveStop(){
-
-    sendAck();
-    Otto.home();
-    sendFinalAck();
-
-}
-
-
-//-- Function to receive LED commands
-void receiveLED(){
-
-    //sendAck & stop if necessary
-    sendAck();
-    Otto.home();
-
-    //Examples of receiveLED Bluetooth commands
-    //L 000000001000010100100011000000000
-    //L 001111111111111111111111111111111 (todos los LED encendidos)
-    unsigned long int matrix;
-    char *arg;
-    char *endstr;
-    arg=SCmd.next();
-    //Serial.println (arg);
-    if (arg != NULL) {
-      matrix=strtoul(arg,&endstr,2);    // Converts a char string to unsigned long integer
-      Otto.putMouth(matrix,false);
-    }else{
-      Otto.putMouth(xMouth);
-      delay(2000);
-      Otto.clearMouth();
-    }
-
-    sendFinalAck();
-
-}
-
-
-//-- Function to receive buzzer commands
-void recieveBuzzer(){
-
-    //sendAck & stop if necessary
-    sendAck();
-    Otto.home();
-
-    bool error = false;
-    int frec;
-    int duration;
-    char *arg;
-
-    arg = SCmd.next();
-    if (arg != NULL) { frec=atoi(arg); }    // Converts a char string to an integer
-    else {error=true;}
-
-    arg = SCmd.next();
-    if (arg != NULL) { duration=atoi(arg); } // Converts a char string to an integer
-    else {error=true;}
-
-    if(error==true){
-
-      Otto.putMouth(xMouth);
-      delay(2000);
-      Otto.clearMouth();
-
-    }else{
-
-      Otto._tone(frec, duration, 1);
-    }
-
-    sendFinalAck();
-
-}
-
-
-//-- Function to receive TRims commands
-void receiveTrims(){
-
-    //sendAck & stop if necessary
-    sendAck();
-    Otto.home();
-
-    int trim_YL,trim_YR,trim_RL,trim_RR;
-
-    //Definition of Servo Bluetooth command
-    //C trim_YL trim_YR trim_RL trim_RR
-    //Examples of receiveTrims Bluetooth commands
-    //C 20 0 -8 3
-    bool error = false;
-    char *arg;
-    arg=SCmd.next();
-    if (arg != NULL) { trim_YL=atoi(arg); }    // Converts a char string to an integer
-    else {error=true;}
-
-    arg = SCmd.next();
-    if (arg != NULL) { trim_YR=atoi(arg); }    // Converts a char string to an integer
-    else {error=true;}
-
-    arg = SCmd.next();
-    if (arg != NULL) { trim_RL=atoi(arg); }    // Converts a char string to an integer
-    else {error=true;}
-
-    arg = SCmd.next();
-    if (arg != NULL) { trim_RR=atoi(arg); }    // Converts a char string to an integer
-    else {error=true;}
-
-    if(error==true){
-
-      Otto.putMouth(xMouth);
-      delay(2000);
-      Otto.clearMouth();
-
-    }else{ //Save it on EEPROM
-      Otto.setTrims(trim_YL, trim_YR, trim_RL, trim_RR);
-      Otto.saveTrimsOnEEPROM(); //Uncomment this only for one upload when you finaly set the trims.
-    }
-
-    sendFinalAck();
-
-}
-
-
-//-- Function to receive Servo commands
-void receiveServo(){
-
-    sendAck();
-    moveId = 30;
-
-    //Definition of Servo Bluetooth command
-    //G  servo_YL servo_YR servo_RL servo_RR
-    //Example of receiveServo Bluetooth commands
-    //G 90 85 96 78
-    bool error = false;
-    char *arg;
-    int servo_YL,servo_YR,servo_RL,servo_RR;
-
-    arg=SCmd.next();
-    if (arg != NULL) { servo_YL=atoi(arg); }    // Converts a char string to an integer
-    else {error=true;}
-
-    arg = SCmd.next();
-    if (arg != NULL) { servo_YR=atoi(arg); }    // Converts a char string to an integer
-    else {error=true;}
-
-    arg = SCmd.next();
-    if (arg != NULL) { servo_RL=atoi(arg); }    // Converts a char string to an integer
-    else {error=true;}
-
-    arg = SCmd.next();
-    if (arg != NULL) { servo_RR=atoi(arg); }    // Converts a char string to an integer
-    else {error=true;}
-
-    if(error==true){
-
-      Otto.putMouth(xMouth);
-      delay(2000);
-      Otto.clearMouth();
-
-    }else{ //Update Servo:
-
-      int servoPos[4]={servo_YL, servo_YR, servo_RL, servo_RR};
-      Otto._moveServos(200, servoPos);   //Move 200ms
-
-    }
-
-    sendFinalAck();
-
-}
-
-
-//-- Function to receive movement commands
-void receiveMovement(){
-
-    sendAck();
-
-    if (Otto.getRestState()==true){
-        Otto.setRestState(false);
-    }
-
-    //Definition of Movement Bluetooth commands
-    //M  MoveID  T   MoveSize
-    char *arg;
-    arg = SCmd.next();
-    if (arg != NULL) {moveId=atoi(arg);}
-    else{
-      Otto.putMouth(xMouth);
-      delay(2000);
-      Otto.clearMouth();
-      moveId=0; //stop
-    }
-
-    arg = SCmd.next();
-    if (arg != NULL) {T=atoi(arg);}
-    else{
-      T=1000;
-    }
-
-    arg = SCmd.next();
-    if (arg != NULL) {moveSize=atoi(arg);}
-    else{
-      moveSize =15;
-    }
-}
-
-
-//-- Function to execute the right movement according the movement command received.
-void move(int moveId){
-
-  bool manualMode = false;
-
-  switch (moveId) {
-    case 0:
-      Otto.home();
-      break;
-    case 1: //M 1 1000
-      Otto.walk(1,T,1);
-      break;
-    case 2: //M 2 1000
-      Otto.walk(1,T,-1);
-      break;
-    case 3: //M 3 1000
-      Otto.turn(1,T,1);
-      break;
-    case 4: //M 4 1000
-      Otto.turn(1,T,-1);
-      break;
-    case 5: //M 5 1000 30
-      Otto.updown(1,T,moveSize);
-      break;
-    case 6: //M 6 1000 30
-      Otto.moonwalker(1,T,moveSize,1);
-      break;
-    case 7: //M 7 1000 30
-      Otto.moonwalker(1,T,moveSize,-1);
-      break;
-    case 8: //M 8 1000 30
-      Otto.swing(1,T,moveSize);
-      break;
-    case 9: //M 9 1000 30
-      Otto.crusaito(1,T,moveSize,1);
-      break;
-    case 10: //M 10 1000 30
-      Otto.crusaito(1,T,moveSize,-1);
-      break;
-    case 11: //M 11 1000
-      Otto.jump(1,T);
-      break;
-    case 12: //M 12 1000 30
-      Otto.flapping(1,T,moveSize,1);
-      break;
-    case 13: //M 13 1000 30
-      Otto.flapping(1,T,moveSize,-1);
-      break;
-    case 14: //M 14 1000 20
-      Otto.tiptoeSwing(1,T,moveSize);
-      break;
-    case 15: //M 15 500
-      Otto.bend(1,T,1);
-      break;
-    case 16: //M 16 500
-      Otto.bend(1,T,-1);
-      break;
-    case 17: //M 17 500
-      Otto.shakeLeg(1,T,1);
-      break;
-    case 18: //M 18 500
-      Otto.shakeLeg(1,T,-1);
-      break;
-    case 19: //M 19 500 20
-      Otto.jitter(1,T,moveSize);
-      break;
-    case 20: //M 20 500 15
-      Otto.ascendingTurn(1,T,moveSize);
-      break;
-    default:
-        manualMode = true;
-      break;
-  }
-
-  if(!manualMode){
-    sendFinalAck();
-  }
-
-}
-
-
-//-- Function to receive gesture commands
-void receiveGesture(){
-
-    //sendAck & stop if necessary
-    sendAck();
-    Otto.home();
-
-    //Definition of Gesture Bluetooth commands
-    //H  GestureID
-    int gesture = 0;
-    char *arg;
-    arg = SCmd.next();
-    if (arg != NULL) {gesture=atoi(arg);}
-    else
-    {
-      Otto.putMouth(xMouth);
-      delay(2000);
-      Otto.clearMouth();
-    }
-
-    switch (gesture) {
-      case 1: //H 1
-        Otto.playGesture(OttoHappy);
-        break;
-      case 2: //H 2
-        Otto.playGesture(OttoSuperHappy);
-        break;
-      case 3: //H 3
-        Otto.playGesture(OttoSad);
-        break;
-      case 4: //H 4
-        Otto.playGesture(OttoSleeping);
-        break;
-      case 5: //H 5
-        Otto.playGesture(OttoFart);
-        break;
-      case 6: //H 6
-        Otto.playGesture(OttoConfused);
-        break;
-      case 7: //H 7
-        Otto.playGesture(OttoLove);
-        break;
-      case 8: //H 8
-        Otto.playGesture(OttoAngry);
-        break;
-      case 9: //H 9
-        Otto.playGesture(OttoFretful);
-        break;
-      case 10: //H 10
-        Otto.playGesture(OttoMagic);
-        break;
-      case 11: //H 11
-        Otto.playGesture(OttoWave);
-        break;
-      case 12: //H 12
-        Otto.playGesture(OttoVictory);
-        break;
-      case 13: //H 13
-        Otto.playGesture(OttoFail);
-        break;
-      default:
-        break;
-    }
-
-    sendFinalAck();
-}
-
-//-- Function to receive sing commands
-void receiveSing(){
-
-    //sendAck & stop if necessary
-    sendAck();
-    Otto.home();
-
-    //Definition of Sing Bluetooth commands
-    //K  SingID
-    int sing = 0;
-    char *arg;
-    arg = SCmd.next();
-    if (arg != NULL) {sing=atoi(arg);}
-    else
-    {
-      Otto.putMouth(xMouth);
-      delay(2000);
-      Otto.clearMouth();
-    }
-
-    switch (sing) {
-      case 1: //K 1
-        Otto.sing(S_connection);
-        break;
-      case 2: //K 2
-        Otto.sing(S_disconnection);
-        break;
-      case 3: //K 3
-        Otto.sing(S_surprise);
-        break;
-      case 4: //K 4
-        Otto.sing(S_OhOoh);
-        break;
-      case 5: //K 5
-        Otto.sing(S_OhOoh2);
-        break;
-      case 6: //K 6
-        Otto.sing(S_cuddly);
-        break;
-      case 7: //K 7
-        Otto.sing(S_sleeping);
-        break;
-      case 8: //K 8
-        Otto.sing(S_happy);
-        break;
-      case 9: //K 9
-        Otto.sing(S_superHappy);
-        break;
-      case 10: //K 10
-        Otto.sing(S_happy_short);
-        break;
-      case 11: //K 11
-        Otto.sing(S_sad);
-        break;
-      case 12: //K 12
-        Otto.sing(S_confused);
-        break;
-      case 13: //K 13
-        Otto.sing(S_fart1);
-        break;
-      case 14: //K 14
-        Otto.sing(S_fart2);
-        break;
-      case 15: //K 15
-        Otto.sing(S_fart3);
-        break;
-      case 16: //K 16
-        Otto.sing(S_mode1);
-        break;
-      case 17: //K 17
-        Otto.sing(S_mode2);
-        break;
-      case 18: //K 18
-        Otto.sing(S_mode3);
-        break;
-      case 19: //K 19
-        Otto.sing(S_buttonPushed);
-        break;
-      default:
-        break;
-    }
-
-    sendFinalAck();
-}
-
-
-//-- Function to receive Name command
-void receiveName(){
-
-    //sendAck & stop if necessary
-    sendAck();
-    Otto.home();
-
-    char newOttoName[11] = "";  //Variable to store data read from Serial.
-    int eeAddress = 5;          //Location we want the data to be in EEPROM.
-    char *arg;
-    arg = SCmd.next();
-
-    if (arg != NULL) {
-
-      //Complete newOttoName char string
-      int k = 0;
-      while((*arg) && (k<11)){
-          newOttoName[k]=*arg++;
-          k++;
-      }
-
-      EEPROM.put(eeAddress, newOttoName);
-    }
-    else
-    {
-      Otto.putMouth(xMouth);
-      delay(2000);
-      Otto.clearMouth();
-    }
-
-    sendFinalAck();
-
-}
-
-
-//-- Function to send Otto's name
-void requestName(){
-
-    Otto.home(); //stop if necessary
-
-    char actualOttoName[11]= "";  //Variable to store data read from EEPROM.
-    int eeAddress = 5;            //EEPROM address to start reading from
-
-    //Get the float data from the EEPROM at position 'eeAddress'
-    EEPROM.get(eeAddress, actualOttoName);
-
-    Serial.print(F("&&"));
-    Serial.print(F("E "));
-    Serial.print(actualOttoName);
-    Serial.println(F("%%"));
-    Serial.flush();
-}
-
-
-//-- Function to send ultrasonic sensor measure (distance in "cm")
-void requestDistance(){
-
-    Otto.home();  //stop if necessary
-
-    int distance = Otto.getDistance();
-    Serial.print(F("&&"));
-    Serial.print(F("D "));
-    Serial.print(distance);
-    Serial.println(F("%%"));
-    Serial.flush();
-}
-
-
-//-- Function to send noise sensor measure
-void requestNoise(){
-
-    Otto.home();  //stop if necessary
-
-    int microphone= Otto.getNoise(); //analogRead(PIN_NoiseSensor);
-    Serial.print(F("&&"));
-    Serial.print(F("N "));
-    Serial.print(microphone);
-    Serial.println(F("%%"));
-    Serial.flush();
-}
-
-
-//-- Function to send battery voltage percent
-void requestBattery(){
-
-    Otto.home();  //stop if necessary
-
-    //The first read of the batery is often a wrong reading, so we will discard this value.
-    double batteryLevel = Otto.getBatteryLevel();
-
-    Serial.print(F("&&"));
-    Serial.print(F("B "));
-    Serial.print(batteryLevel);
-    Serial.println(F("%%"));
-    Serial.flush();
-}
-
-
-//-- Function to send program ID
-void requestProgramId(){
-
-    Otto.home();   //stop if necessary
-
-    Serial.print(F("&&"));
-    Serial.print(F("I "));
-    Serial.print(programID);
-    Serial.println(F("%%"));
-    Serial.flush();
-}
-
-
-//-- Function to send Ack comand (A)
-void sendAck(){
-
-  delay(30);
-
-  Serial.print(F("&&"));
-  Serial.print(F("A"));
-  Serial.println(F("%%"));
-  Serial.flush();
-}
-
-
-//-- Function to send final Ack comand (F)
-void sendFinalAck(){
-
-  delay(30);
-
-  Serial.print(F("&&"));
-  Serial.print(F("F"));
-  Serial.println(F("%%"));
-  Serial.flush();
-}
-
-
-
-//-- Functions with animatics
-//--------------------------------------------------------
-
-void OttoLowBatteryAlarm(){
-
-    double batteryLevel = Otto.getBatteryLevel();
-
-    if(batteryLevel<45){
-
-      while(!buttonPushed){
-
-          Otto.putMouth(thunder);
-          Otto.bendTones (880, 2000, 1.04, 8, 3);  //A5 = 880
-
-          delay(30);
-
-          Otto.bendTones (2000, 880, 1.02, 8, 3);  //A5 = 880
-          Otto.clearMouth();
-          delay(500);
-      }
-    }
-}
-
-void OttoSleeping_withInterrupts(){
-
-  int bedPos_0[4]={100, 80, 60, 120};
-
-  if(!buttonPushed){
-    Otto._moveServos(700, bedPos_0);
-  }
-
-  for(int i=0; i<4;i++){
-
-    if(buttonPushed){break;}
-      Otto.putAnimationMouth(dreamMouth,0);
-      Otto.bendTones (100, 200, 1.04, 10, 10);
-
-    if(buttonPushed){break;}
-      Otto.putAnimationMouth(dreamMouth,1);
-      Otto.bendTones (200, 300, 1.04, 10, 10);
-
-    if(buttonPushed){break;}
-      Otto.putAnimationMouth(dreamMouth,2);
-      Otto.bendTones (300, 500, 1.04, 10, 10);
-
-    delay(500);
-
-    if(buttonPushed){break;}
-      Otto.putAnimationMouth(dreamMouth,1);
-      Otto.bendTones (400, 250, 1.04, 10, 1);
-
-    if(buttonPushed){break;}
-      Otto.putAnimationMouth(dreamMouth,0);
-      Otto.bendTones (250, 100, 1.04, 10, 1);
-
-    delay(500);
-  }
-
-  if(!buttonPushed){
-    Otto.putMouth(lineMouth);
-    Otto.sing(S_cuddly);
-  }
-
-  Otto.home();
-  if(!buttonPushed){Otto.putMouth(happyOpen);}
-
-}
+//----------------------------------------------------------------
+//-- Otto basic firmware v3 adapted from Zowi (ottobot.org)
+//-- CC BY SA
+//-- 04 December 2016
+//-----------------------------------------------------------------
+//-- Otto will be controlled with Bluetooth!
+//-----------------------------------------------------------------
+#include <Servo.h>
+#include <Oscillator.h>
+#include <EEPROM.h>
+#include <BatReader.h>
+#include <US.h>
+#include <LedMatrix.h>
+#include <EnableInterrupt.h> //-- Library to manage external interruptions
+#include <OttoSerialCommand.h> //-- Library to manage serial commands
+OttoSerialCommand SCmd;  //The SerialCommand object
+#include <Otto.h> //-- Otto Library
+Otto Otto;  //This is Otto!
+//---------------------------------------------------------
+//-- First step: Configure the pins where the servos are attached
+/*
+         ---------------
+        |     O   O     |
+        |---------------|
+YR 3==> |               | <== YL 2
+         ---------------
+            ||     ||
+RR 5==>   -----   ------  <== RL 4
+         |-----   ------|
+*/
+  #define PIN_YL 2 //servo[2]
+  #define PIN_YR 3 //servo[3]
+  #define PIN_RL 4 //servo[4]
+  #define PIN_RR 5 //servo[5]
+//---Otto Buttons
+#define PIN_SecondButton 6
+#define PIN_ThirdButton 7
+///////////////////////////////////////////////////////////////////
+//-- Global Variables -------------------------------------------//
+///////////////////////////////////////////////////////////////////
+
+const char programID[]="Otto_todo"; //Each program will have a ID
+
+const char name_fac='$'; //Factory name
+const char name_fir='#'; //First name
+
+//-- Movement parameters
+int T=1000;              //Initial duration of movement
+int moveId=0;            //Number of movement
+int moveSize=15;         //Asociated with the height of some movements
+//---------------------------------------------------------
+//-- Otto has 5 modes:
+//--    * MODE = 0: Otto is awaiting
+//--    * MODE = 1: Dancing mode!
+//--    * MODE = 2: Obstacle detector mode
+//--    * MODE = 3: Noise detector mode
+//--    * MODE = 4: OttoPAD or any Teleoperation mode (listening SerialPort).
+//---------------------------------------------------------
+volatile int MODE=0; //State of Otto in the principal state machine.
+
+volatile bool buttonPushed=false;  //Variable to remember when a button has been pushed
+volatile bool buttonAPushed=false; //Variable to remember when A button has been pushed
+volatile bool buttonBPushed=false; //Variable to remember when B button has been pushed
+
+unsigned long previousMillis=0;
+
+int randomDance=0;
+int randomSteps=0;
+
+bool obstacleDetected = false;
+
+///////////////////////////////////////////////////////////////////
+//-- Setup ------------------------------------------------------//
+///////////////////////////////////////////////////////////////////
+void setup(){
+
+  //Serial communication initialization
+  Serial.begin(115200);
+
+  pinMode(PIN_SecondButton,INPUT);
+  pinMode(PIN_ThirdButton,INPUT);
+
+  //Set the servo pins
+  Otto.init(PIN_YL,PIN_YR,PIN_RL,PIN_RR,true);
+
+  //Uncomment this to set the servo trims manually and save on EEPROM
+    //Otto.setTrims(TRIM_YL, TRIM_YR, TRIM_RL, TRIM_RR);
+    //Otto.saveTrimsOnEEPROM(); //Uncomment this only for one upload when you finaly set the trims.
+
+  //Set a random seed
+  randomSeed(analogRead(A6));
+
+  //Interrumptions
+  enableInterrupt(PIN_SecondButton, secondButtonPushed, RISING);
+  enableInterrupt(PIN_ThirdButton, thirdButtonPushed, RISING);
+
+  //Setup callbacks for SerialCommand commands
+  SCmd.addCommand("S", receiveStop);      //  sendAck & sendFinalAck
+  SCmd.addCommand("L", receiveLED);       //  sendAck & sendFinalAck
+  SCmd.addCommand("T", recieveBuzzer);    //  sendAck & sendFinalAck
+  SCmd.addCommand("M", receiveMovement);  //  sendAck & sendFinalAck
+  SCmd.addCommand("H", receiveGesture);   //  sendAck & sendFinalAck
+  SCmd.addCommand("K", receiveSing);      //  sendAck & sendFinalAck
+  SCmd.addCommand("C", receiveTrims);     //  sendAck & sendFinalAck
+  SCmd.addCommand("G", receiveServo);     //  sendAck & sendFinalAck
+  SCmd.addCommand("R", receiveName);      //  sendAck & sendFinalAck
+  SCmd.addCommand("E", requestName);
+  SCmd.addCommand("D", requestDistance);
+  SCmd.addCommand("N", requestNoise);
+  SCmd.addCommand("B", requestBattery);
+  SCmd.addCommand("I", requestProgramId);
+  SCmd.addDefaultHandler(receiveStop);
+
+  //Otto wake up!
+  Otto.sing(S_connection);
+  Otto.home();
+
+  //If Otto's name is '&' (factory name) means that is the first time this program is executed.
+  //This first time, Otto mustn't do anything. Just born at the factory!
+  //5 = EEPROM address that contains first name character
+  if (EEPROM.read(5)==name_fac){
+
+    EEPROM.put(5, name_fir); //From now, the name is '#'
+    EEPROM.put(6, '\0');
+    Otto.putMouth(culito);
+
+    while(true){
+       delay(1000);
+    }
+  }
+
+  //Send Otto name, programID & battery level.
+  requestName();
+  delay(50);
+  requestProgramId();
+  delay(50);
+
+  //If Otto's name is '#' means that Otto hasn't been baptized
+  //In this case, Otto does a longer greeting
+  //5 = EEPROM address that contains first name character
+  if (EEPROM.read(5)==name_fir){
+
+    if(!buttonPushed){
+        Otto.jump(1,700);
+        delay(200);
+    }
+
+    if(!buttonPushed){
+        Otto.shakeLeg(1,T,1);
+    }
+
+    if(!buttonPushed){
+        Otto.putMouth(smallSurprise);
+        Otto.swing(2,800,20);
+        Otto.home();
+    }
+  }
+
+  if(!buttonPushed){
+    Otto.putMouth(happyOpen);
+  }
+
+  previousMillis = millis();
+
+}
+
+///////////////////////////////////////////////////////////////////
+//-- Principal Loop ---------------------------------------------//
+///////////////////////////////////////////////////////////////////
+void loop() {
+
+  if (Serial.available()>0 && MODE!=4){
+
+    MODE=4;
+    Otto.putMouth(happyOpen);
+
+    //Disable Pin Interruptions
+    disableInterrupt(PIN_SecondButton);
+    disableInterrupt(PIN_ThirdButton);
+
+    buttonPushed=false;
+  }
+
+  //First attemp to initial software
+  if (buttonPushed){
+
+    Otto.home();
+
+    delay(100); //Wait for all buttons
+    Otto.sing(S_buttonPushed);
+    delay(200); //Wait for all buttons
+
+    if      ( buttonAPushed && !buttonBPushed){ MODE=1; Otto.sing(S_mode1);}
+    else if (!buttonAPushed && buttonBPushed) { MODE=2; Otto.sing(S_mode2);}
+    else if ( buttonAPushed && buttonBPushed) { MODE=3; Otto.sing(S_mode3);} //else
+
+    Otto.putMouth(MODE);
+
+    int showTime = 2000;
+    while((showTime>0)){ //Wait to show the MODE number
+
+        showTime-=10;
+        delay(10);
+    }
+
+    Otto.putMouth(happyOpen);
+
+    buttonPushed=false;
+    buttonAPushed=false;
+    buttonBPushed=false;
+
+  }else{
+
+    switch (MODE) {
+
+      //-- MODE 0 - Otto is awaiting
+      //---------------------------------------------------------
+      case 0:
+
+        //Every 80 seconds in this mode, Otto falls asleep
+        if (millis()-previousMillis>=80000){
+            Otto.sing(S_sleeping); //ZZzzzzz...
+            previousMillis=millis();
+        }
+
+        break;
+
+
+      //-- MODE 1 - Dance Mode!
+      //---------------------------------------------------------
+      case 1:
+
+        randomDance=random(5,21); //5,20
+        if((randomDance>14)&&(randomDance<19)){
+            randomSteps=1;
+            T=1600;
+        }
+        else{
+            randomSteps=random(3,6); //3,5
+            T=1000;
+        }
+
+        Otto.putMouth(random(10,21));
+
+        for (int i=0;i<randomSteps;i++){
+            move(randomDance);
+            if(buttonPushed){break;}
+        }
+        break;
+
+
+      //-- MODE 2 - Obstacle detector mode
+      //---------------------------------------------------------
+      case 2:
+
+        if(obstacleDetected){
+
+            if(!buttonPushed){
+              Otto.putMouth(bigSurprise);
+              Otto.sing(S_surprise);
+              Otto.jump(5, 500);
+            }
+
+            if(!buttonPushed){
+              Otto.putMouth(confused);
+              Otto.sing(S_cuddly);
+            }
+
+            //Otto takes two steps back
+            for(int i=0;i<3;i++){
+              if(buttonPushed){break;}
+              Otto.walk(1,1300,-1);
+            }
+
+            delay(100);
+            obstacleDetector();
+            delay(100);
+
+
+           //If there are no obstacles and no button is pressed, Otto shows a smile
+           if((obstacleDetected==true)||(buttonPushed==true)){break;}
+           else{
+              Otto.putMouth(smile);
+              delay(50);
+              obstacleDetector();
+           }
+
+
+           //If there are no obstacles and no button is pressed, Otto shows turns left
+           for(int i=0; i<3; i++){
+              if((obstacleDetected==true)||(buttonPushed==true)){break;}
+              else{
+                  Otto.turn(1,1000,1);
+                  obstacleDetector();
+              }
+           }
+
+            //If there are no obstacles and no button is pressed, Otto is happy
+            if((obstacleDetected==true)||(buttonPushed==true)){break;}
+            else{
+                Otto.home();
+                Otto.putMouth(happyOpen);
+                Otto.sing(S_happy_short);
+                delay(200);
+            }
+
+
+        }else{
+
+            Otto.walk(1,1000,1); //Otto walk straight
+            obstacleDetector();
+        }
+
+        break;
+
+
+      //-- MODE 3 - Noise detector mode
+      //---------------------------------------------------------
+      case 3:
+        if (Otto.getNoise()>=650){ //740
+
+          delay(50);  //Wait for the possible 'lag' of the button interruptions.
+                      //Sometimes, the noise sensor detect the button before the interruption takes efect
+
+          if(!buttonPushed){
+
+            Otto.putMouth(bigSurprise);
+            Otto.sing(S_OhOoh);
+
+            if(buttonPushed){break;}
+            Otto.putMouth(random(10,21));
+            randomDance=random(5,21);
+            move(randomDance);
+            Otto.home();
+            delay(500); //Wait for possible noise of the servos while get home
+          }
+
+          if(!buttonPushed){Otto.putMouth(happyOpen);}
+        }
+        break;
+
+
+      //-- MODE 4 - OttoPAD or any Teleoperation mode (listening SerialPort)
+      //---------------------------------------------------------
+      case 4:
+
+        SCmd.readSerial();
+
+        //If Otto is moving yet
+        if (Otto.getRestState()==false){
+          move(moveId);
+        }
+
+        break;
+
+
+      default:
+          MODE=4;
+          break;
+    }
+
+  }
+
+}
+
+
+
+///////////////////////////////////////////////////////////////////
+//-- Functions --------------------------------------------------//
+///////////////////////////////////////////////////////////////////
+
+//-- Function executed when second button is pushed
+void secondButtonPushed(){
+
+    buttonAPushed=true;
+
+    if(!buttonPushed){
+        buttonPushed=true;
+        Otto.putMouth(smallSurprise);
+    }
+}
+
+//-- Function executed when third button is pushed
+void thirdButtonPushed(){
+
+    buttonBPushed=true;
+
+    if(!buttonPushed){
+        buttonPushed=true;
+        Otto.putMouth(smallSurprise);
+    }
+}
+
+
+//-- Function to read distance sensor & to actualize obstacleDetected variable
+void obstacleDetector(){
+
+   int distance = Otto.getDistance();
+
+        if(distance<15){
+          obstacleDetected = true;
+        }else{
+          obstacleDetected = false;
+        }
+}
+
+
+//-- Function to receive Stop command.
+void receiveStop(){
+
+    sendAck();
+    Otto.home();
+    sendFinalAck();
+
+}
+
+
+//-- Function to receive LED commands
+void receiveLED(){
+
+    //sendAck & stop if necessary
+    sendAck();
+    Otto.home();
+
+    //Examples of receiveLED Bluetooth commands
+    //L 000000001000010100100011000000000
+    //L 001111111111111111111111111111111 (todos los LED encendidos)
+    unsigned long int matrix;
+    char *arg;
+    char *endstr;
+    arg=SCmd.next();
+    //Serial.println (arg);
+    if (arg != NULL) {
+      matrix=strtoul(arg,&endstr,2);    // Converts a char string to unsigned long integer
+      Otto.putMouth(matrix,false);
+    }else{
+      Otto.putMouth(xMouth);
+      delay(2000);
+      Otto.clearMouth();
+    }
+
+    sendFinalAck();
+
+}
+
+
+//-- Function to receive buzzer commands
+void recieveBuzzer(){
+
+    //sendAck & stop if necessary
+    sendAck();
+    Otto.home();
+
+    bool error = false;
+    int frec;
+    int duration;
+    char *arg;
+
+    arg = SCmd.next();
+    if (arg != NULL) { frec=atoi(arg); }    // Converts a char string to an integer
+    else {error=true;}
+
+    arg = SCmd.next();
+    if (arg != NULL) { duration=atoi(arg); } // Converts a char string to an integer
+    else {error=true;}
+
+    if(error==true){
+
+      Otto.putMouth(xMouth);
+      delay(2000);
+      Otto.clearMouth();
+
+    }else{
+
+      Otto._tone(frec, duration, 1);
+    }
+
+    sendFinalAck();
+
+}
+
+
+//-- Function to receive TRims commands
+void receiveTrims(){
+
+    //sendAck & stop if necessary
+    sendAck();
+    Otto.home();
+
+    int trim_YL,trim_YR,trim_RL,trim_RR;
+
+    //Definition of Servo Bluetooth command
+    //C trim_YL trim_YR trim_RL trim_RR
+    //Examples of receiveTrims Bluetooth commands
+    //C 20 0 -8 3
+    bool error = false;
+    char *arg;
+    arg=SCmd.next();
+    if (arg != NULL) { trim_YL=atoi(arg); }    // Converts a char string to an integer
+    else {error=true;}
+
+    arg = SCmd.next();
+    if (arg != NULL) { trim_YR=atoi(arg); }    // Converts a char string to an integer
+    else {error=true;}
+
+    arg = SCmd.next();
+    if (arg != NULL) { trim_RL=atoi(arg); }    // Converts a char string to an integer
+    else {error=true;}
+
+    arg = SCmd.next();
+    if (arg != NULL) { trim_RR=atoi(arg); }    // Converts a char string to an integer
+    else {error=true;}
+
+    if(error==true){
+
+      Otto.putMouth(xMouth);
+      delay(2000);
+      Otto.clearMouth();
+
+    }else{ //Save it on EEPROM
+      Otto.setTrims(trim_YL, trim_YR, trim_RL, trim_RR);
+      Otto.saveTrimsOnEEPROM(); //Uncomment this only for one upload when you finaly set the trims.
+    }
+
+    sendFinalAck();
+
+}
+
+
+//-- Function to receive Servo commands
+void receiveServo(){
+
+    sendAck();
+    moveId = 30;
+
+    //Definition of Servo Bluetooth command
+    //G  servo_YL servo_YR servo_RL servo_RR
+    //Example of receiveServo Bluetooth commands
+    //G 90 85 96 78
+    bool error = false;
+    char *arg;
+    int servo_YL,servo_YR,servo_RL,servo_RR;
+
+    arg=SCmd.next();
+    if (arg != NULL) { servo_YL=atoi(arg); }    // Converts a char string to an integer
+    else {error=true;}
+
+    arg = SCmd.next();
+    if (arg != NULL) { servo_YR=atoi(arg); }    // Converts a char string to an integer
+    else {error=true;}
+
+    arg = SCmd.next();
+    if (arg != NULL) { servo_RL=atoi(arg); }    // Converts a char string to an integer
+    else {error=true;}
+
+    arg = SCmd.next();
+    if (arg != NULL) { servo_RR=atoi(arg); }    // Converts a char string to an integer
+    else {error=true;}
+
+    if(error==true){
+
+      Otto.putMouth(xMouth);
+      delay(2000);
+      Otto.clearMouth();
+
+    }else{ //Update Servo:
+
+      int servoPos[4]={servo_YL, servo_YR, servo_RL, servo_RR};
+      Otto._moveServos(200, servoPos);   //Move 200ms
+
+    }
+
+    sendFinalAck();
+
+}
+
+
+//-- Function to receive movement commands
+void receiveMovement(){
+
+    sendAck();
+
+    if (Otto.getRestState()==true){
+        Otto.setRestState(false);
+    }
+
+    //Definition of Movement Bluetooth commands
+    //M  MoveID  T   MoveSize
+    char *arg;
+    arg = SCmd.next();
+    if (arg != NULL) {moveId=atoi(arg);}
+    else{
+      Otto.putMouth(xMouth);
+      delay(2000);
+      Otto.clearMouth();
+      moveId=0; //stop
+    }
+
+    arg = SCmd.next();
+    if (arg != NULL) {T=atoi(arg);}
+    else{
+      T=1000;
+    }
+
+    arg = SCmd.next();
+    if (arg != NULL) {moveSize=atoi(arg);}
+    else{
+      moveSize =15;
+    }
+}
+
+
+//-- Function to execute the right movement according the movement command received.
+void move(int moveId){
+
+  bool manualMode = false;
+
+  switch (moveId) {
+    case 0:
+      Otto.home();
+      break;
+    case 1: //M 1 1000
+      Otto.walk(1,T,1);
+      break;
+    case 2: //M 2 1000
+      Otto.walk(1,T,-1);
+      break;
+    case 3: //M 3 1000
+      Otto.turn(1,T,1);
+      break;
+    case 4: //M 4 1000
+      Otto.turn(1,T,-1);
+      break;
+    case 5: //M 5 1000 30
+      Otto.updown(1,T,moveSize);
+      break;
+    case 6: //M 6 1000 30
+      Otto.moonwalker(1,T,moveSize,1);
+      break;
+    case 7: //M 7 1000 30
+      Otto.moonwalker(1,T,moveSize,-1);
+      break;
+    case 8: //M 8 1000 30
+      Otto.swing(1,T,moveSize);
+      break;
+    case 9: //M 9 1000 30
+      Otto.crusaito(1,T,moveSize,1);
+      break;
+    case 10: //M 10 1000 30
+      Otto.crusaito(1,T,moveSize,-1);
+      break;
+    case 11: //M 11 1000
+      Otto.jump(1,T);
+      break;
+    case 12: //M 12 1000 30
+      Otto.flapping(1,T,moveSize,1);
+      break;
+    case 13: //M 13 1000 30
+      Otto.flapping(1,T,moveSize,-1);
+      break;
+    case 14: //M 14 1000 20
+      Otto.tiptoeSwing(1,T,moveSize);
+      break;
+    case 15: //M 15 500
+      Otto.bend(1,T,1);
+      break;
+    case 16: //M 16 500
+      Otto.bend(1,T,-1);
+      break;
+    case 17: //M 17 500
+      Otto.shakeLeg(1,T,1);
+      break;
+    case 18: //M 18 500
+      Otto.shakeLeg(1,T,-1);
+      break;
+    case 19: //M 19 500 20
+      Otto.jitter(1,T,moveSize);
+      break;
+    case 20: //M 20 500 15
+      Otto.ascendingTurn(1,T,moveSize);
+      break;
+    default:
+        manualMode = true;
+      break;
+  }
+
+  if(!manualMode){
+    sendFinalAck();
+  }
+
+}
+
+//-- Function to receive gesture commands
+void receiveGesture(){
+
+    //sendAck & stop if necessary
+    sendAck();
+    Otto.home();
+
+    //Definition of Gesture Bluetooth commands
+    //H  GestureID
+    int gesture = 0;
+    char *arg;
+    arg = SCmd.next();
+    if (arg != NULL) {gesture=atoi(arg);}
+    else
+    {
+      Otto.putMouth(xMouth);
+      delay(2000);
+      Otto.clearMouth();
+    }
+
+    switch (gesture) {
+      case 1: //H 1
+        Otto.playGesture(OttoHappy);
+        break;
+      case 2: //H 2
+        Otto.playGesture(OttoSuperHappy);
+        break;
+      case 3: //H 3
+        Otto.playGesture(OttoSad);
+        break;
+      case 4: //H 4
+        Otto.playGesture(OttoSleeping);
+        break;
+      case 5: //H 5
+        Otto.playGesture(OttoFart);
+        break;
+      case 6: //H 6
+        Otto.playGesture(OttoConfused);
+        break;
+      case 7: //H 7
+        Otto.playGesture(OttoLove);
+        break;
+      case 8: //H 8
+        Otto.playGesture(OttoAngry);
+        break;
+      case 9: //H 9
+        Otto.playGesture(OttoFretful);
+        break;
+      case 10: //H 10
+        Otto.playGesture(OttoMagic);
+        break;
+      case 11: //H 11
+        Otto.playGesture(OttoWave);
+        break;
+      case 12: //H 12
+        Otto.playGesture(OttoVictory);
+        break;
+      case 13: //H 13
+        Otto.playGesture(OttoFail);
+        break;
+      default:
+        break;
+    }
+
+    sendFinalAck();
+}
+
+//-- Function to receive sing commands
+void receiveSing(){
+
+    //sendAck & stop if necessary
+    sendAck();
+    Otto.home();
+
+    //Definition of Sing Bluetooth commands
+    //K  SingID
+    int sing = 0;
+    char *arg;
+    arg = SCmd.next();
+    if (arg != NULL) {sing=atoi(arg);}
+    else
+    {
+      Otto.putMouth(xMouth);
+      delay(2000);
+      Otto.clearMouth();
+    }
+
+    switch (sing) {
+      case 1: //K 1
+        Otto.sing(S_connection);
+        break;
+      case 2: //K 2
+        Otto.sing(S_disconnection);
+        break;
+      case 3: //K 3
+        Otto.sing(S_surprise);
+        break;
+      case 4: //K 4
+        Otto.sing(S_OhOoh);
+        break;
+      case 5: //K 5
+        Otto.sing(S_OhOoh2);
+        break;
+      case 6: //K 6
+        Otto.sing(S_cuddly);
+        break;
+      case 7: //K 7
+        Otto.sing(S_sleeping);
+        break;
+      case 8: //K 8
+        Otto.sing(S_happy);
+        break;
+      case 9: //K 9
+        Otto.sing(S_superHappy);
+        break;
+      case 10: //K 10
+        Otto.sing(S_happy_short);
+        break;
+      case 11: //K 11
+        Otto.sing(S_sad);
+        break;
+      case 12: //K 12
+        Otto.sing(S_confused);
+        break;
+      case 13: //K 13
+        Otto.sing(S_fart1);
+        break;
+      case 14: //K 14
+        Otto.sing(S_fart2);
+        break;
+      case 15: //K 15
+        Otto.sing(S_fart3);
+        break;
+      case 16: //K 16
+        Otto.sing(S_mode1);
+        break;
+      case 17: //K 17
+        Otto.sing(S_mode2);
+        break;
+      case 18: //K 18
+        Otto.sing(S_mode3);
+        break;
+      case 19: //K 19
+        Otto.sing(S_buttonPushed);
+        break;
+      default:
+        break;
+    }
+
+    sendFinalAck();
+}
+
+
+//-- Function to receive Name command
+void receiveName(){
+
+    //sendAck & stop if necessary
+    sendAck();
+    Otto.home();
+
+    char newOttoName[11] = "";  //Variable to store data read from Serial.
+    int eeAddress = 5;          //Location we want the data to be in EEPROM.
+    char *arg;
+    arg = SCmd.next();
+
+    if (arg != NULL) {
+
+      //Complete newOttoName char string
+      int k = 0;
+      while((*arg) && (k<11)){
+          newOttoName[k]=*arg++;
+          k++;
+      }
+
+      EEPROM.put(eeAddress, newOttoName);
+    }
+    else
+    {
+      Otto.putMouth(xMouth);
+      delay(2000);
+      Otto.clearMouth();
+    }
+
+    sendFinalAck();
+
+}
+
+
+//-- Function to send Otto's name
+void requestName(){
+
+    Otto.home(); //stop if necessary
+
+    char actualOttoName[11]= "";  //Variable to store data read from EEPROM.
+    int eeAddress = 5;            //EEPROM address to start reading from
+
+    //Get the float data from the EEPROM at position 'eeAddress'
+    EEPROM.get(eeAddress, actualOttoName);
+
+    Serial.print(F("&&"));
+    Serial.print(F("E "));
+    Serial.print(actualOttoName);
+    Serial.println(F("%%"));
+    Serial.flush();
+}
+
+
+//-- Function to send ultrasonic sensor measure (distance in "cm")
+void requestDistance(){
+
+    Otto.home();  //stop if necessary
+
+    int distance = Otto.getDistance();
+    Serial.print(F("&&"));
+    Serial.print(F("D "));
+    Serial.print(distance);
+    Serial.println(F("%%"));
+    Serial.flush();
+}
+
+
+//-- Function to send noise sensor measure
+void requestNoise(){
+
+    Otto.home();  //stop if necessary
+
+    int microphone= Otto.getNoise(); //analogRead(PIN_NoiseSensor);
+    Serial.print(F("&&"));
+    Serial.print(F("N "));
+    Serial.print(microphone);
+    Serial.println(F("%%"));
+    Serial.flush();
+}
+
+
+//-- Function to send battery voltage percent
+void requestBattery(){
+
+    Otto.home();  //stop if necessary
+
+    //The first read of the batery is often a wrong reading, so we will discard this value.
+    double batteryLevel = Otto.getBatteryLevel();
+
+    Serial.print(F("&&"));
+    Serial.print(F("B "));
+    Serial.print(batteryLevel);
+    Serial.println(F("%%"));
+    Serial.flush();
+}
+
+
+//-- Function to send program ID
+void requestProgramId(){
+
+    Otto.home();   //stop if necessary
+
+    Serial.print(F("&&"));
+    Serial.print(F("I "));
+    Serial.print(programID);
+    Serial.println(F("%%"));
+    Serial.flush();
+}
+
+
+//-- Function to send Ack comand (A)
+void sendAck(){
+
+  delay(30);
+
+  Serial.print(F("&&"));
+  Serial.print(F("A"));
+  Serial.println(F("%%"));
+  Serial.flush();
+}
+
+
+//-- Function to send final Ack comand (F)
+void sendFinalAck(){
+
+  delay(30);
+
+  Serial.print(F("&&"));
+  Serial.print(F("F"));
+  Serial.println(F("%%"));
+  Serial.flush();
+}


Convert the code to the current schematic, use of hardware interrupts

--- Pingduino/Pingduino.ino
+++ Pingduino/Pingduino.ino
@@ -1,68 +1,15 @@
-#define P1PIN           8
-#define P1LIGHTPIN     12
-#define P2PIN           9
-#define P2LIGHTPIN     13
-#define SPEAKERPIN      6
+#define P1PIN           0 // Interrupts
+#define P2PIN           1
+#define BUTTONLIGHTPIN  4
+#define SPEAKERPIN      9
 #define LONGPRESS_TIME 25
-#define LATCH 4
-#define CLK   3
-#define DATA  2
+#define LATCH           7
+#define CLK             6
+#define DATA            5
 #define INACTIVEMILLIS 1200000 // 20 minutes

 // Seven Segment Display Handling
 byte DigitBytes[10]= {0x3F, 0x06, 0x5B, 0x4F, 0x66, 0x6D, 0x7D, 0x07, 0x7F, 0x67};
-
-// Button handling
-enum { BUTTON_NONE=0, BUTTON_SHORT, BUTTON_LONG};
-
-class Button {
-  public:
-    Button(int pin, int longpress_time=LONGPRESS_TIME);
-    void init();
-    int handle();
-
-  protected:
-    boolean pressed;
-    int counter;
-    const int pin;
-    const int longpress_time;
-};
-
-Button::Button(int p, int t) : pin(p), longpress_time(t)
-{
-}
-
-void Button::init()
-{
-  pinMode(pin, INPUT);
-  digitalWrite(pin, HIGH);
-  pressed = false;
-  counter = 0;
-}
-
-int Button::handle()
-{
-  int event;
-  int now_pressed = !digitalRead(pin);
-
-  if (!now_pressed && pressed) {
-    if (counter < longpress_time)
-      event = BUTTON_SHORT;
-    else
-      event = BUTTON_LONG;
-  } else {
-    event = BUTTON_NONE;
-  }
-
-  if (now_pressed) {
-    ++counter;
-  } else {
-    counter = 0;
-  }
-
-  pressed = now_pressed;
-  return event;
-}

 // Victory Song
 class VictorySong {
@@ -123,55 +70,131 @@
 }

 // Game Logics
-int p1Score = 0;
-int p2Score = 0;
-int currentServe = 0; // p1 = 0, p2 = 1
-int numServes = 0;
+volatile int p1Score = 0;
+volatile int p2Score = 0;
+volatile int currentServe = 0; // p1 = 0, p2 = 1
+volatile int requestReset = 0;
+volatile int numServes = 0;
+volatile unsigned long lastActivityTime;
+
 int gameOver = 0;
 int victoryBlinkCounter = 0;
-
-unsigned long lastActivityTime;

 boolean p1Win = false;
 boolean p2Win = false;
 boolean blinkToggle = true;
 boolean sleeping = false;

-Button p1Button(P1PIN);
-Button p2Button(P2PIN);
 VictorySong victorySong(SPEAKERPIN);

 void setup()
 {
+  // Interrupts for scoring buttons
+  attachInterrupt(P1PIN, p1ButtonPressed, CHANGE);
+  attachInterrupt(P2PIN, p2ButtonPressed, CHANGE);
+
+  // Shift Registers for score display
   pinMode(LATCH, OUTPUT);
   pinMode(CLK, OUTPUT);
   pinMode(DATA, OUTPUT);

-  pinMode(P1LIGHTPIN, OUTPUT);
-  pinMode(P2LIGHTPIN, OUTPUT);
-
-  digitalWrite(P1LIGHTPIN, HIGH);
-  digitalWrite(P2LIGHTPIN, HIGH);
+  // LEDs inside the scoring buttons
+  pinMode(BUTTONLIGHTPIN, OUTPUT);
+  digitalWrite(BUTTONLIGHTPIN, HIGH);

   showScore();
-
-  p1Button.init();
-  p2Button.init();
+
   victorySong.init();
   lastActivityTime = millis();
 }

+// Intterupt handlers for scoring
+volatile int trackP1Button = 0;
+volatile unsigned long p1ButtonChangeMillis = 0;
+void p1ButtonPressed()
+{
+  lastActivityTime = millis();
+  if (sleeping) {
+    return;
+  }
+
+  if (trackP1Button == 0) {
+    p1ButtonChangeMillis = millis();
+    trackP1Button = 1;
+    return;
+  }
+
+  if (trackP1Button == 1) {
+    trackP1Button = 0;
+    unsigned long now = millis();
+    if ((now - p1ButtonChangeMillis) <= 500) {
+      // Short press, score
+      if (gameOver)
+        return;
+      p1Score++;
+      numServes++;
+    } else {
+      // Long press, reset
+      currentServe = 0;
+      requestReset = 1;
+    }
+  }
+}
+
+volatile int trackP2Button = 0;
+volatile unsigned long p2ButtonChangeMillis = 0;
+void p2ButtonPressed()
+{
+  lastActivityTime = millis();
+  if (sleeping) {
+    return;
+  }
+
+  if (trackP2Button == 0) {
+    p2ButtonChangeMillis = millis();
+    trackP2Button = 1;
+    return;
+  }
+
+  if (trackP2Button == 1) {
+    trackP2Button = 0;
+    unsigned long now = millis();
+    if ((now - p2ButtonChangeMillis) <= 500) {
+      // Short press, score
+      if (gameOver)
+        return;
+      p2Score++;
+      numServes++;
+    } else {
+      // Long press, reset
+      currentServe = 0;
+      requestReset = 1;
+    }
+  }
+}

 void loop()
 {
-  int p1Event, p2Event;
-
+  // The loop will:
+
+  // Check whether it should go to sleep or wake up
   if ((millis() - lastActivityTime) > INACTIVEMILLIS) {
     goToSleep();
   } else {
     wakeup();
   }
-
+
+  if (sleeping)
+    return;
+
+  // Show the score
+  showScore();
+
+  // Check for a win state
+  if (!gameOver)
+    checkForWinner();
+
+  // Handle the victory state
   if (gameOver) {
     victoryBlinkCounter++;

@@ -185,53 +208,12 @@
       blinkToggle = !blinkToggle;
     }
   }
-
-  p1Event = p1Button.handle();
-  if (!gameOver && p1Event == BUTTON_SHORT) {
-    lastActivityTime = millis();
-    if (sleeping)
-      return;
-
-    p1Score++;
-    numServes++;
-    showScore();
-    checkForWinner();
-  }
-
-  if (gameOver && p1Event == BUTTON_LONG) {
-    lastActivityTime = millis();
-    if (sleeping)
-      return;
-
+
+  // Reset the game when requested
+  if (gameOver && requestReset) {
     resetGame();
-    currentServe = 0;
-    numServes = 0;
-    showScore();
-  }
-
-  p2Event = p2Button.handle();
-  if (!gameOver && p2Event == BUTTON_SHORT) {
-    lastActivityTime = millis();
-    if (sleeping)
-      return;
-
-    p2Score++;
-    numServes++;
-    showScore();
-    checkForWinner();
-  }
-
-  if (gameOver && p2Event == BUTTON_LONG) {
-    lastActivityTime = millis();
-    if (sleeping)
-      return;
-
-    resetGame();
-    currentServe = 1;
-    numServes = 0;
-    showScore();
-  }
-
+  }
+
   delay(20);
 }

@@ -280,18 +262,19 @@
   // Blank p2 score

   // Turn off button lights
-  digitalWrite(P1LIGHTPIN, LOW);
-  digitalWrite(P2LIGHTPIN, LOW);
+  digitalWrite(BUTTONLIGHTPIN, LOW);
 }

  void wakeup()
  {
+   if (!sleeping)
+     return;
+
    sleeping = false;

    showScore();

-   digitalWrite(P1LIGHTPIN, HIGH);
-   digitalWrite(P2LIGHTPIN, HIGH);
+   digitalWrite(BUTTONLIGHTPIN, HIGH);
  }

@@ -316,7 +299,8 @@
   p1Win = false;
   p2Win = false;
   gameOver = 0;
-}
-
-
-
+  numServes = 0;
+}
+
+
+


fix pitch output to conform to 1V/oct fro 0 to 5V

--- Sequencer/Sequencer.ino
+++ Sequencer/Sequencer.ino
@@ -191,9 +191,8 @@
   stepTime = 60000.f / bpm;
   numSteps = (analogRead(NSTEPSPIN) / 171) + 1; // from 1 to 6 steps
   for(int i = 0; i < 8; i++){
-    pitch[i] = analogRead(i) >> 3; // from 0 to 127
-  }
-
+    pitch[i] = analogRead(i) >> 2; // from 0 to 255 (0 to 5V at a V/oct would be 5 octaves)
+  }

   // Encoder
   button = digitalRead(ENCB);


fixed center visualizer

fixed center visualizer

--- arducode/arducode.ino
+++ arducode/arducode.ino
@@ -68,25 +68,25 @@
 int numpix=0;
 int integerValue=0;
 char incomingByte;
-void center_vis(int integerValue, Adafruit_NeoPixel strip, int** ciroc) {
-    for(int i=(strip.numPixels())/2;
-                  i>=((strip.numPixels())/2)-(integerValue/2) && i>=0; i--){ //select lights from center towards pos 0
+void center_vis(int integerValue, Adafruit_NeoPixel strip, int** ciroc) {
+  int center_val = 36;
+  int move = integerValue/2;
+    for(int i=center_val; i >= center_val-move && i>0; i--){ //select lights from center towards pos 0
       strip.setPixelColor(i,ciroc[i][0],ciroc[i][1],ciroc[i][2]);
     }
-    for(int i=0;i<((strip.numPixels())/2)-(integerValue/2);i++){ //set remaining lights from left end to black
+    for(int i=0; i<center_val-move; i++){ //set remaining lights from left end to black
       strip.setPixelColor(i,0,0,0);
     }
-    for(int i=((strip.numPixels())/2)+1;
-            i<=((strip.numPixels())/2)+(integerValue/2) && i<strip.numPixels(); i++){ //select lights from center towards numPixels()
-      strip.setPixelColor(i,ciroc[i][0],ciroc[i][1],ciroc[i][2]);
-    }
-    for(int i=strip.numPixels();i>((strip.numPixels())/2)+(integerValue/2);i--){ //set lights from numPixels() end to black
-      strip.setPixelColor(i,0,0,0);
-    }
-    strip.show();
-}
-
-int gabe = 1;
+    // for(int i=center_val; i<= (center_val+move) && i<strip.numPixels(); i++){ //select lights from center towards numPixels()
+    //   strip.setPixelColor(i,ciroc[i][0],ciroc[i][1],ciroc[i][2]);
+    // }
+    // for(int i=center_val + move; i<strip.numPixels(); i++){ //set lights up to numPixels() to black
+    //   strip.setPixelColor(i,0,0,0);
+    // }
+    strip.show();
+}
+
+int gabe = 0;
 void loop() {
 //<<<<<<< HEAD
   if(Serial.available()){
@@ -109,10 +109,24 @@
       }
       strip.show();
     }
-    else
-      {center_vis(integerValue, strip, ciroc);}
-  }
-
+    else{
+        int center_val = 36;
+        int move = integerValue/2;
+          for(int i=center_val; i >= center_val-move && i>0; i--){ //select lights from center towards pos 0
+            strip.setPixelColor(i,ciroc[i][0],ciroc[i][1],ciroc[i][2]);
+          }
+          for(int i=0; i<center_val-move; i++){ //set remaining lights from left end to black
+            strip.setPixelColor(i,0,0,0);
+          }
+          for(int i=center_val; i<= (center_val+move) && i<strip.numPixels(); i++){ //select lights from center towards numPixels()
+            strip.setPixelColor(i,ciroc[i][0],ciroc[i][1],ciroc[i][2]);
+          }
+          for(int i=center_val + move; i<strip.numPixels(); i++){ //set lights up to numPixels() to black
+            strip.setPixelColor(i,0,0,0);
+          }
+          strip.show();
+        }
+  }
 }

 // Fill the dots one after the other with a color


Changed filler text

Changed filler text

--- Masterfile_Rev_1/Masterfile_Rev_1.ino
+++ Masterfile_Rev_1/Masterfile_Rev_1.ino
@@ -8,7 +8,7 @@
 #define DHTPIN 6
 #define DHTTYPE DHT11
 #define p0 1013.25 // Sea level pressure in mb
-#define fill " | "
+#define fill "|"

 #define delayBTChecks 30000 // delayBetweenTimeChecks in milliseconds (less than 60,000)
 // Note: there will be error that comes up that cycles through a range of delayBTChecks


correction of the ROS example

correction of the ROS example

--- example/SixLasersROS/SixLasersROS.ino
+++ example/SixLasersROS/SixLasersROS.ino
@@ -65,12 +65,12 @@

 void beginLidars() {
   // Initialisation of the lidars objects
-  LZ1.begin(Z1_LASER_EN, Z1_LASER_PIN, Z1_LASER_TRIG, Z1_LASER_AD, 2, 'x');
-  LZ2.begin(Z2_LASER_EN, Z2_LASER_PIN, Z2_LASER_TRIG, Z2_LASER_AD, 2, 'X');
-  LZ3.begin(Z3_LASER_EN, Z3_LASER_PIN, Z3_LASER_TRIG, Z3_LASER_AD, 2, 'y');
-  LZ4.begin(Z4_LASER_EN, Z4_LASER_PIN, Z4_LASER_TRIG, Z4_LASER_AD, 2, 'Z');
-  LZ5.begin(Z5_LASER_EN, Z5_LASER_PIN, Z5_LASER_TRIG, Z5_LASER_AD, 2, 'y');
-  LZ6.begin(Z6_LASER_EN, Z6_LASER_PIN, Z6_LASER_TRIG, Z6_LASER_AD, 2, 'Z');
+  LZ1.begin(Z1_LASER_EN, Z1_LASER_PIN, Z1_LASER_TRIG, Z1_LASER_AD, 2, DISTANCE,'x');
+  LZ2.begin(Z2_LASER_EN, Z2_LASER_PIN, Z2_LASER_TRIG, Z2_LASER_AD, 2, DISTANCE,'X');
+  LZ3.begin(Z3_LASER_EN, Z3_LASER_PIN, Z3_LASER_TRIG, Z3_LASER_AD, 2, DISTANCE,'y');
+  LZ4.begin(Z4_LASER_EN, Z4_LASER_PIN, Z4_LASER_TRIG, Z4_LASER_AD, 2, DISTANCE,'Z');
+  LZ5.begin(Z5_LASER_EN, Z5_LASER_PIN, Z5_LASER_TRIG, Z5_LASER_AD, 2, DISTANCE,'y');
+  LZ6.begin(Z6_LASER_EN, Z6_LASER_PIN, Z6_LASER_TRIG, Z6_LASER_AD, 2, DISTANCE,'Z');

   // Initialisation of the controller
   Controller.begin(WIRE400K);
@@ -103,16 +103,6 @@
   }
 }

-void laserprint(){
-  for(byte i = 0; i < 6; i++){
-    Serial.print(i);
-    Serial.print(" - ");
-    Serial.print(Controller.lidars[i]->distance);
-    Serial.print(" - ");
-    Serial.println(Controller.lidars[i]->strength);
-  }
-}
-
 void laserpublish(){
   distance_msg.lasers_length = 6;
   distance_msg.status_length = 6;
@@ -123,4 +113,4 @@
   }

   distance_publisher.publish(&distance_msg);
-}+}


Changed text of KeyboardMessage to make it easier for international keyboards.

--- build/shared/examples/9.Keyboard/KeyboardMessage/KeyboardMessage.ino
+++ build/shared/examples/9.Keyboard/KeyboardMessage/KeyboardMessage.ino
@@ -8,6 +8,7 @@
  * 10-kilohm resistor attached from pin 4 to ground

  created 24 Oct 2011
+ modified 19 Mar 2012
  by Tom Igoe

  This example code is in the public domain.
@@ -34,7 +35,7 @@
       // increment the button counter
     counter++;
     // type out a message
-    Keyboard.print("You pressed the button: ");
+    Keyboard.print("You pressed the button ");
     Keyboard.print(counter);
     Keyboard.println(" times.");
   }


Auto Detect ATtiny85

Auto Detect ATtiny85

--- Temperature_Node/Temperature_Node.ino
+++ Temperature_Node/Temperature_Node.ino
@@ -23,9 +23,7 @@
 #include <avr/interrupt.h>
 #include <avr/sleep.h>

-#define  ATtiny85 // ATtiny85 mode
-
-#ifdef ATtiny85
+#if defined(__AVR_ATtiny85__) // ATtiny85 mode
 #define ECO // Low Power Mode

 #define txLed 3  // TX LED


debug: remove println

debug: remove println

--- src/src.ino
+++ src/src.ino
@@ -213,12 +213,6 @@
       (double) cos(ticks * (2 * PI / 96)),
       (double) sin(ticks * (2 * PI / 96))
     );
-    Serial.print(current_loc.lat, 6);
-    Serial.print(", ");
-    Serial.println(current_loc.lng, 6);
-    Serial.println(current_dist);
-    Serial.println(current_bearing);
-    Serial.println();
     draw();
   }
 }


Delete base2clock3.ino

Delete base2clock3.ino

--- base2clock3.ino
+++ base2clock3.ino
@@ -1,166 +0,0 @@
-/***************************
-*
-* base 2 clock code for arduino
-* Copyleft (C) 2015 Daniel K. Spicer
-*
-* This program is free software: you can redistribute it and/or modify
-* it under the terms of the GNU General Public License as published by
-* the Free Software Foundation, either version 3 of the License, or
-* (at your option) any later version.
-*
-* This program is distributed in the hope that it will be useful,
-* but WITHOUT ANY WARRANTY; without even the implied warranty of
-* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
-* GNU General Public License for more details.
-*
-* You should have received a copy of the GNU General Public License
-* along with this program. If not, see <http://www.gnu.org/licenses/>.
-*
-****************************/
-// adjust this value to tune the clock. 5000 should be close, lower if clock runs slow, higher if fast.
-#define INTERVAL_TIME 5000
-//int's representing led's. 1 = on, 0 = off. Initially they will all be off.
-int p1,p2,p3,p4,p5,p6,p7,p8,p9 = 0;
-int iterations = 0;
-boolean increment, reset_fast, reset_slow, time_set, setting_time = false;
-void setup(){
-//8 led's as outputs.
-pinMode(13, OUTPUT);
-pinMode(12, OUTPUT);
-pinMode(11, OUTPUT);
-pinMode(10, OUTPUT);
-pinMode(9, OUTPUT);
-pinMode(8, OUTPUT);
-pinMode(7, OUTPUT);
-pinMode(6, OUTPUT);
-//2 time set buttons as inputs.
-pinMode(5, INPUT);
-digitalWrite(5, HIGH);
-pinMode(4, INPUT);
-digitalWrite(4, HIGH);
-} // end setup()
-void loop(){
-//reinitialize time setting booleans to false.
-reset_fast = false;
-reset_slow = false;
-setting_time = false;
-//see if the time is being set.
-if(digitalRead(5) == LOW){
-reset_slow = true;
-resetTimeSetFlags();
-} else if(digitalRead(4) == LOW){
-reset_fast = true;
-resetTimeSetFlags();
-}
-if(!time_set){ // time has never been set so just blink the lights.
-if(p8 == 1)
-p8 = p7 = p6 = p5 = p4 = p3 = p2 = p1 = 0;
-else
-p8 = p7 = p6 = p5 = p4 = p3 = p2 = p1 = 1;
-lightLights();
-} else { //time has been set, so increment as instructed.
-if(!increment){
-iterations++;
-if(iterations == 12){
-iterations = 0;
-increment = true;
-}
-}
-if(increment){
-increment = false;
-if(setting_time || !setIncrementFlag()){
-doIncrement();
-lightLights();
-}
-}
-} //end time_set is true
-// figure out the loop delay, typically 5 seconds, 1 second if the time has never been set
-// and we are blinking the lights, 0.1 second if we are coarse setting the time and 1 second
-// if we are fine setting the time.
-if(reset_fast){
-delay(100);
-iterations = 0;
-} else if(reset_slow){
-delay(1000);
-iterations = 0;
-} else if(!time_set) {
-delay(1000);
-} else delay(INTERVAL_TIME);
-} // end loop()
-void resetTimeSetFlags(){
-setting_time = true;
-increment = true;
-time_set = true;
-iterations = 0;
-p9 = 0;
-}
-boolean setIncrementFlag(){
-if(p9 == 0 && (p8 == 0 || p7 == 0 || p6 == 0)){
-p9 = 1;
-return true;
-}
-return false;
-}
-void doIncrement(){
-p9 = 0;
-if(p8 == 0){
-p8 = 1;
-} else if (p7 == 0){
-p7 = 1;
-p8 = 0;
-} else if (p6 == 0){
-p6 = 1;
-p8 = p7 = 0;
-} else if (p5 == 0){
-p5 = 1;
-p8 = p7 = p6 = 0;
-} else if (p4 == 0){
-p4 = 1;
-p8 = p7 = p6 = p5 = 0;
-} else if (p3 == 0){
-p3 = 1;
-p8 = p7 = p6 = p5 = p4 = 0;
-} else if (p2 == 0){
-p2 = 1;
-p8 = p7 = p6 = p5 = p4 = p3 = 0;
-} else if (p1 == 0){
-p1 = 1;
-p8 = p7 = p6 = p5 = p4 = p3 = p2 = 0;
-} else { // all lights were lit, so this increment resets them to all off
-p8 = p7 = p6 = p5 = p4 = p3 = p2 = p1 = 0;
-}
-} // end doIncrement()
-void lightLights() {
-if(p1 == 1)
-digitalWrite(13, HIGH);
-else
-digitalWrite(13, LOW);
-if(p2 == 1)
-digitalWrite(12, HIGH);
-else
-digitalWrite(12, LOW);
-if(p3 == 1)
-digitalWrite(11, HIGH);
-else
-digitalWrite(11, LOW);
-if(p4 == 1)
-digitalWrite(10, HIGH);
-else
-digitalWrite(10, LOW);
-if(p5 == 1)
-digitalWrite(9, HIGH);
-else
-digitalWrite(9, LOW);
-if(p6 == 1)
-digitalWrite(8, HIGH);
-else
-digitalWrite(8, LOW);
-if(p7 == 1)
-digitalWrite(7, HIGH);
-else
-digitalWrite(7, LOW);
-if(p8 == 1)
-digitalWrite(6, HIGH);
-else
-digitalWrite(6, LOW);
-} // end lightLights()


Update ESP8266_DirectConnect.ino

Update ESP8266_DirectConnect.ino

--- examples/BoardsAndShields/ESP8266_DirectConnect/ESP8266_DirectConnect.ino
+++ examples/BoardsAndShields/ESP8266_DirectConnect/ESP8266_DirectConnect.ino
@@ -17,7 +17,9 @@
  * without using intermediate server.
  *
  * Please note: this example is for experienced users.
- *
+ * Use this wiring for improved stability:
+ *   https://github.com/esp8266/Arduino/blob/esp8266/docs/ESP_improved_stability.png
+ *
  * !!!
  * NOTE: Blynk Apps currently do not support direct TCP connection.
  * Wait for new updates!
@@ -31,7 +33,7 @@
 #include <BlynkSimpleUserDefined.h>

 // Uncomment this to set WiFi to Access Point mode
-//#define WIFI_MODE_AP
+#define WIFI_MODE_AP

 // Set these to your credentials.
 const char ssid[]     = "Blynk-AP";    // WiFi name
@@ -46,7 +48,8 @@
 // Description of the dashboard
 char profile[] = R"raw({"dashBoards":[
   {"id":1,"name":"Direct connect","boardType":"ESP8266","widgets":[
-    {"id":2,"type":"DIGIT4_DISPLAY","pinType":"VIRTUAL","pin":9,"x":5,"y":1,"frequency":1000}
+    {"id":2,"type":"DIGIT4_DISPLAY","pinType":"VIRTUAL","pin":9,"x":5,"y":1,"frequency":1000},
+    {"id":3,"type":"BUTTON","pinType":"VIRTUAL","pin":1,"x":2,"y":1}
   ]}
 ]})raw";

@@ -54,6 +57,10 @@

 BLYNK_READ(V9) {
   Blynk.virtualWrite(9, millis() / 1000);
+}
+
+BLYNK_WRITE(V1) {
+  BLYNK_LOG("Button event");
 }

 // Next goes the hard stuff: connection management, etc...
@@ -137,3 +144,4 @@
     Serial.println("Client disconnected.");
   }
 }
+


Delete InkyBoard_07_display_showNumbers.ino

--- examples/InkyBoard_07_display_showNumbers/InkyBoard_07_display_showNumbers.ino
+++ examples/InkyBoard_07_display_showNumbers/InkyBoard_07_display_showNumbers.ino
@@ -1,19 +0,0 @@
-#include <Servo.h>
-#include <InkyBoard.h>
-#include <Display.h>
-
-InkyBoard B; // definieer B als InkyBoard
-
-void setup() {
-  // put your setup code here, to run once:
-  B.init();
-  B.display.wakeup();
-}
-
-void loop() {
-  // put your main code here, to run repeatedly:
-  for (int i = 0; i < 99; i++){
-    B.display.number(i);
-    delay(250);
-  }
-}


Cleaning up Shield Demo.

Cleaning up Shield Demo.

--- examples/ESP8266_Shield_Demo/ESP8266_Shield_Demo.ino
+++ examples/ESP8266_Shield_Demo/ESP8266_Shield_Demo.ino
@@ -134,7 +134,6 @@
   // connected. 0 indicates disconnected. (Negative values
   // equate to communication errors.)
   retVal = esp8266.status();
-  Serial.println(retVal);
   if (retVal <= 0)
   {
     Serial.print(F("Connecting to "));


Added more delay between GET request and close connection command

--- frontdesk/module.wifi.ino
+++ frontdesk/module.wifi.ino
@@ -54,8 +54,7 @@
     delay(500);

     WIFI.print(getStr);
-    //delay(waitForFlush);
-    delay(100);
+    delay(waitForFlush);

     WIFI.println(closeConnection);
 }


とりあえず実装.

とりあえず実装.

--- arduino/GeneralIoSerial/GeneralIoSerial.ino
+++ arduino/GeneralIoSerial/GeneralIoSerial.ino
@@ -10,6 +10,8 @@

 // 各々のコマンドに対して処理を行う関数の型
 typedef char* (*TASK_FUNC_PTR)(String str);
+
+typedef void (*VOID_FUNC_PTR)(void);

 // コマンドの文字列の先頭の文字と処理する関数を保持する構造体
 struct TASK_FUNC {
@@ -74,6 +76,27 @@
   {"EXTERNAL", EXTERNAL}
 };

+const struct STR_UINT8_PEAR INTERRUPT_MODE_TBL[] = {
+  {"LOW", LOW},
+  {"CHANGE", CHANGE},
+  {"RISING", RISING},
+  {"FALLING", FALLING}
+};
+
+volatile uint32_t interrupt_counter[2] = {0};
+void interrupt0(void){
+  ++interrupt_counter[0];
+}
+
+void interrupt1(void){
+  ++interrupt_counter[1];
+}
+
+const VOID_FUNC_PTR INTERRUPT_FUNC_TBL[] = {
+  interrupt0,
+  interrupt1,
+};
+
 #endif

 const prog_char ILLEGAL_COMMAND[] PROGMEM       = "Illegal command.";
@@ -158,6 +181,32 @@
      example => d/mode/3?type=INPUT
    */
   {"d/mode/", &switchPinModeTask},
+
+
+  /*
+     attachInterrupt
+     format  => d/int/on/{num}?type={type}
+     {num}   => number of interrupt.
+     {type}  => LOW | CHANGE | RISING | FALLING
+     example => d/int/on/0?type=LOW
+
+     LOW ピンがLOWのとき発生
+     CHANGE ピンの状態が変化したときに発生
+     RISING ピンの状態がLOWからHIGHに変わったときに発生
+     FALLING ピンの状態がHIGHからLOWに変わったときに発生
+
+     // TODO: typeじゃなくてmodeにしたい
+   */
+  {"d/int/on/", &attachInterruptTask},
+
+
+  /*
+     detachInterrupt
+     format  => d/int/off/{num}
+     {num}   => number of interrupt.
+     example => d/int/off/0
+   */
+  {"d/int/off/", &detachInterruptTask},

   /*
     DI連続転送ON
@@ -267,7 +316,15 @@
     }
   }

-}
+  for(uint8_t i = 0; i < ARRAYSIZE(interrupt_counter); i++){
+    if(interrupt_counter[i] > 0){
+      Serial.println(wrapEventJson("int", interruptJson(i, interrupt_counter[i])));
+      interrupt_counter[i] = 0;
+    }
+  }
+
+}
+

 char* task(String msg){
   // 関数テーブルからタスクを決定する.
@@ -522,6 +579,45 @@
   return okIoJson(port, 0);
 }

+
+/*
+   LOWは何も繋げていないと大量に割込が入ってくるので注意
+*/
+char* attachInterruptTask(String queryWithType){
+
+  char type[16 + 1] = {0};
+  uint8_t num = 0;
+
+  char* error = checkPortWithType(queryWithType, &num, &type[0]);
+  if(error){
+    return error;
+  }
+
+  if(num >= ARRAYSIZE(INTERRUPT_FUNC_TBL)){
+    // ポートじゃないので後で別のエラーを作る.
+    return NgReturnJson(ILLEGAL_PORT_NUMBER);
+  }
+
+  for(uint8_t i = 0; i < ARRAYSIZE(INTERRUPT_MODE_TBL); i++){
+    if(strcmp(type, INTERRUPT_MODE_TBL[i].key) == 0){
+      attachInterrupt(num, INTERRUPT_FUNC_TBL[num], INTERRUPT_MODE_TBL[i].value);
+      return okAttachInterruptJson(num, i);
+    }
+  }
+
+  return NgReturnJson(ILLEGAL_TYPE);
+}
+
+char* detachInterruptTask(String queryWithPort){
+
+  char* error = checkPortQuery(queryWithPort);
+  if(error){
+    return error;
+  }
+
+  uint8_t num = strToInt(queryWithPort);
+  detachInterrupt(num);
+}

 // スタックオーバーフローにより強制的にリセット
 char* resetTask(String empty){
@@ -628,6 +724,22 @@
   return RESULT_MSG_PTR;
 }

+char* okAttachInterruptJson(uint8_t num, uint8_t mode){
+  snprintf(result_msg_buf, ARRAYSIZE(result_msg_buf),
+      "{\"msg\":\"OK\",\"num\":%d,\"mode\":\"%s\"}",
+      num,
+      INTERRUPT_MODE_TBL[mode].key);
+  return RESULT_MSG_PTR;
+}
+
+char* interruptJson(uint8_t num, uint32_t count){
+  snprintf(result_msg_buf, ARRAYSIZE(result_msg_buf),
+      "{\"msg\":\"OK\",\"num\":%d,\"count\":%d}",
+      num,
+      count);
+  return RESULT_MSG_PTR;
+}
+

 /*

@@ -738,6 +850,30 @@
   return NULL;
 }

+// {port}?type={type string}なqueryを分解.
+// typeのバッファに16文字コピーを行うので、
+// typeのバッファサイズは16以上にすること。
+// またtypeの文字列は16文字未満の文字列とすること.
+char* checkPortWithType(String portWithType, uint8_t *port, char *type){
+  String typeQuery;
+
+  char* error = checkPortWithQuery(portWithType, port, &typeQuery);
+  if(error){
+    return error;
+  }
+
+  if(typeQuery.startsWith("type=")){
+    typeQuery.replace("type=", "");
+    if(typeQuery.length() > 16){
+      return NgReturnJson(ILLEGAL_QUERY);
+    }
+    typeQuery.toCharArray(type, 16);
+  }else{
+    return NgReturnJson(ILLEGAL_QUERY);
+  }
+  return NULL;
+}
+
 char* checkPortWithValue(String portWithValue, uint8_t *port, int *val){

   String valQuery;


-Reduced TFT SPI clock to 20MHz.

-Reduced TFT SPI clock to 20MHz.

--- tfttask.ino
+++ tfttask.ino
@@ -10,7 +10,7 @@
   SPI.begin( SPI_SCK_PIN, SPI_MISO_PIN, SPI_MOSI_PIN );
   SPI.setFrequency( 60000000 );

-  tft.begin( 35000000, SPI );
+  tft.begin( 20000000, SPI );
   uint8_t x = tft.readcommand8( ILI9341_RDSELFDIAG );
   Serial.printf( "ILI9341 TFT Self Diagnostic: 0x%x\n", x );
   if ( x != 0xe0 )


Fix colors

Fix colors

--- src/src.ino
+++ src/src.ino
@@ -11,7 +11,7 @@
 #define TUNING 9 // Tunes range-finding to cm distance between pixels
 #define FPS 50
 #define NUM_PIXELS 50
-#define TRAIL_LENGTH 10
+#define TRAIL_LENGTH 4

 Adafruit_NeoPixel strip = Adafruit_NeoPixel(NUM_PIXELS, PIN, NEO_GRB + NEO_KHZ800);

@@ -23,10 +23,10 @@
   pinMode(TRIG_PIN, OUTPUT);
   pinMode(ECHO_PIN, INPUT);

-#if defined(__AVR_ATtiny85__)
-  if (F_CPU == 16000000)
-    clock_prescale_set(clock_div_1);
-#endif
+  #if defined(__AVR_ATtiny85__)
+    if (F_CPU == 16000000)
+      clock_prescale_set(clock_div_1);
+  #endif

   initColors();

@@ -52,19 +52,19 @@
 void colorWipe(uint8_t pixel) {
    std::rotate(trailingPixels, trailingPixels + TRAIL_LENGTH - 1, trailingPixels + TRAIL_LENGTH);
    trailingPixels[0] = pixel;
-
+
   for (uint8_t i = 0; i < strip.numPixels(); i++) {
     int *leaveLit = std::find(std::begin(trailingPixels), std::end(trailingPixels), pixel);
-
+
 //    if (leaveLit == std::end(trailingPixels)) {
-    strip.setPixelColor(i, 0);
+      strip.setPixelColor(i, 0);
 //    }
   }
 }

 void initColors() {
   for (int i = 0; i < strip.numPixels(); i++) {
-    int rgb = random(5);
+    int rgb = i % 5;
     switch (rgb) {
       case 0:
         colors[i] = strip.Color(255, 0, 0);
@@ -79,7 +79,7 @@
         colors[i] = strip.Color(255, 255, 0);
         break;
       case 4:
-        colors[i] = strip.Color(255, 0, 255);
+        colors[i] = strip.Color(0, 255, 255);
         break;
     }
   }


add lamp test

add lamp test

--- Christmas/Christmas.ino
+++ Christmas/Christmas.ino
@@ -115,6 +115,11 @@
 // This where you should start experimenting!

 void loop() {
+   allOn();
+   delay(5000);
+   allOff();
+
+
   for (int i=0; i<3; ++i)   // flash between green and red three times
   {
     // first we turn on all the red LEDs, and wait a second


Added pid speed

Added pid speed

--- LIDAR_withPIDs/LIDAR_withPIDs.ino
+++ LIDAR_withPIDs/LIDAR_withPIDs.ino
@@ -30,12 +30,14 @@
 const int steeringPin = 9, motorPin = 8;

 //define PID variables
-double Setpoint_theta = 0, Input_theta, Output_theta, Setpoint_dist = 90, Input_dist, Output_dist;
+double Setpoint_theta = 0, Input_theta, Output_theta, Setpoint_dist = 90, Input_dist, Output_dist, Setpoint_speed = 60, Input_speed, Output_speed;
 //specify initial PID links and tuning
 int KpT = 1.7, KiT = 0, KdT = 0.01;  // {1.7, 0, 0.01}
 int KpD = 1.1, KiD = 0.075, KdD = 0.97;   // {1, 0, 0}
+int KpS = 0.5, KiS = 0, KdS = 0;
 PID PID_theta(&Input_theta, &Output_theta, &Setpoint_theta, KpT, KiT, KdT, DIRECT);
 PID PID_dist(&Input_dist, &Output_dist, &Setpoint_dist, KpD, KiD, KdD, DIRECT);
+PID PID_speed(&Input_speed, &Output_speed, &Setpoint_speed, KpS, KiS, KdS, DIRECT);

 //Spacing between LIDARs on vehicle
 double LIDARspacing = 22.5;
@@ -57,10 +59,12 @@
 void setup() {
   PID_theta.SetMode(AUTOMATIC);
   PID_dist.SetMode(AUTOMATIC);
+  PID_speed.SetMode(AUTOMATIC);

   // Servo range limits
-  PID_theta.SetOutputLimits(-50, 50);
-  PID_dist.SetOutputLimits(-50, 50);
+  PID_theta.SetOutputLimits(-45, 45);
+  PID_dist.SetOutputLimits(-45, 45);
+  PID_speed.SetOutputLimits(-20, 20);

   steeringServo.attach(steeringPin);
   motorServo.attach(motorPin);
@@ -94,15 +98,16 @@
 }

 void loop() {
-  if(xbeeStream.available() && xbeeStream.read() == 0x00){
+  if (xbeeStream.available() && xbeeStream.read() == 0x00) {
     state_paused = !state_paused;
-    if(state_paused)  motorServo.write(90);
+    if (state_paused)  motorServo.write(90);
   }
   if (!state_paused) {
     now = millis();
     read_lidars();
     if (now - timestamp > 40) {
       wheel_rpm = ENCODER_SCALING * encoder_count / (now - timestamp);
+      Input_speed = wheel_rpm;
       encoder_count = 0;
       timestamp = now;
       //    dist[0] = 4.5 + dist[0] * (0.98); //scaling equation
@@ -112,13 +117,18 @@
       //    Setpoint_theta = constrain(0.5*(Input_dist - Setpoint_dist), -45, 45);

       // Run the PID loops
-//      xbeeStream.write(String(String(dist[0], 1) + ',' + String(dist[1], 1) + ',' + String(Input_theta, 1) + ',' + String(Input_dist, 1)));
+      //      xbeeStream.write(String(String(dist[0], 1) + ',' + String(dist[1], 1) + ',' + String(Input_theta, 1) + ',' + String(Input_dist, 1)));
       PID_theta.Compute();
       PID_dist.Compute();
+      PID_speed.Compute();

-      motorServo.write(90  - 20 );
-      steeringServo.write(90 + (Output_theta) - Output_dist); // + Output_theta - Output_dist
-    } // (50/abs(Input_dist - Setpoint_dist)) *
+      motorServo.write(90  - Output_speed );
+      if (abs(Output_dist - Setpoint_dist) < 10 && abs(Output_theta - Setpoint_theta) < 5) {
+        steeringServo.write(90 - Output_theta);
+      } else {
+        steeringServo.write(90 + (Output_theta) - Output_dist); // + Output_theta - Output_dist
+      } // (50/abs(Input_dist - Setpoint_dist)) *
+    }
   }
   delay(2);
 }


Added Heartbeat pulse and fixed value overflow

--- Pan_Tilt_laser/Pan_Tilt_laser.ino
+++ Pan_Tilt_laser/Pan_Tilt_laser.ino
@@ -14,7 +14,7 @@
     v1.0.0 - First release
     v1.1.0 - Added Markov-based speed control
     v1.2.0 - Added Markov-based pause length/frequency
-
+    v1.3.1 - Added Heartbeat during pause
 */
 /**************************************************************************/

@@ -102,11 +102,7 @@
   }

   else if(random(100001) < 10){
-    laser.fire(0);
-    panTilt.detach();
-    int delayVal = 60000 * random(5, 30);
-    delay(delayVal);
-    panTilt.begin();
+    sleep(30, 40);
   }

   laser.fire(1);
@@ -200,3 +196,33 @@
     return random(500, 750);
   }
 }
+
+void sleep(unsigned int minTime, unsigned int maxTime){
+  unsigned int delayVal = random(minTime, maxTime);
+  laser.fire(0);
+  panTilt.detach();
+  unsigned long startTime = millis();
+  for(unsigned int i = 0; i < delayVal; i++){}
+  while(millis() - startTime < 60000){
+    heartBeat();
+    delay(10000);
+  }
+  startTime = millis();
+}
+  panTilt.begin();
+  laser.fire(1);
+
+}
+
+void heartbeat(){
+  uint8_t ledPin = 13;
+  pinMode(ledPin, OUTPUT);
+  digitalWrite(ledPin, HIGH);
+  delay(250);
+  digitalWrite(ledPin, LOW);
+  delay(250);
+  digitalWrite(ledPin, HIGH);
+  delay(250);
+  digitalWrite(ledPin, LOW);
+
+}


Use a ring buffer between the interrupt and main loop

--- arduino/Main/Main.ino
+++ arduino/Main/Main.ino
@@ -1,9 +1,11 @@
 #include <Wire.h>
+#include <RingBufCPP.h>
 #include "LCD.h"
 #include "Motor.h"

 #define I2C_ADDRESS 0x1A
 #define I2C_BUFFER_SIZE 32
+#define MAX_NUM_ELEMENTS 30
 #define LCD_TX_PIN 10
 #define LCD_ROWS 4
 #define LCD_COLUMNS 20
@@ -18,10 +20,18 @@
 LCD lcd = LCD(LCD_TX_PIN, LCD_ROWS, LCD_COLUMNS);
 char i2c_buffer[I2C_BUFFER_SIZE];

+struct Event
+{
+  byte internal_address;
+  byte direction;
+  byte speed;
+};
+
 // Remember to use Arduino pins, not physical ones
 Motor motor1 = Motor(2, 3);
 Motor motor2 = Motor(4, 5);
 volatile byte sensor_left, sensor_front, sensor_right;
+RingBufCPP<struct Event, MAX_NUM_ELEMENTS> buf;

 void setup()
 {
@@ -45,13 +55,27 @@
 // L  :  0  5  5     F  :  0  5  5     R  :  0  5  5
 void loop()
 {
+  struct Event e;
+
+  while (buf.pull(&e))
+  {
+    switch (e.internal_address)
+    {
+    case MOTOR1_ADDRESS:
+      motor_event(&motor1, e.direction, e.speed);
+      break;
+    case MOTOR2_ADDRESS:
+      motor_event(&motor2, e.direction, e.speed);
+      break;
+    }
+  }
   lcd.set_cursor(1, 0);
   lcd.print("M1: %s", motor1.status());
   lcd.set_cursor(1, 9);
   lcd.print("M2: %s", motor2.status());
   lcd.set_cursor(3, 0);
   lcd.print("L:%03d F:%03d R:%03d", sensor_left, sensor_front, sensor_right);
-  delay(1000);
+  delay(2000);
 }

 void requestEvent()
@@ -70,33 +94,32 @@
   {
     return;
   }
+  byte internal_address = Wire.read();

-  byte internal_address = Wire.read();
-  switch (internal_address)
+  if (internal_address == MOTOR1_ADDRESS || internal_address == MOTOR2_ADDRESS)
   {
-  case MOTOR1_ADDRESS:
-    motor_event(&motor1, receive_size - 1);
-    break;
-  case MOTOR2_ADDRESS:
-    motor_event(&motor2, receive_size - 1);
-    break;
-  default:
-    return;
+    if (!Wire.available())
+    {
+      return;
+    }
+    byte direction = Wire.read();
+
+    if (!Wire.available())
+    {
+      return;
+    }
+    byte speed = Wire.read();
+
+    struct Event e;
+    e.internal_address = internal_address;
+    e.direction = direction;
+    e.speed = speed;
+    buf.add(e);
   }
 }

-void motor_event(Motor *motor, int receive_size)
+void motor_event(Motor *motor, byte direction, byte speed)
 {
-  if (!Wire.available())
-  {
-    return;
-  }
-  byte direction = Wire.read();
-  if (!Wire.available())
-  {
-    return;
-  }
-  byte speed = Wire.read();
   if (0x01 == direction)
   {
     motor->forward(speed);


added the arduino sketches from lab and presentation.

--- arduino/hellodotstar/hellodotstar.ino
+++ arduino/hellodotstar/hellodotstar.ino
@@ -7,18 +7,14 @@
 #define CLOCKPIN   MOSI
 #define NUMPIXELS 30 // Number of LEDs in strip

-#define red  0xFF0000
-#define green  0x00FF00
-#define blue  0x0000FF
+#define RED  0xFF0000
+#define GREEN  0x00FF00
+#define BLUE  0x0000FF

 uint32_t frame[NUMPIXELS];
+uint32_t colors[3] = {RED, GREEN, BLUE };

-
-uint32_t colors[3] = {green, red, blue };
-
-Adafruit_DotStar strip = Adafruit_DotStar(NUMPIXELS, DATAPIN, CLOCKPIN, DOTSTAR_BRG);
-
-
+Adafruit_DotStar strip = Adafruit_DotStar(NUMPIXELS, DATAPIN, CLOCKPIN, DOTSTAR_BGR);

 void setup() {
   strip.begin(); // Initialize pins for output
@@ -26,10 +22,25 @@

   //initialize strip array
   for (int ii = 0; ii < NUMPIXELS; ii++) {
-    frame[ii] = 0xFF0000; //initialize all to red
+    frame[ii] = RED; //initialize all to red
     strip.setPixelColor(ii, frame[ii]);
   }
   strip.show();
+  delay(1000);
+  //initialize strip array
+  for (int ii = 0; ii < NUMPIXELS; ii++) {
+    frame[ii] = GREEN; //initialize all to GREEN
+    strip.setPixelColor(ii, frame[ii]);
+  }
+  strip.show();
+  delay(1000);
+  //initialize strip array
+  for (int ii = 0; ii < NUMPIXELS; ii++) {
+    frame[ii] = BLUE; //initialize all to BLUE
+    strip.setPixelColor(ii, frame[ii]);
+  }
+  strip.show();
+  delay(1000);

 }


Fix example for Milkcocoa

Fix example for Milkcocoa

--- examples/mqtt_esp8266/mqtt_esp8266.ino
+++ examples/mqtt_esp8266/mqtt_esp8266.ino
@@ -1,34 +1,28 @@
 /***************************************************
   Adafruit MQTT Library ESP8266 Example
-
   Must use ESP8266 Arduino from:
     https://github.com/esp8266/Arduino
-
-  Works great with Adafruit's Huzzah ESP board:
-  ----> https://www.adafruit.com/product/2471
-
   Adafruit invests time and resources providing this open source code,
   please support Adafruit and open-source hardware by purchasing
   products from Adafruit!
-
   Written by Tony DiCola for Adafruit Industries.
   MIT license, all text above must be included in any redistribution
  ****************************************************/
 #include <ESP8266WiFi.h>
-#include "Adafruit_MQTT.h"
-#include "Adafruit_MQTT_Client.h"
+#include <Adafruit_MQTT.h>
+#include <Adafruit_MQTT_Client.h>

 /************************* WiFi Access Point *********************************/

-#define WLAN_SSID       "...your SSID..."
-#define WLAN_PASS       "...your password..."
+#define WLAN_SSID       "your_SSID"
+#define WLAN_PASS       "your_password"

-/************************* Adafruit.io Setup *********************************/
+/************************* Milkcocoa Setup *********************************/

-#define AIO_SERVER      "io.adafruit.com"
-#define AIO_SERVERPORT  1883
-#define AIO_USERNAME    "...your AIO username (see https://accounts.adafruit.com)..."
-#define AIO_KEY         "...your AIO key..."
+#define MILKCOCOA_SERVER      "your_milkcocoa_app_id.mlkcca.com"
+#define MILKCOCOA_SERVERPORT  1883
+#define MILKCOCOA_USERNAME    "sdammy"
+#define MILKCOCOA_KEY         "your_milkcocoa_app_id"

 /************ Global State (you don't need to change this!) ******************/

@@ -37,27 +31,25 @@

 // Store the MQTT server, client ID, username, and password in flash memory.
 // This is required for using the Adafruit MQTT library.
-const char MQTT_SERVER[] PROGMEM    = AIO_SERVER;
-// Set a unique MQTT client ID using the AIO key + the date and time the sketch
+const char MQTT_SERVER[] PROGMEM    = MILKCOCOA_SERVER;
+// Set a unique MQTT client ID using the Milkcocoa key + the date and time the sketch
 // was compiled (so this should be unique across multiple devices for a user,
 // alternatively you can manually set this to a GUID or other random value).
-const char MQTT_CLIENTID[] PROGMEM  = __TIME__ AIO_USERNAME;
-const char MQTT_USERNAME[] PROGMEM  = AIO_USERNAME;
-const char MQTT_PASSWORD[] PROGMEM  = AIO_KEY;
+const char MQTT_CLIENTID[] PROGMEM  = __TIME__ MILKCOCOA_USERNAME;
+const char MQTT_USERNAME[] PROGMEM  = MILKCOCOA_USERNAME;
+const char MQTT_PASSWORD[] PROGMEM  = MILKCOCOA_KEY;

 // Setup the MQTT client class by passing in the WiFi client and MQTT server and login details.
-Adafruit_MQTT_Client mqtt(&client, MQTT_SERVER, AIO_SERVERPORT, MQTT_CLIENTID, MQTT_USERNAME, MQTT_PASSWORD);
+Adafruit_MQTT_Client mqtt(&client, MQTT_SERVER, MILKCOCOA_SERVERPORT, MQTT_CLIENTID, MQTT_USERNAME, MQTT_PASSWORD);

-/****************************** Feeds ***************************************/
+/****************************** Topic ***************************************/

-// Setup a feed called 'photocell' for publishing.
-// Notice MQTT paths for AIO follow the form: <username>/feeds/<feedname>
-const char PHOTOCELL_FEED[] PROGMEM = AIO_USERNAME "/feeds/photocell";
-Adafruit_MQTT_Publish photocell = Adafruit_MQTT_Publish(&mqtt, PHOTOCELL_FEED);
-
-// Setup a feed called 'onoff' for subscribing to changes.
-const char ONOFF_FEED[] PROGMEM = AIO_USERNAME "/feeds/onoff";
-Adafruit_MQTT_Subscribe onoffbutton = Adafruit_MQTT_Subscribe(&mqtt, ONOFF_FEED);
+// Setup a pubulisher & subscriber.
+// Topic Name: "your_milkcocoa_app_id/your_milkcocoa_datastore_name/method_name"
+const char PUSH_TOPIC[] PROGMEM = "your_milkcocoa_app_id/your_milkcocoa_datastore_name/push";
+const char SEND_TOPIC[] PROGMEM = "your_milkcocoa_app_id/your_milkcocoa_datastore_name/send";
+Adafruit_MQTT_Publish pushPublisher = Adafruit_MQTT_Publish(&mqtt, PUSH_TOPIC);
+Adafruit_MQTT_Subscribe pushSubscriber = Adafruit_MQTT_Subscribe(&mqtt, PUSH_TOPIC);

 /*************************** Sketch Code ************************************/

@@ -83,7 +75,7 @@
   Serial.println("IP address: "); Serial.println(WiFi.localIP());

   // Setup MQTT subscription for onoff feed.
-  mqtt.subscribe(&onoffbutton);
+  mqtt.subscribe(&pushSubscriber);
 }

 uint32_t x=0;
@@ -94,32 +86,18 @@
   // function definition further below.
   MQTT_connect();

-  // Try to ping the MQTT server
-  /*
-  if (! mqtt.ping(3) ) {
-    // MQTT pings failed, lets reconnect
-    Serial.println("Ping fail!");
-  }
-  */
+  // this is our 'wait for incoming subscription packets' busy subloop
+ Adafruit_MQTT_Subscribe *subscription;
+ while ((subscription = mqtt.readSubscription(1000))) {
+   if (subscription == &pushSubscriber) {
+     Serial.print(F("Got: "));
+     Serial.println((char *)pushSubscriber.lastread);
+   }
+ }

-  // this is our 'wait for incoming subscription packets' busy subloop
-  Adafruit_MQTT_Subscribe *subscription;
-  while ((subscription = mqtt.readSubscription(1000))) {
-    if (subscription == &onoffbutton) {
-      Serial.print(F("Got: "));
-      Serial.println((char *)onoffbutton.lastread);
-    }
-  }
-
-  // Now we can publish stuff!
-  Serial.print(F("\nSending photocell val "));
-  Serial.print(x);
-  Serial.print("...");
-  if (! photocell.publish(x++)) {
-    Serial.println(F("Failed"));
-  } else {
-    Serial.println(F("OK!"));
-  }
+  // publish
+  // e.g. publish("{ \"params\":{ \"your_key1\":\"your_value1\", \"your_key2\":\"your_value2\",...} }");
+  pushPublisher.publish("{\"params\":{\"content\":\"from ESP8266!\"}}");

   delay(1000);
 }
@@ -138,9 +116,10 @@

   while ((ret = mqtt.connect()) != 0) { // connect will return 0 for connected
        Serial.println(mqtt.connectErrorString(ret));
+       Serial.println(ret);
        Serial.println("Retrying MQTT connection in 5 seconds...");
        mqtt.disconnect();
        delay(5000);  // wait 5 seconds
   }
   Serial.println("MQTT Connected!");
-}
+}


remove debug stmt from example

remove debug stmt from example

--- examples/BasicUsage/BasicUsage.ino
+++ examples/BasicUsage/BasicUsage.ino
@@ -64,9 +64,7 @@
 	}

 	delete my_cursor;
-
-	Serial.println("toaster");
-
+
 	/* Always clean up your dictionaries when you're done with them! */
 	delete dict;
 }


Small code cleanup.

Small code cleanup.

--- tfttask.ino
+++ tfttask.ino
@@ -151,8 +151,7 @@
   {
     TS_Point p = touch.getPoint();

-    tftPoint_t clickedLocation;
-    clickedLocation = mapToTft( p.x, p.y );
+    tftPoint_t clickedLocation = mapToTft( p.x, p.y );

     if ( buttonPressed( LIGHTSON_BUTTON , clickedLocation ) )
     {


Logging uses built-in HEX print

Logging uses built-in HEX print

--- firmware/via_midi_2/via_midi_2.ino
+++ firmware/via_midi_2/via_midi_2.ino
@@ -1,7 +1,7 @@

 /*
- * Unser development.
+ * Under development.
  * Midi with a custom software serial implementation with non-blocking read/write.
  */

@@ -20,7 +20,7 @@
   midi_soft_serial.begin();

   MIDI.begin(MIDI_CHANNEL_OMNI);
-  // MIDI.turnThruOff();  // midi through should work non-blocking now
+  MIDI.turnThruOff();  // midi through should work non-blocking now
 }

@@ -40,15 +40,14 @@
   }
 #endif

-#if 0  // test receive
+#if 1  // test receive
   while (midi_rx_available())
   {
     uint8_t in = midi_rx_read();
     if (midi_tx_available)
       midi_tx_write(in);
     Serial.print("RX: ");
-    Serial.print(to_hex((in>>4)&0x0f));
-    Serial.println(to_hex(in&0x0f));
+    Serial.println(in, HEX);
   }
 #endif

@@ -56,13 +55,5 @@

-
-char to_hex(uint8_t in)
-{
-  if (in>16) return ' ';
-  if (in<10) return '0'+in;
-  else return 'A'+in-10;
-}
-
 //=====================================================================================================


Updated som Comments

Updated som Comments

--- ComboKeypad/Keypad.ino
+++ ComboKeypad/Keypad.ino
@@ -8,7 +8,7 @@

 const bool useFiveVoltpin = false;   //Change this if you want to use the
 ///////////////////////////////////////5Volt PIN instead of PIN 2(IN MY CASE)
-//USER VARIABLES END
+/*----(USER VARIABLES END)----*/

 ///
 const int numOfCombos = sizeof(combination) / sizeof(combination[0]);   //How many combinations
@@ -19,7 +19,7 @@
 int count = 0;   //How many buttens that have been pressed
 int input[numOfCombos]; //Saves what the user has clicked
 //
-const int del = 10000;   //Timer(10s), if user inputs a number
+const short int del = 10000;   //Timer(10s), if user inputs a number
 ///////////////////////////and not inputs another within 10 seconds program will reset
 unsigned long time;
 short int buttonsRead[numOfButtons];   //Stores the "digitalRead()" of buttons[]
@@ -33,7 +33,7 @@
 void debug();
 //Function prototyping

-//Function setup
+/*----(Function setup)----*/
 void setup() {
     //Used to be able to add as many buttons as you want
     for(int setb = 0; setb < numOfButtons; setb++){
@@ -48,10 +48,10 @@
   digitalWrite(buttonStream, 1);   //Starts the Electricity for button pull down resistor
 }
   Serial.begin(9600); //Debug
-}//Setup Function End
+}/*----(Setup Function End)----*/

-//Function loop
+/*----(Function loop)----*/
 void loop() {
     //Gets the Button States
 for(int state = 0; state < numOfButtons; state++){