Initial commit

Initial commit

error: pathspec 'source/vedroidmobil.ino' did not match any file(s) known to git

--- source/vedroidmobil.ino
+++ source/vedroidmobil.ino
@@ -0,0 +1,517 @@
+#include <AFMotor.h> // Подключаем библиотеку для управления двигателями
+#include <Servo.h>   // Подключаем библиотеку для сервоприводов
+#include <SoftwareSerial.h> // Подключаем библиотеку для работы с Serial через дискретные порты
+
+//Создаем объекты для двигателей
+AF_DCMotor motor1(1); //канал М1 на Motor Shield — задний левый
+AF_DCMotor motor2(2); //канал М2 на Motor Shield — задний правый
+AF_DCMotor motor3(3); //канал М3 на Motor Shield — передний левый
+AF_DCMotor motor4(4); //канал М4 на Motor Shield — передний правый
+// Создаем объект для сервопривода
+Servo vservo;
+// Прописываем пины используемые модулем Bluetooth
+SoftwareSerial BTSerial(50, 51); // RX, TX
+// Создаем переменную для команд Bluetooth
+char vcmd;
+// Создаем переменные для запоминания скорости левых и правых двигателей
+int vspdL, vspdR;
+/* Создаем переменную, на значение которой будет уменьшаться скорость при плавных поворотах.
+Текущая скорость должна быть больше этого значения.  В противном случае двигатели со стороны направления поворота просто не будут вращаться */
+const int vspd = 200;
+// Заносим в массив пины, к которым подключены светодиоды
+const int vledpins[6]={
+  22,23,24,25,26,27};
+// Создаем переменную для сохранения режима работы
+int vmode;
+// Создаем переменную для сохранения предыдущего режима работы
+int vmodeprev = -1;
+// Заносим в массив пины, к которым подключены кнопки
+const int vbtn[2]={
+  28,29};
+// Массив для хранения углов поворота сервопривода (шаг 15 градусов)
+const int vservo_array[13]={
+  0,15,30,45,60,75,90,105,120,135,150,165,180};
+// Массив для хранения данных о расстоянии под различными углами поворота сервопривода
+int vHC_SR04_array[13];
+// Пины, используемые ультразвуковым дальномером
+const int vTrig = 31;
+const int vEcho = 30;
+// Переменные, для хранения данных с дальномера
+unsigned int vtime_us=0;
+unsigned int vdistance_sm=0;
+// Минимальное расстояние в сантиметрах, при котором нужно искать новый маршрут движения
+const int vmindistance = 30;
+// Переменная для циклов перебора значения массивов vservo_array и vHC_SR04_array
+int vservo_int;
+// Переменные для цикла поиска максимального значения в массивах
+int vmaxarrayindex_int;
+int vmaxarrayvalue_int;
+
+void setup() {
+  // Устанавливаем скорость передачи данных по Bluetooth
+  BTSerial.begin(9600);
+  // Устанавливаем скорость передачи данных по кабелю
+  Serial.begin(9600);
+  // Выбираем пин к которому подключен сервопривод
+  vservo.attach(9); // или 10, если воткнули в крайний разъём
+  // Поворачиваем сервопривод в положение 90 градусов при каждом включении
+  vservo.write(90);
+  // Устанавливаем максимальную скорость вращения двигателей
+  vspeed(255,255);
+  /* Устанавливаем все выводы, к которым подключены светодиоды,
+   в OUTPUT. Зажигаем и гасим светодиоды с интервалом в 0.5 сек
+   для проверки */
+  for (int i = 0; i < 6; i++) {
+    pinMode(vledpins[i], OUTPUT);
+    digitalWrite(vledpins[i], HIGH);
+    delay (500);
+    digitalWrite(vledpins[i], LOW);
+  }
+  /* Устанавливаем выводы, к которым подключены кнопки, в INPUT. */
+  pinMode(vbtn[0], INPUT);
+  pinMode(vbtn[1], INPUT);
+  // Устанавливаем значение первого режима работы робота
+  vmode = 0;
+  // Устанавливаем значение для пинов, к которым подключен ультразвуковой дальномер
+  pinMode(vTrig, OUTPUT);
+  pinMode(vEcho, INPUT);
+
+  delay(500);
+}
+
+void loop() {
+  /* Переключение режимов работы робота */
+  // Кнопка переключения на следующий режим — BTN1
+  if (digitalRead(vbtn[0]) == HIGH) {
+    vmode = vmode + 1;
+    vmodeprev = vmode - 1;
+    if (vmode > 5) {
+      vmode = 0;
+      vmodeprev = 5;
+    }
+    vrelease();
+    delay (500);
+  }
+  // Кнопка переключения на предыдущий режим — BTN2
+  if (digitalRead(vbtn[1]) == HIGH) {
+    vmode = vmode - 1;
+    vmodeprev = vmode + 1;
+    if (vmode < 0) {
+      vmode = 5;
+      vmodeprev = 0;
+    }
+    vrelease();
+    delay (500);
+  }
+  // Засвечиваем светодиод текущего режима работы
+  digitalWrite(vledpins[vmode], HIGH);
+  // Гасим светодиод предыдущего режима работы
+  if (vmodeprev > -1) {
+    digitalWrite(vledpins[vmodeprev], LOW);
+  }
+  /* Выбор режима работы */
+  switch (vmode) {
+  case 0:
+    // Режим ожидания
+    vultrasoundmode();
+    break;
+  case 1:
+    // Режим работы с использованием ультразвукового дальномера
+    // vultrasoundmode();
+    break;
+  case 2:
+    // Режим
+    break;
+  case 3:
+    // Режим управления через Bluetooth
+    vbluetoothmode();
+    break;
+  case 4:
+    // Режим
+    break;
+  case 5:
+    // Режим
+    break;
+  }
+}
+
+void vultrasoundmode() {
+  vservo.write(90);
+  delay(500);
+
+  int distance = vHC_SR04();
+
+  if (distance >= vmindistance) {
+    vforward();
+    return;
+  }
+
+  Serial.println("stop");
+  vrelease();
+
+  int dmin = -1;
+  int amin = -1;
+  int dmax = -1;
+  int amax = -1;
+
+  for (int ang = 0; ang <= 180; ang += 15) {
+    vservo.write(ang);
+    delay(250);
+
+    int dist = vHC_SR04();
+
+    if (dmin < 0 || dist < dmin) {
+      dmin = dist;
+      amin = ang;
+    }
+
+    if (dmax < 0 || dist > dmax) {
+      dmax = dist;
+      amax = ang;
+    }
+  }
+
+  Serial.print("amin = ");
+  Serial.print(amin);
+  Serial.print(", dmin = ");
+  Serial.print(dmin);
+  Serial.print(", amax = ");
+  Serial.print(amax);
+  Serial.print(", dmax = ");
+  Serial.println(dmax);
+
+  if (amax >= 0 && dmax < vmindistance) {
+    vbackward();
+    delay(500);
+    return;
+  }
+
+  if (amin < 0) {
+    return;
+  }
+
+  vservo.write(amin);
+  delay(300);
+
+  Serial.print("amin = ");
+  Serial.println(amin);
+
+  for (int max_iterations = 0; max_iterations < 100; max_iterations++) {
+    int dist = vHC_SR04();
+
+    Serial.print("dist = ");
+    Serial.print(dist);
+    Serial.print(", up to = ");
+    Serial.println(vmindistance * 2);
+
+    if (dist > vmindistance * 2) {
+      break;
+    }
+
+    if (amin > 90) {
+      vright();
+    } else {
+      vleft();
+    }
+
+    delay(50);
+  }
+
+  Serial.println("done");
+}
+
+/* Режим работы с использованием ультразвукового дальномера */
+void vultrasoundmode_old(){
+  vservo.write(90);
+  delay(200);
+  Serial.print("Now ");
+  Serial.println(vHC_SR04());
+  // Если расстояние меньше наименьшего, то
+  if (vHC_SR04() < vmindistance) {
+    // Останавливаем двигатели
+    vrelease();
+    // Крутим серву измеряя расстояния и занося данные в массив
+    for (vservo_int = 0; vservo_int < 13; vservo_int = vservo_int + 1) {
+      vservo.write(vservo_array[vservo_int]);
+      delay(200);
+      vHC_SR04_array[vservo_int] = vHC_SR04();
+      // Выводим данные для отладки
+      Serial.print(vservo_int);
+      Serial.print(" ");
+      Serial.println(vHC_SR04_array[vservo_int]);
+    }
+    vservo.write(90);
+    delay(500);
+    // Поиск в массиве позиции с максимальным значением
+    vmaxarrayindex_int = 0;
+    vmaxarrayvalue_int = 0;
+    for (vservo_int = 0; vservo_int < 13; vservo_int = vservo_int + 1) {
+      if (vHC_SR04_array[vservo_int] > vmaxarrayvalue_int) {
+        vmaxarrayindex_int = vservo_int;
+        vmaxarrayvalue_int = vHC_SR04_array[vservo_int];
+      }
+    }
+    Serial.print("Max index ");
+    Serial.println(vmaxarrayindex_int);
+    // Проверка — если максимальное значение массива меньше минимального расстояния, то едем назад
+    if (vHC_SR04_array[vmaxarrayindex_int] < vmindistance) {
+      vbackward();
+      delay(500);
+    }
+    /* Проверка — если индекс максимального значения массива меньше 6 то поворачиваем вправо,
+    иначе влево */
+    if (vmaxarrayindex_int < 6) {
+      vright();
+      delay(500);
+    }
+    else
+    {
+      vleft();
+      delay(500);
+    }
+  }
+  else
+  {
+    // Едем прямо
+    vforward();
+  }
+}
+
+/* Режим управления через Bluetooth */
+void vbluetoothmode() {
+  // Если есть данные с Bluetooth
+  if (BTSerial.available())
+  {
+    /* Читаем команды и заносим их в переменную.
+     (char) преобразует код символа команды в символ */
+    vcmd = (char)BTSerial.read();
+    // Отправляем команду в порт, чтобы можно было их проверить в "Мониторе порта"
+    Serial.println(vcmd);
+
+    // Вперед
+    if (vcmd == 'F') {
+      vforward();
+    }
+    // Назад
+    if (vcmd == 'B')
+    {
+      vbackward();
+    }
+    // Влево
+    if (vcmd == 'L')
+    {
+      vleft();
+    }
+    // Вправо
+    if (vcmd == 'R')
+    {
+      vright();
+    }
+    // Прямо и влево
+    if (vcmd == 'G')
+    {
+      vforwardleft();
+    }
+    // Прямо и вправо
+    if (vcmd == 'I')
+    {
+      vforwardright();
+    }
+    // Назад и влево
+    if (vcmd == 'H')
+    {
+      vbackwardleft();
+    }
+    // Назад и вправо
+    if (vcmd == 'J')
+    {
+      vbackwardright();
+    }
+    // Стоп
+    if (vcmd == 'S')
+    {
+      vrelease();
+    }
+    // Скорость 0%
+    if (vcmd == '0')
+    {
+      vspeed(0,0);
+    }
+    // Скорость 10%
+    if (vcmd == '1')
+    {
+      vspeed(25,25);
+    }
+    // Скорость 20%
+    if (vcmd == '2')
+    {
+      vspeed(50,50);
+    }
+    // Скорость 30%
+    if (vcmd == '3')
+    {
+      vspeed(75,75);
+    }
+    // Скорость 40%
+    if (vcmd == '4')
+    {
+      vspeed(100,100);
+    }
+    // Скорость 50%
+    if (vcmd == '5')
+    {
+      vspeed(125,125);
+    }
+    // Скорость 60%
+    if (vcmd == '6')
+    {
+      vspeed(150,150);
+    }
+    // Скорость 70%
+    if (vcmd == '7')
+    {
+      vspeed(175,175);
+    }
+    // Скорость 80%
+    if (vcmd == '8')
+    {
+      vspeed(200,200);
+    }
+    // Скорость 90%
+    if (vcmd == '9')
+    {
+      vspeed(225,225);
+    }
+    // Скорость 100%
+    if (vcmd == 'q')
+    {
+      vspeed(255,255);
+    }
+  }
+}
+
+/* Функция определения расстояния с дальномера */
+int vHC_SR04() {
+  digitalWrite(vTrig, HIGH); // Подаем сигнал на выход микроконтроллера
+  delayMicroseconds(10); // Удерживаем 10 микросекунд
+  digitalWrite(vTrig, LOW); // Затем убираем
+  vtime_us=pulseIn(vEcho, HIGH); // Замеряем длину импульса
+  vdistance_sm=vtime_us/58; // Пересчитываем в сантиметры
+  return vdistance_sm; // Возвращаем значение
+}
+
+/* Функции управления двигателями */
+
+// Вперед
+void vforward() {
+  vspeed(vspdL,vspdR);
+  vforwardRL();
+}
+
+// Вперед для RL
+void vforwardRL() {
+  motor1.run(FORWARD);
+  motor2.run(FORWARD);
+  motor3.run(FORWARD);
+  motor4.run(FORWARD);
+}
+
+// Назад
+void vbackward() {
+  vspeed(vspdL,vspdR);
+  vbackwardRL();
+}
+
+// Назад для RL
+void vbackwardRL() {
+  motor1.run(BACKWARD);
+  motor2.run(BACKWARD);
+  motor3.run(BACKWARD);
+  motor4.run(BACKWARD);
+}
+
+// Влево
+void vleft() {
+  vspeed(vspdL,vspdR);
+  motor1.run(BACKWARD);
+  motor2.run(FORWARD);
+  motor3.run(BACKWARD);
+  motor4.run(FORWARD);
+}
+
+// Вправо
+void vright() {
+  vspeed(vspdL,vspdR);
+  motor1.run(FORWARD);
+  motor2.run(BACKWARD);
+  motor3.run(FORWARD);
+  motor4.run(BACKWARD);
+}
+
+// Вперед и влево
+void vforwardleft() {
+  if (vspdL > vspd) {
+    vspeed(vspdL-vspd,vspdR);
+  }
+  else
+  {
+    vspeed(0,vspdR);
+  }
+  vforwardRL();
+}
+
+// Вперед и вправо
+void vforwardright() {
+  if (vspdR > vspd) {
+    vspeed(vspdL,vspdR-vspd);
+  }
+  else
+  {
+    vspeed(vspdL,0);
+  }
+  vforwardRL();
+}
+
+// Назад и влево
+void vbackwardleft() {
+  if (vspdL > vspd) {
+    vspeed(vspdL-vspd,vspdR);
+  }
+  else
+  {
+    vspeed(0,vspdR);
+  }
+  vbackwardRL();
+}
+
+// Назад и вправо
+void vbackwardright() {
+  if (vspdR > vspd) {
+    vspeed(vspdL,vspdR-vspd);
+  }
+  else
+  {
+    vspeed(vspdL,0);
+  }
+  vbackwardRL();
+}
+
+// Стоп
+void vrelease(){
+  motor1.run(RELEASE);
+  motor2.run(RELEASE);
+  motor3.run(RELEASE);
+  motor4.run(RELEASE);
+}
+
+// Изменение скорости
+void vspeed(int spdL,int spdR){
+  if (spdL == spdR) {
+    vspdL=spdL;
+    vspdR=spdR;
+  }
+  motor1.setSpeed(spdL);
+  motor2.setSpeed(spdR);
+  motor3.setSpeed(spdL);
+  motor4.setSpeed(spdR);
+}


Tested adjusting bitLength slider plus I added second Arduino project to listen to UART

remote: Support for password authentication was removed on August 13, 2021.
remote: Please see https://docs.github.com/en/get-started/getting-started-with-git/about-remote-repositories#cloning-with-https-urls for information on currently recommended modes of authentication.
fatal: Authentication failed for 'https://github.com/embeddedwes/AndroidToArduino.git/'


Commit Before Full Code Clean Up

Commit Before Full Code Clean Up

Saving code before I branch into a new, hopefully cleaner setup.

remote: Support for password authentication was removed on August 13, 2021.
remote: Please see https://docs.github.com/en/get-started/getting-started-with-git/about-remote-repositories#cloning-with-https-urls for information on currently recommended modes of authentication.
fatal: Authentication failed for 'https://github.com/lowjunen/THOR.git/'


Create MRLCommMicro.ino

Create MRLCommMicro.ino

error: pathspec 'home/GroG/MRLCommMicro.ino' did not match any file(s) known to git

--- home/GroG/MRLCommMicro.ino
+++ home/GroG/MRLCommMicro.ino
@@ -0,0 +1,1279 @@
+/**
+*
+* @author GroG (at) myrobotlab.org
+*
+* This file is part of MyRobotLab.
+*
+* Enjoy !
+*
+* MRLComm.ino
+* -----------------
+* Purpose: support servos, sensors, analog & digital polling
+* oscope, motors, range sensors, pingdar & steppers.
+*
+* Requirements: MyRobotLab running on a computer & a serial connection
+*
+*/
+
+#include <Servo.h>
+#define MRLCOMM_VERSION				21
+
+// serial protocol functions
+#define MAGIC_NUMBER  					170 // 10101010
+
+// ----- MRLCOMM FUNCTION GENERATED INTERFACE BEGIN -----------
+///// INO GENERATED DEFINITION BEGIN //////
+// {publishMRLCommError Integer}
+#define PUBLISH_MRLCOMM_ERROR		1
+
+// {getVersion}
+#define GET_VERSION		2
+
+// {publishVersion Integer}
+#define PUBLISH_VERSION		3
+
+// {analogReadPollingStart Integer}
+#define ANALOG_READ_POLLING_START		4
+
+// {analogReadPollingStop Integer}
+#define ANALOG_READ_POLLING_STOP		5
+
+// {analogWrite Integer Integer}
+#define ANALOG_WRITE		6
+
+// {digitalReadPollingStart Integer}
+#define DIGITAL_READ_POLLING_START		7
+
+// {digitalReadPollingStop Integer}
+#define DIGITAL_READ_POLLING_STOP		8
+
+// {digitalWrite Integer Integer}
+#define DIGITAL_WRITE		9
+
+// {motorAttach String String Integer Integer Integer}
+#define MOTOR_ATTACH		10
+
+// {motorDetach String}
+#define MOTOR_DETACH		11
+
+// {motorMove String}
+#define MOTOR_MOVE		12
+
+// {motorMoveTo String double}
+#define MOTOR_MOVE_TO		13
+
+// {pinMode Integer Integer}
+#define PIN_MODE		14
+
+// {publishCustomMsg Integer}
+#define PUBLISH_CUSTOM_MSG		15
+
+// {publishLoadTimingEvent Long}
+#define PUBLISH_LOAD_TIMING_EVENT		16
+
+// {publishPin Pin}
+#define PUBLISH_PIN		17
+
+// {publishPulse Integer}
+#define PUBLISH_PULSE		18
+
+// {publishServoEvent Integer}
+#define PUBLISH_SERVO_EVENT		19
+
+// {publishSesorData SensorData}
+#define PUBLISH_SESOR_DATA		20
+
+// {publishStepperEvent StepperData}
+#define PUBLISH_STEPPER_EVENT		21
+
+// {publishTrigger Pin}
+#define PUBLISH_TRIGGER		22
+
+// {pulseIn int int int String}
+#define PULSE_IN		23
+
+// {sensorAttach UltrasonicSensor}
+#define SENSOR_ATTACH		24
+
+// {sensorPollingStart String int}
+#define SENSOR_POLLING_START		25
+
+// {sensorPollingStop String}
+#define SENSOR_POLLING_STOP		26
+
+// {servoAttach Servo Integer}
+#define SERVO_ATTACH		27
+
+// {servoDetach String}
+#define SERVO_DETACH		28
+
+// {servoSweepStart String int int int}
+#define SERVO_SWEEP_START		29
+
+// {servoSweepStop String}
+#define SERVO_SWEEP_STOP		30
+
+// {servoWrite String Integer}
+#define SERVO_WRITE		31
+
+// {servoWriteMicroseconds String Integer}
+#define SERVO_WRITE_MICROSECONDS		32
+
+// {setDebounce int}
+#define SET_DEBOUNCE		33
+
+// {setDigitalTriggerOnly Boolean}
+#define SET_DIGITAL_TRIGGER_ONLY		34
+
+// {setLoadTimingEnabled boolean}
+#define SET_LOAD_TIMING_ENABLED		35
+
+// {setPWMFrequency Integer Integer}
+#define SET_PWMFREQUENCY		36
+
+// {setSampleRate int}
+#define SET_SAMPLE_RATE		37
+
+// {setSerialRate int}
+#define SET_SERIAL_RATE		38
+
+// {setServoEventsEnabled String boolean}
+#define SET_SERVO_EVENTS_ENABLED		39
+
+// {setServoSpeed String Float}
+#define SET_SERVO_SPEED		40
+
+// {setStepperSpeed Integer}
+#define SET_STEPPER_SPEED		41
+
+// {setTrigger int int int}
+#define SET_TRIGGER		42
+
+// {softReset}
+#define SOFT_RESET		43
+
+// {stepperAttach StepperControl}
+#define STEPPER_ATTACH		44
+
+// {stepperDetach String}
+#define STEPPER_DETACH		45
+
+// {stepperMoveTo String Integer}
+#define STEPPER_MOVE_TO		46
+
+// {stepperReset String}
+#define STEPPER_RESET		47
+
+// {stepperStep String Integer Integer}
+#define STEPPER_STEP		48
+
+// {stepperStop String}
+#define STEPPER_STOP		49
+
+// {stopService}
+#define STOP_SERVICE		50
+
+///// INO GENERATED DEFINITION END //////
+
+// ----- MRLCOMM FUNCTION GENERATED INTERFACE END -----------
+
+#define STEPPER_TYPE_SIMPLE  			1
+
+// ------ event types ------
+#define STEPPER_EVENT_STOP				1
+#define STEPPER_EVENT_STEP				2
+
+// servo event types
+#define  SERVO_EVENT_STOPPED			1
+#define  SERVO_EVENT_POSITION_UPDATE 	2
+
+// error types
+#define ERROR_SERIAL					1
+#define ERROR_UNKOWN_CMD				2
+#define ERROR_ALREADY_EXISTS			3
+#define ERROR_DOES_NOT_EXIST			4
+
+// ------ error types ------
+#define SENSOR_ULTRASONIC				1
+
+#define CUSTOM_MSG						50
+
+// need a method to identify type of board
+// http://forum.arduino.cc/index.php?topic=100557.0
+
+#define COMMUNICATION_RESET	   252
+#define SOFT_RESET			   253
+#define NOP  				   255
+
+// ----------  MRLCOMM FUNCTION INTERFACE END -----------
+
+// MAX definitions
+// MAX_SERVOS defined by boardtype/library
+#define PINGDARS_MAX		6
+#define SENSORS_MAX			12
+#define STEPPERS_MAX		6
+
+#define MAX_MOTORS 10
+
+#define ECHO_STATE_START 1
+#define ECHO_STATE_TRIG_PULSE_BEGIN 2
+#define ECHO_STATE_TRIG_PULSE_END 3
+#define ECHO_STATE_MIN_PAUSE_PRE_LISTENING 4
+#define ECHO_STATE_LISTENING 5
+#define ECHO_STATE_GOOD_RANGE	6
+#define ECHO_STATE_TIMEOUT	7
+
+// --VENDOR DEFINE SECTION BEGIN--
+// --VENDOR DEFINE SECTION END--
+
+// FIXME FIXME FIXME
+// -- FIXME - modified by board type BEGIN --
+// Need Arduino to do a hardware abstraction layer
+// https://code.google.com/p/arduino/issues/detail?id=59
+// AHAAA !! - many defintions in - pins_arduino.h !!!
+// Need a "board" identifier at least !!!
+
+#define ANALOG_PIN_COUNT 16 // mega
+#define DIGITAL_PIN_COUNT 54 // mega
+// #define MAX_SERVOS 48 - is defined @ compile time !!
+// -- FIXME - modified by board type END --
+
+#define ARDUINO_TYPE_INT 16; // :) type identifier - not size - but what the hell ;)
+
+/*
+* TODO - CRC for last byte
+* getCommand - retrieves a command message
+* inbound and outbound messages are the same format, the following represents a basic message
+* format
+*
+* MAGIC_NUMBER|NUM_BYTES|FUNCTION|DATA0|DATA1|....|DATA(N)
+*              NUM_BYTES - is the number of bytes after NUM_BYTES to the end
+*
+*/
+
+int msgSize = 0; // the NUM_BYTES of current message
+
+unsigned int debounceDelay = 50; // in ms
+long lastDebounceTime[DIGITAL_PIN_COUNT];
+byte msgBuf[64];
+
+typedef struct
+  {
+      int type;
+      int trigPin;
+      int echoPin;
+      bool isRunning;
+      int timeoutUS;
+      unsigned long ts;
+      unsigned long lastValue;
+      int state;
+      //NewPing* ping;
+  }  sensor_type;
+
+sensor_type sensors[SENSORS_MAX];
+
+typedef struct
+  {
+  	  int ts;
+      int type;
+      int index; // is this redundant?
+      int currentPos;
+      int targetPos;
+      int speed;
+
+      int dirPin;
+      int stepPin;
+
+	  // support up to 5 wire steppers
+      int pin0;
+      int pin1;
+      int pin2;
+      int pin3;
+      int pin4;
+
+  }  stepper_type;
+
+stepper_type steppers[STEPPERS_MAX];
+
+// Servos
+typedef struct
+{
+    Servo* servo;
+    int index; // index of this servo
+    int speed;
+    int targetPos;
+    int currentPos;
+    bool isMoving;
+
+    int step; // affects speed usually 1
+
+    // sweep related
+    int min;
+    int max;
+    // int delay; - related to speed
+    int increment;
+    bool isSweeping;
+
+    // event related
+    bool eventsEnabled;
+} servo_type;
+
+
+servo_type servos[MAX_SERVOS];
+
+// Motors
+typedef struct
+{
+    int index; // index of this motor
+    int speed;
+    int targetPos;
+    int currentPos;
+    bool isMoving;
+
+    int step; // affects speed usually 1
+
+    // sweep related
+    int min;
+    int max;
+    // int delay; - related to speed
+    int increment;
+    bool isSweeping;
+
+    // event related
+    bool eventsEnabled;
+} motor_type;
+
+motor_type motors[MAX_MOTORS];
+
+unsigned long loopCount     = 0;
+unsigned long lastMicros 	= 0;
+int byteCount               = 0;
+unsigned char newByte 		= 0;
+unsigned char ioCmd[64];  // message buffer for all inbound messages
+int readValue;
+
+// FIXME - normalize with sampleRate ..
+int loadTimingModulus = 1000;
+
+boolean loadTimingEnabled = false;
+unsigned long loadTime = 0;
+// TODO - avg load time
+
+unsigned int sampleRate = 1; // 1 - 65,535 modulus of the loopcount - allowing you to sample less
+
+int digitalReadPin[DIGITAL_PIN_COUNT];        // array of pins to read from
+int digitalReadPollingPinCount = 0;           // number of pins currently reading
+int lastDigitalInputValue[DIGITAL_PIN_COUNT]; // array of last input values
+bool digitalTriggerOnly	= false;      // send data back only if its different
+
+int analogReadPin[ANALOG_PIN_COUNT];          // array of pins to read from
+int analogReadPollingPinCount = 0;            // number of pins currently reading
+int lastAnalogInputValue[ANALOG_PIN_COUNT];   // array of last input values
+bool analogTriggerOnly = false;         // send data back only if its different
+
+//---- data record definitions begin -----
+
+
+// TODO - all well and good .. but you dont control Servo's data (yet)
+// should have a struct for it too - contains all the data info you'd want to have
+// in a servo - same with stepper
+
+typedef struct
+  {
+      int servoIndex; // id of servo in servos array
+      //int servoPos; in servo
+      int sensorIndex;
+      int sweepMin;
+      int sweepMax;
+      int step;
+      bool isRunning; // needed ? - is combo of two
+  }  pingdar_type;
+
+pingdar_type pingdars[PINGDARS_MAX];
+//===custom msg interface begin===
+	byte customParams[256];
+	int paramBuffIndex;
+	int paramCnt;
+//===custom msg interface end===
+
+void sendServoEvent(servo_type& s, int eventType);
+void sendStepperEvent(stepper_type& s, int eventType);
+unsigned long getUltrasonicRange(sensor_type& sensor);
+// void sendMsg ( int num, ... );
+
+//---- data record definitions end -----
+
+void append(const int& data) {
+	++paramCnt;
+	customParams[paramBuffIndex] = ARDUINO_TYPE_INT;
+	customParams[++paramBuffIndex] = (byte)(data >> 8);
+	customParams[++paramBuffIndex] = ((byte) data & 0xff);
+	++paramBuffIndex;
+}
+
+void startMsg() {
+}
+
+void setup() {
+	Serial.begin(57600);        // connect to the serial port
+
+	softReset();
+
+	// --VENDOR SETUP BEGIN--
+	// --VENDOR SETUP END--
+}
+
+
+void sendMsg(){
+
+	// unbox
+	Serial.write(MAGIC_NUMBER);
+	Serial.write(paramBuffIndex + 2); // = param buff size + FN + paramCnt
+	//Serial.write(2); // = param buff size + FN + paramCnt
+	Serial.write(CUSTOM_MSG);
+	Serial.write(paramCnt);
+
+	for (int i = 0; i < paramBuffIndex; ++i){
+		Serial.write(customParams[i]);
+	}
+
+	paramCnt = 0;
+	paramBuffIndex = 0;
+}
+
+
+
+void softReset()
+{
+	for (int i = 0; i < MAX_SERVOS - 1; ++i)
+	{
+                servo_type& s = servos[i];
+		s.speed = 100;
+                if (s.servo != 0){
+		  s.servo->detach();
+                }
+	}
+
+	for (int j = 0; j < DIGITAL_PIN_COUNT - 1; ++j)
+	{
+		pinMode(j, OUTPUT);
+	}
+
+
+	digitalReadPollingPinCount = 0;
+	analogReadPollingPinCount = 0;
+	loopCount = 0;
+
+}
+
+/*
+void setPWMFrequency (int address, int prescalar)
+{
+	int clearBits = 0x07;
+	if (address == 0x25)
+	{
+		TCCR0B &= ~clearBits;
+		TCCR0B |= prescalar;
+	} else if (address == 0x2E)
+	{
+		TCCR1B &= ~clearBits;
+		TCCR1B |= prescalar;
+	} else if (address == 0xA1)
+	{
+		TCCR2B &= ~clearBits;
+		TCCR2B |= prescalar;
+	}
+
+}
+*/
+/**
+ * checks the existance of the searched value in the array
+ * - good for not adding to a dynamic list of values if it
+ * already exists
+ */
+bool exists(int array[], int len, int searchValue){
+	for (int i = 0; i < len; ++i)
+	{
+		if (searchValue == array[i])
+		{
+			return true;
+		}
+	}
+	return false;
+}
+
+/**
+ * adds new value to a pseudo dynamic array/list
+ * if successful - if value already exists on list
+ * sends back an error
+ */
+bool addNewValue(int array[], int& len, int addValue)
+{
+	if (!exists(array, len, addValue)){
+		array[len] = addValue;
+		++len;
+		return true;
+	} else {
+		sendError(ERROR_ALREADY_EXISTS);
+		return false;
+	}
+}
+
+
+bool removeAndShift (int array [], int& len, int removeValue)
+{
+	if (!exists(array, len, removeValue)){
+		sendError(ERROR_DOES_NOT_EXIST);
+		return false;
+	}
+
+	int pos = -1;
+
+	if (len == 0)
+	{
+		// "should" never happen
+		// would be calling remove on an empty list
+		// the error ERROR_DOES_NOT_EXIST - "should" be called
+		return true;
+	}
+
+	// find position of value
+	for (int i = 0; i < len; ++i)
+	{
+		if (removeValue == array[i])
+		{
+			pos = i;
+			break;
+		}
+	}
+	// if at the end just decrement size
+	if (pos == len - 1)
+	{
+		--len;
+		return true;
+	}
+
+	// if found somewhere else shift left
+	if (pos < len && pos > -1)
+	{
+		for (int j = pos; j < len - 1; ++j)
+		{
+			array[j] = array[j+1];
+		}
+		--len;
+	}
+
+	return true;
+}
+
+boolean getCommand ()
+{
+	// handle serial data begin
+	if (Serial.available() > 0)
+	{
+		// read the incoming byte:
+		newByte = Serial.read();
+		++byteCount;
+
+		// checking first byte - beginning of message?
+		if (byteCount == 1 && newByte != MAGIC_NUMBER)
+		{
+			sendError(ERROR_SERIAL);
+
+			// reset - try again
+			byteCount = 0;
+			return false;
+		}
+
+		if (byteCount == 2)
+		{
+		   // get the size of message
+		   // todo check msg < 64 (MAX_MSG_SIZE)
+		   msgSize = newByte;
+		}
+
+		if (byteCount > 2) {
+		  // fill in msg data - (2) headbytes -1 (offset)
+		  ioCmd[byteCount - 3] = newByte;
+		}
+
+		// if received header + msg
+		if (byteCount == 2 + msgSize)
+		{
+          return true;
+		}
+	} // if Serial.available
+
+	return false;
+}
+
+void loop () {
+
+	++loopCount;
+
+	if (getCommand())
+	{
+		switch (ioCmd[0])
+		{
+
+		case DIGITAL_WRITE:{
+			digitalWrite(ioCmd[1], ioCmd[2]);
+			break;
+		}
+
+		case ANALOG_WRITE:{
+			analogWrite(ioCmd[1], ioCmd[2]);
+			break;
+		}
+
+		case PIN_MODE:{
+			pinMode(ioCmd[1], ioCmd[2]);
+			break;
+		}
+
+		case SERVO_ATTACH:{
+			servo_type& s = servos[ioCmd[1]];
+			s.index = ioCmd[1];
+			if (s.servo == NULL){
+				s.servo = new Servo();
+			}
+			s.servo->attach(ioCmd[2]);
+			s.step = 1;
+			s.eventsEnabled = false;
+			break;
+		}
+
+		case SERVO_SWEEP_START:{
+			servo_type& s = servos[ioCmd[1]];
+			s.min = ioCmd[2];
+			s.max = ioCmd[3];
+			s.step = ioCmd[4];
+			s.isMoving = true;
+			s.isSweeping = true;
+			break;
+		}
+
+		case SERVO_SWEEP_STOP:{
+			servo_type& s = servos[ioCmd[1]];
+			s.isMoving = false;
+			s.isSweeping = false;
+			break;
+		}
+
+		case SERVO_WRITE:{
+			servo_type& s = servos[ioCmd[1]];
+			if (s.speed == 100 && s.servo != 0)// move at regular/full 100% speed
+			{
+				s.targetPos = ioCmd[2];
+				s.currentPos = ioCmd[2];
+				s.isMoving = false;
+				s.servo->write(ioCmd[2]);
+				if (s.eventsEnabled) sendServoEvent(s, SERVO_EVENT_STOPPED);
+			} else if (s.speed < 100 && s.speed > 0) {
+				s.targetPos = ioCmd[2];
+				s.isMoving = true;
+			}
+			break;
+		}
+
+		case PUBLISH_SERVO_EVENT:{
+			servo_type& s = servos[ioCmd[1]];
+			s.eventsEnabled = ioCmd[2];
+			break;
+		}
+
+		case SET_LOAD_TIMING_ENABLED:{
+			loadTimingEnabled = ioCmd[1];
+			//loadTimingModulus = ioCmd[2];
+			loadTimingModulus = 1000;
+			break;
+		}
+
+		case SERVO_WRITE_MICROSECONDS:{
+			// TODO - incorporate into speed control etc
+			// normalize - currently by itself doesn't effect events
+			// nor is it involved in speed control
+			servo_type& s = servos[ioCmd[1]];
+			if (s.servo != 0) {
+				// 1500 midpoint
+				s.servo->writeMicroseconds(ioCmd[2]);
+			}
+
+			break;
+		}
+
+		case SET_SERVO_SPEED:{
+			// setting the speed of a servo
+			servo_type& servo = servos[ioCmd[1]];
+			servo.speed = ioCmd[2];
+			break;
+		}
+
+		case SERVO_DETACH:{
+			servo_type& s = servos[ioCmd[1]];
+			if (s.servo != 0){
+			  s.servo->detach();
+			}
+			break;
+		}
+/*
+		case SET_PWMFREQUENCY:{
+			setPWMFrequency (ioCmd[1], ioCmd[2]);
+			break;
+		}
+*/
+
+		case ANALOG_READ_POLLING_START:{
+			int pin = ioCmd[1];
+			addNewValue(analogReadPin, analogReadPollingPinCount, pin);
+			break;
+		}
+
+		case ANALOG_READ_POLLING_STOP:{
+			int pin = ioCmd[1];
+			removeAndShift(analogReadPin, analogReadPollingPinCount, pin);
+			break;
+		}
+
+		case DIGITAL_READ_POLLING_START:{
+			int pin = ioCmd[1];
+			addNewValue(digitalReadPin, digitalReadPollingPinCount, pin);
+			break;
+		}
+
+		case DIGITAL_READ_POLLING_STOP:{
+			int pin = ioCmd[1];
+			removeAndShift(digitalReadPin, digitalReadPollingPinCount, pin);
+			break;
+		}
+
+		case SET_TRIGGER:{
+                        // FIXME !!! - you need 1. a complete pin list !!!   analog & digital should be defined by attribute not
+                        // data structure !!!  if (pin.type == ??? if needed
+			// TODO - if POLLING ALREADY DON'T RE-ADD - MAKE RE-ENTRANT
+			analogReadPin[analogReadPollingPinCount] = ioCmd[1]; // put on polling read list
+			++analogReadPollingPinCount;
+			break;
+		}
+
+		case SET_DEBOUNCE:{
+			// default debounceDelay = 50;
+			debounceDelay = ((ioCmd[1]<<8) + ioCmd[2]);
+			break;
+		}
+
+		case SET_DIGITAL_TRIGGER_ONLY:{
+			digitalTriggerOnly = ioCmd[1];
+			break;
+                }
+
+		case SET_SERIAL_RATE:
+		{
+			Serial.end();
+			delay(500);
+			Serial.begin(ioCmd[1]);
+			break;
+		}
+
+		case GET_VERSION:{
+			Serial.write(MAGIC_NUMBER);
+			Serial.write(2); // size
+			Serial.write(PUBLISH_VERSION);
+			Serial.write((byte)MRLCOMM_VERSION);
+			break;
+			}
+
+		case PULSE_IN: {
+		    // might need to hack the pulseIn lib - but would
+		    // like to do it without delay
+			// http://arduino.cc/en/Tutorial/BlinkWithoutDelay
+			int trigPin = ioCmd[1];
+			int echoPin = ioCmd[2];
+			// TODO - implement HI/LOW value & timeout & variable delay for trigger
+
+			pinMode(trigPin, OUTPUT);
+			pinMode(echoPin, INPUT);
+
+			digitalWrite(trigPin, LOW);
+			delayMicroseconds(2);
+
+			digitalWrite(trigPin, HIGH);
+			delayMicroseconds(10);
+
+			digitalWrite(trigPin, LOW);
+			unsigned long duration = pulseIn(echoPin, HIGH);
+
+			//Calculate the distance (in cm) based on the speed of sound.
+			// distance = duration/58.2;
+
+			//sendMsg(4, ANALOG_VALUE, analogReadPin[i], readValue >> 8, readValue & 0xff);
+ 			//sendMsg(5, PULSE_IN, duration >> 24, duration >> 16, duration >> 8, duration & 0xff);
+ 			//sendMsg(6, SENSOR_DATA, 47, duration >> 24, duration >> 16, duration >> 8, duration & 0xff);
+
+
+			Serial.write(MAGIC_NUMBER);
+			Serial.write(5); // size 1 FN + 4 bytes of unsigned long
+			Serial.write(PULSE_IN);
+            // write the long value out
+			Serial.write((byte)(duration >> 24));
+			Serial.write((byte)(duration >> 16));
+			Serial.write((byte)(duration >> 8));
+			Serial.write((byte)duration & 0xff);
+
+
+			break;
+		}
+
+		case SET_SAMPLE_RATE:{
+			// 2 byte int - valid range 1-65,535
+			sampleRate = (ioCmd[1]<<8) + ioCmd[2];
+			if (sampleRate == 0)
+				{ sampleRate = 1; } // avoid /0 error - FIXME - time estimate param
+			break;
+		}
+
+		case SOFT_RESET:{
+			softReset();
+			break;
+		}
+
+		case STEPPER_ATTACH:{
+			stepper_type& stepper = steppers[ioCmd[1]];
+			stepper.index = ioCmd[1];
+			stepper.type = ioCmd[2];
+			stepper.currentPos = 0;
+			stepper.targetPos = 0;
+			stepper.speed = 100;
+
+			if (stepper.type == STEPPER_TYPE_SIMPLE) {
+				stepper.dirPin = ioCmd[3]; // dir pin
+				stepper.stepPin = ioCmd[4]; // step pin
+				pinMode(stepper.dirPin, OUTPUT);
+				pinMode(stepper.stepPin, OUTPUT);
+			} else {
+				sendError(ERROR_UNKOWN_CMD);
+			}
+			break;
+		}
+
+		case STEPPER_RESET:{
+			stepper_type& stepper = steppers[ioCmd[1]];
+			stepper.currentPos = 0;
+			stepper.targetPos = 0;
+			stepper.speed = 100;
+			break;
+		}
+
+		/* absolute position - not relative */
+		case STEPPER_MOVE_TO:{
+			stepper_type& stepper = steppers[ioCmd[1]];
+			if (stepper.type == STEPPER_TYPE_SIMPLE) {
+				stepper.targetPos = stepper.currentPos + (ioCmd[2]<<8) + ioCmd[3];
+				// relative position & direction
+				if (stepper.targetPos < 0) {
+					// direction
+					digitalWrite(stepper.dirPin, 1);
+				} else {
+					digitalWrite(stepper.dirPin, 0);
+				}
+			} else {
+				sendError(ERROR_UNKOWN_CMD);
+			}
+			break;
+		}
+
+		case STEPPER_STOP:{
+			stepper_type& stepper = steppers[ioCmd[1]];
+			if (stepper.type == STEPPER_TYPE_SIMPLE) {
+				stepper.targetPos = stepper.currentPos;
+				sendStepperEvent(stepper, STEPPER_EVENT_STOP);
+			} else {
+				sendError(ERROR_UNKOWN_CMD);
+			}
+			break;
+		}
+
+			// --VENDOR CODE BEGIN--
+			// --VENDOR CODE END--
+// not sure if its worth implementing - same info can be retrieved from publish servo event & publish pin
+/*
+
+		case PINGDAR_ATTACH:{
+			int pingdarIndex = ioCmd[1];
+			pingdar_type& pingdar = pingdars[pingdarIndex];
+			pingdar.servoIndex = ioCmd[2];
+			pingdar.sensorIndex = ioCmd[3];
+			pingdar.step = 1;
+			break;
+		}
+*/
+		// used by ultrasonic sensor
+		case SENSOR_ATTACH:{
+			int sensorIndex = ioCmd[1];
+			sensor_type& sensor = sensors[sensorIndex];
+			sensor.type = ioCmd[2];
+
+
+
+			// initialize based on sensor type
+			if (sensor.type == SENSOR_ULTRASONIC){
+				sensor.trigPin = ioCmd[3];
+				sensor.echoPin = ioCmd[4];
+				pinMode(sensor.trigPin, OUTPUT);
+				pinMode(sensor.echoPin, INPUT);
+				//sensor.ping = new NewPing(sensor.trigPin, sensor.echoPin, 100);
+			}
+
+			break;
+		}
+
+		case SENSOR_POLLING_START:{
+			int sensorIndex = ioCmd[1];
+			sensor_type& sensor = sensors[sensorIndex];
+			sensor.isRunning = true;
+
+			// I'm used to ms - and would need to change some
+			// interfaces if i was to support inbound longs
+			//sensor.timeoutUS = ioCmd[2] * 1000;
+			sensor.timeoutUS = 20000; // 20 ms
+			sensor.state = ECHO_STATE_START;
+
+			break;
+		}
+
+		case SENSOR_POLLING_STOP:{
+			int sensorIndex = ioCmd[1];
+			sensor_type& sensor = sensors[sensorIndex];
+			sensor.isRunning = false;
+			break;
+		}
+
+		case NOP:{
+			// No Operation
+			break;
+		}
+
+		default:{
+		    sendError(ERROR_UNKOWN_CMD);
+			break;
+		}
+		} // end switch
+
+		// reset buffer
+		memset(ioCmd,0,sizeof(ioCmd));
+		byteCount = 0;
+
+	} // if getCommand()
+
+	// all reads are affected by sample rate
+	if (loopCount%sampleRate == 0) {
+		// digital polling read - send data for pins which are currently in INPUT mode only AND whose state has changed
+		for (int i  = 0; i < digitalReadPollingPinCount; ++i)
+		{
+			if (debounceDelay)
+			{
+			  if (millis() - lastDebounceTime[digitalReadPin[i]] < debounceDelay)
+			  {
+			    continue;
+			  }
+			}
+
+			// FIXME !! - normalize with analog
+			// read the pin
+			readValue = digitalRead(digitalReadPin[i]);
+
+			// if my value is different from last time  && config - send it
+			if (lastDigitalInputValue[digitalReadPin[i]] != readValue  || !digitalTriggerOnly)
+			{
+				Serial.write(MAGIC_NUMBER);
+				Serial.write(4); // size
+				Serial.write(PUBLISH_PIN);
+				Serial.write(digitalReadPin[i]);// Pin#
+				Serial.write((byte)0); 	// MSB
+				Serial.write(readValue); 	// LSB
+
+			    lastDebounceTime[digitalReadPin[i]] = millis();
+			}
+
+			// set the last input value of this pin
+			lastDigitalInputValue[digitalReadPin[i]] = readValue;
+		}
+
+
+		// analog polling read - send data for pins which are currently in INPUT mode only AND whose state has changed
+		for (int i  = 0; i < analogReadPollingPinCount; ++i)
+		{
+			// read the pin
+			readValue = analogRead(analogReadPin[i]);
+
+			// if my value is different from last time - send it
+			if (lastAnalogInputValue[analogReadPin[i]] != readValue   || !analogTriggerOnly) //TODO - SEND_DELTA_MIN_DIFF
+			{
+				//sendMsg(4, ANALOG_VALUE, analogReadPin[i], readValue >> 8, readValue & 0xff);
+
+				Serial.write(MAGIC_NUMBER);
+				Serial.write(4); //size
+				Serial.write(PUBLISH_PIN);
+				Serial.write(analogReadPin[i]);
+				Serial.write(readValue >> 8);   // MSB
+				Serial.write(readValue & 0xff);	// LSB
+
+	        }
+			// set the last input value of this pin
+			lastAnalogInputValue[analogReadPin[i]] = readValue;
+		}
+	}
+
+	// update moving servos - send events if required
+	for (int i = 0; i < MAX_SERVOS; ++i)
+	{
+		servo_type& s = servos[i];
+		if (s.isMoving && s.servo != 0){
+			if (s.currentPos != s.targetPos)
+			{
+				// caclulate the appropriate modulus to drive
+				// the servo to the next position
+				// TODO - check for speed > 0 && speed < 100 - send ERROR back?
+				int speedModulus = (100 - s.speed) * 10;
+				if (loopCount % speedModulus == 0)
+				{
+					int increment = s.step * ((s.currentPos < s.targetPos)?1:-1);
+					// move the servo an increment
+					s.currentPos = s.currentPos + increment;
+					s.servo->write(s.currentPos);
+					if (s.eventsEnabled) sendServoEvent(s, SERVO_EVENT_POSITION_UPDATE);
+				}
+			} else {
+				if (s.isSweeping) {
+					if (s.targetPos == s.min){
+						s.targetPos = s.max;
+					} else {
+						s.targetPos = s.min;
+					}
+				} else {
+					if (s.eventsEnabled) sendServoEvent(s, SERVO_EVENT_STOPPED);
+					s.isMoving = false;
+				}
+			}
+		}
+	}
+
+	unsigned long ts;
+
+	for (int i = 0; i < SENSORS_MAX; ++i) {
+		sensor_type& sensor = sensors[i];
+		if (sensor.isRunning == true){
+			if (sensor.type == SENSOR_ULTRASONIC){
+
+				// we are running & have an ultrasonic (ping) sensor
+				// check to see what state we  are in
+
+				if (sensor.state == ECHO_STATE_START){
+					// trigPin prepare - start low for an
+					// upcoming high pulse
+					pinMode(sensor.trigPin, OUTPUT);
+					digitalWrite(sensor.trigPin, LOW);
+
+					// put the echopin into a high state
+					// is this necessary ???
+					pinMode(sensor.echoPin, OUTPUT);
+					digitalWrite(sensor.echoPin, HIGH);
+
+					ts = micros();
+					if (ts - sensor.ts > 2){
+						sensor.ts = ts;
+						sensor.state = ECHO_STATE_TRIG_PULSE_BEGIN;
+					}
+				} else if (sensor.state == ECHO_STATE_TRIG_PULSE_BEGIN){
+
+					// begin high pulse for at least 10 us
+					pinMode(sensor.trigPin, OUTPUT);
+					digitalWrite(sensor.trigPin, HIGH);
+
+					ts = micros();
+					if (ts - sensor.ts > 10){
+						sensor.ts = ts;
+						sensor.state = ECHO_STATE_TRIG_PULSE_END;
+					}
+				} else if (sensor.state == ECHO_STATE_TRIG_PULSE_END){
+					// end of pulse
+					pinMode(sensor.trigPin, OUTPUT);
+					digitalWrite(sensor.trigPin, LOW);
+
+					sensor.state = ECHO_STATE_MIN_PAUSE_PRE_LISTENING;
+					sensor.ts = micros();
+				} else if (sensor.state == ECHO_STATE_MIN_PAUSE_PRE_LISTENING){
+
+					ts = micros();
+					if (ts - sensor.ts > 1500){
+						sensor.ts = ts;
+
+						// putting echo pin into listen mode
+						pinMode(sensor.echoPin, OUTPUT);
+						digitalWrite(sensor.echoPin, HIGH);
+						pinMode(sensor.echoPin, INPUT);
+
+						sensor.state = ECHO_STATE_LISTENING;
+					}
+
+				} else if (sensor.state == ECHO_STATE_LISTENING) {
+					// timeout or change states..
+					int value = digitalRead(sensor.echoPin);
+					ts = micros();
+
+					if (value == LOW) {
+						sensor.lastValue = ts - sensor.ts;
+						sensor.ts = ts;
+						sensor.state = ECHO_STATE_GOOD_RANGE;
+					} else if (ts - sensor.ts > sensor.timeoutUS) {
+						sensor.state = ECHO_STATE_TIMEOUT;
+						sensor.ts = ts;
+						sensor.lastValue = 0;
+					}
+
+				} else if (sensor.state == ECHO_STATE_GOOD_RANGE || sensor.state == ECHO_STATE_TIMEOUT) {
+					Serial.write(MAGIC_NUMBER);
+					Serial.write(6); // size 1 FN + 4 bytes of unsigned long
+					Serial.write(PUBLISH_SESOR_DATA);
+					Serial.write(i);
+		            // write the long value out
+					Serial.write((byte)(sensor.lastValue >> 24));
+					Serial.write((byte)(sensor.lastValue >> 16));
+					Serial.write((byte)(sensor.lastValue >> 8));
+					Serial.write((byte) sensor.lastValue & 0xff);
+					sensor.state = ECHO_STATE_START;
+				} // end else if
+
+			} // if (sensor.type == SENSOR_ULTRASONIC)
+
+		} // end isRunning
+
+	} // end for each sensor
+
+	// TODO - brake - speed - fractional stepping - other stepper types
+	for (int i = 0; i < STEPPERS_MAX; ++i) {
+		stepper_type& stepper = steppers[i];
+		if (stepper.currentPos != stepper.targetPos){
+			if (stepper.type == STEPPER_TYPE_SIMPLE){
+
+				// direction is already set in initial STEPPER_MOVE
+
+				if (stepper.currentPos < stepper.targetPos) {
+					digitalWrite(stepper.stepPin, 1);
+					delayMicroseconds(1); // :P should require another state? loop is ~106us min ?
+					digitalWrite(stepper.stepPin, 0);
+					stepper.currentPos++;
+					sendStepperEvent(stepper, STEPPER_EVENT_STEP);
+
+				} else if (stepper.currentPos > stepper.targetPos) {
+					digitalWrite(stepper.stepPin, 1);
+					delayMicroseconds(1); // :P should require another state? loop is ~106us min ?
+					digitalWrite(stepper.stepPin, 0);
+					stepper.currentPos--;
+					sendStepperEvent(stepper, STEPPER_EVENT_STEP);
+				}
+
+				if (stepper.currentPos == stepper.targetPos){
+					sendStepperEvent(stepper, STEPPER_EVENT_STOP);
+				}
+
+			}
+		}
+	}
+
+	// FIXME - fix overflow with getDiff() method !!!
+	unsigned long now = micros();
+	loadTime = now - lastMicros; // avg outside
+ 	lastMicros = now;
+
+	// report load time
+	if (loadTimingEnabled && (loopCount%loadTimingModulus == 0)) {
+
+ 		// send it
+		Serial.write(MAGIC_NUMBER);
+		Serial.write(5); // size 1 FN + 4 bytes of unsigned long
+		Serial.write(PUBLISH_LOAD_TIMING_EVENT);
+        // write the long value out
+		Serial.write((byte)(loadTime >> 24));
+		Serial.write((byte)(loadTime >> 16));
+		Serial.write((byte)(loadTime >> 8));
+		Serial.write((byte) loadTime & 0xff);
+	}
+
+
+} // end of big loop
+
+unsigned long getUltrasonicRange(sensor_type& sensor){
+
+		// added for sensors which have single pin !
+		pinMode(sensor.trigPin, OUTPUT);
+		digitalWrite(sensor.trigPin, LOW);
+		delayMicroseconds(2);
+
+		digitalWrite(sensor.trigPin, HIGH);
+		delayMicroseconds(10);
+
+		digitalWrite(sensor.trigPin, LOW);
+
+		// added for sensors which have single pin !
+		pinMode(sensor.echoPin, INPUT);
+		// CHECKING return pulseIn(sensor.echoPin, HIGH, sensor.timeoutUS);
+		// TODO - adaptive timeout ? - start big - pull in until valid value - push out if range is coming close
+		return pulseIn(sensor.echoPin, HIGH);
+}
+
+void sendServoEvent(servo_type& s, int eventType){
+  	// check type of event - STOP vs CURRENT POS
+
+	Serial.write(MAGIC_NUMBER);
+	Serial.write(5); // size = 1 FN + 1 INDEX + 1 eventType + 1 curPos
+	Serial.write(PUBLISH_SERVO_EVENT);
+	Serial.write(s.index); // send my index
+	// write the long value out
+	Serial.write(eventType);
+	Serial.write(s.currentPos);
+	Serial.write(s.targetPos);
+}
+
+void sendStepperEvent(stepper_type& s, int eventType){
+  	// check type of event - STOP vs CURRENT POS
+
+	Serial.write(MAGIC_NUMBER);
+	Serial.write(5); // size = 1 FN + 1 INDEX + 1 eventType + 1 curPos
+	Serial.write(PUBLISH_STEPPER_EVENT);
+	Serial.write(s.index); // send my index
+	// write the long value out
+	Serial.write(eventType);
+	Serial.write(s.currentPos >> 8); // msb
+	Serial.write(s.currentPos & 0xff); // lsb
+}
+
+
+void sendError(int type){
+	Serial.write(MAGIC_NUMBER);
+	Serial.write(2); // size = 1 FN + 1 TYPE
+	Serial.write(PUBLISH_MRLCOMM_ERROR);
+	Serial.write(type);
+}
+
+/* SEEMED LIKE A GOOD IDEA NOT !!!!
+void sendMsg ( int num, ... )
+{
+	va_list arguments;
+	// Initializing arguments to store all values after num
+	va_start ( arguments, num );
+
+	// write header
+	msgBuf[0] = MAGIC_NUMBER;
+	msgBuf[1] = num;
+
+	// copies msg payload to buffer after header
+	for ( int x = 2; x < num+2; x++ )
+	{
+		msgBuf[x] = (byte) va_arg ( arguments, int );
+	}
+	va_end ( arguments );                  // Cleans up the list
+	Serial.write(msgBuf, num + 2);
+	return;
+}
+*/


Arduino encoder

Arduino encoder

error: pathspec 'arduino/arduino_encoder.ino' did not match any file(s) known to git

--- arduino/arduino_encoder.ino
+++ arduino/arduino_encoder.ino
@@ -0,0 +1,893 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <math.h>
+#include <limits.h>
+
+#define SQRT2 1.4142135623730950488016887242096
+#define MAX_LPC_ORDER 98
+#define MAX_RICE_PARAM 20
+#define BLOCK_SIZE 256
+#define SHORT_MAX 32767
+
+typedef struct rice_encode_context
+{
+	uint32_t buffer;
+	uint32_t filled;
+	uint32_t bits;
+} rice_encode_context;
+
+typedef struct rice_decode_context
+{
+	uint32_t buffer;
+	uint32_t filled;
+	uint32_t bytes;
+} rice_decode_context;
+
+int32_t check_if_constant(const int16_t *data,int32_t num_elements);
+void auto_corr_fun(double *x,int32_t N,int64_t k,int16_t norm,double *rxx);
+void levinson(double *autoc,uint8_t max_order,double *ref,double lpc[][MAX_LPC_ORDER]);
+uint8_t compute_ref_coefs(double *autoc,uint8_t max_order,double *ref);
+int32_t qtz_ref_cof(double *par,uint8_t ord,int32_t *q_ref);
+int32_t dqtz_ref_cof(const int32_t *q_ref,uint8_t ord,double *ref);
+void calc_residue(const int32_t *samples,int64_t N,int16_t ord,int16_t Q,int64_t *coff,int32_t *residues);
+void calc_signal(const int32_t *residues,int64_t N,int16_t ord,int16_t Q,int64_t *coff,int32_t *samples);
+
+int32_t signed_to_unsigned(const uint32_t data_size,const int32_t *input,uint32_t *output);
+int32_t unsigned_to_signed(const uint32_t data_size,const uint32_t *input,int32_t *output);
+uint16_t get_opt_rice_param(const uint32_t *data,int32_t data_size,uint32_t *req_bits);
+uint32_t rice_encode_block(int16_t param,const uint32_t *input,int32_t size,uint32_t *encoded);
+uint32_t rice_decode_block(int16_t param,const uint32_t *encoded,int32_t count,uint32_t *decoded);
+
+//Variables
+	int8_t percent = 0;
+	uint8_t opt_lpc_order = 0;
+	int16_t channels,bps;
+	uint8_t rice_param_ref,rice_param_residue;
+	uint16_t req_int_ref,req_int_residues,samples_per_channel;
+	const int16_t Q = 35;
+	int32_t i,j,k = 0;
+	int32_t sample_rate,read_size;
+	int32_t frame_sync_count = 0;
+	uint32_t req_bits_ref,req_bits_residues;
+	uint32_t seconds,samples;
+	const int64_t corr = ((int64_t)1) << Q;
+
+	//Arrays
+	int16_t short_samples[BLOCK_SIZE];
+	int32_t qtz_ref_coeffs[MAX_LPC_ORDER];
+	int32_t int_samples[BLOCK_SIZE];
+	int32_t residues[BLOCK_SIZE];
+	int32_t spar[MAX_LPC_ORDER];
+	uint32_t unsigned_ref[MAX_LPC_ORDER];
+	uint32_t encoded_ref[MAX_LPC_ORDER];
+	uint32_t u_residues[BLOCK_SIZE];
+	uint32_t encoded_residues[BLOCK_SIZE];
+	int64_t lpc[MAX_LPC_ORDER + 1];
+	size_t written;
+	double qtz_samples[BLOCK_SIZE];
+	double autocorr[MAX_LPC_ORDER + 1];
+	double ref[MAX_LPC_ORDER];
+	double lpc_mat[MAX_LPC_ORDER][MAX_LPC_ORDER];
+
+void setup()
+{
+        //Define read size
+	samples_per_channel = BLOCK_SIZE;
+	for(i = 0; i < samples_per_channel; i++)
+		short_samples[i] = sin(i) * SHORT_MAX;
+
+        Serial.begin(9600);
+        while (!Serial)
+        {}
+}
+
+void loop()
+{
+  	//Main loop
+	//while(1)
+	{
+
+		//Quantize sample data
+		for(j = 0; j < samples_per_channel; j++)
+			qtz_samples[j] = ((double)short_samples[j])/SHORT_MAX;
+
+		//Calculate autocorrelation data
+		auto_corr_fun(qtz_samples,samples_per_channel,MAX_LPC_ORDER,1,
+			autocorr);
+
+		//Calculate reflection coefficients
+		opt_lpc_order = compute_ref_coefs(autocorr,MAX_LPC_ORDER,ref);
+
+		//Quantize reflection coefficients
+		qtz_ref_cof(ref,opt_lpc_order,qtz_ref_coeffs);
+
+		//signed to unsigned
+		signed_to_unsigned(opt_lpc_order,qtz_ref_coeffs,unsigned_ref);
+
+		//get optimum rice param and number of bits
+		rice_param_ref = get_opt_rice_param(unsigned_ref,opt_lpc_order,
+			&req_bits_ref);
+                fprintf(stdout,"%d\n",req_bits_ref);
+
+		//Encode ref coeffs
+		req_bits_ref = rice_encode_block(rice_param_ref,unsigned_ref,
+			opt_lpc_order,encoded_ref);
+                fprintf(stdout,"%d\n",req_bits_ref);
+
+		//Determine number of ints required for storage
+		req_int_ref = ceil((double)(req_bits_ref)/(32));
+
+		//Dequantize reflection
+		dqtz_ref_cof(qtz_ref_coeffs,opt_lpc_order,ref);
+
+		//Reflection to lpc
+		levinson(NULL,opt_lpc_order,ref,lpc_mat);
+		lpc[0] = 0;
+		for(j = 0; j < opt_lpc_order; j++)
+			lpc[j + 1] = corr * lpc_mat[opt_lpc_order - 1][j];
+
+		for(j = opt_lpc_order; j < MAX_LPC_ORDER; j++)
+			lpc[j + 1] = 0;
+
+		//Copy samples
+		for(j = 0; j < samples_per_channel; j++)
+			int_samples[j] = short_samples[j];
+
+		//Get residues
+		calc_residue(int_samples,samples_per_channel,opt_lpc_order,Q,lpc,
+			residues);
+
+		//signed to unsigned
+		signed_to_unsigned(samples_per_channel,residues,u_residues);
+
+		//get optimum rice param and number of bits
+		rice_param_residue = get_opt_rice_param(u_residues,samples_per_channel,&req_bits_residues);
+
+		//Encode residues
+		//rice_encode_block(rice_param_residue,u_residues,samples_per_channel,encoded_residues);
+
+		//
+	}
+        Serial.println("Frame");
+}
+
+/*
+*Golomb-Rice algorithm is optimized to work with unsigned numbers
+*This function maps all positive numbers to even postive numbers
+*and negative numbers to odd positive numbers
+*/
+int32_t signed_to_unsigned(const uint32_t data_size,const int32_t *input,uint32_t *output)
+{
+	for(uint32_t i = 0; i < data_size; i++)
+		(input[i] < 0) ? (output[i] = (-(input[i] << 1)) - 1) : (output[i] = input[i] << 1);
+	return(0);
+}
+
+/*
+*This function does exactly the reverse of the above function
+*Maps all odd integers to negative numbers
+*and even integers to positive numbers
+*/
+int32_t unsigned_to_signed(const uint32_t data_size,const uint32_t *input,int32_t *output)
+{
+	for(uint32_t i = 0; i < data_size; i++)
+		((input[i] & 0x01) == 0x01) ? (output[i] = -((input[i] + 1) >> 1)) : (output[i] = input[i] >> 1);
+	return(0);
+}
+
+/*calculate optimum rice parameter for encoding a block of data*/
+uint16_t get_opt_rice_param(const uint32_t *data,int32_t data_size,uint32_t *req_bits)
+{
+	int32_t i = 0,j;
+	uint32_t bits[MAX_RICE_PARAM];
+	uint32_t temp;
+	uint16_t best_index;
+
+	for(i = 0; i < MAX_RICE_PARAM; i++)
+	{
+		temp = 0;
+		for(j = 0; j < data_size; j++)
+		{
+			//data[j]/2^k;
+			temp += data[j] >> i;
+			temp += 1;
+			temp += i;
+		}
+		bits[i] = temp;
+	}
+
+	//Get the minimum parameter
+	temp = bits[0];
+	best_index = 0;
+	*req_bits = bits[0];
+
+	for(i = 1; i < MAX_RICE_PARAM; i++)
+	{
+		if(bits[i] < temp)
+		{
+			temp = bits[i];
+			*req_bits = bits[i];
+			best_index = (int16_t)i;
+		}
+	}
+	return best_index;
+}
+
+/*
+*Encode a block of data using Golomb-Rice coding
+*For more details visit
+*http://michael.dipperstein.com/rice/index.html
+*/
+uint32_t rice_encode_block(int16_t param,const uint32_t *input,int32_t size,uint32_t *encoded)
+{
+	int32_t i,j,temp,written = 0;
+	uint32_t bits;
+
+	rice_encode_context context;
+	rice_encode_context *ctx = &context;
+
+	ctx->buffer = 0;
+	ctx->filled = 0;
+	ctx->bits = 0;
+
+	for(i = 0; i < size; i++)
+	{
+		//temp = floor(input[i]/2^param)
+		temp = input[i] >> param;
+
+		//Write out 'temp' number of ones
+		for(j = 0; j < temp; j++)
+		{
+			ctx->buffer = ctx->buffer | (0x0001 << ctx->filled);
+			ctx->bits++;
+			if(ctx->filled == 31)
+			{
+				encoded[written++] = ctx->buffer;
+				ctx->buffer = 0;
+				ctx->filled = 0;
+			}
+			else
+				ctx->filled++;
+		}
+
+		//Write out a zero bit
+		ctx->buffer = ctx->buffer | (0x0000 << ctx->filled);
+		ctx->bits++;
+		if(ctx->filled == 31)
+		{
+			encoded[written++] = ctx->buffer;
+			ctx->buffer = 0;
+			ctx->filled = 0;
+		}
+		else
+			ctx->filled++;
+
+		//Write out the last 'param' bits of input[i]
+		for (j = param - 1; j >= 0; j--)
+		{
+			ctx->buffer = ctx->buffer | (((input[i] >> j) & 0x0001) << ctx->filled);
+			ctx->bits++;
+			if(ctx->filled == 31)
+			{
+				encoded[written++] = ctx->buffer;
+				ctx->buffer = 0;
+				ctx->filled = 0;
+			}
+			else
+				ctx->filled++;
+		}
+	}
+
+	//Flush buffer if not empty
+	if(ctx->filled != 0)
+		encoded[written] = ctx->buffer;
+
+	bits = ctx->bits;
+	return(bits);
+}
+
+/*Decode a block of data encoded using Golomb-Rice coding*/
+uint32_t rice_decode_block(int16_t param,const uint32_t *encoded,int32_t count,uint32_t *decoded)
+{
+	int32_t i = 0,j = 0,q,k = 0;
+	int16_t tmp;
+	rice_decode_context context;
+	rice_decode_context *ctx = &context;
+	ctx->filled = 0;
+
+	while(k < count)
+	{
+		//Count ones until a zero is encountered
+		q = 0;
+		while(1)
+		{
+			if(ctx->filled == 0)
+			{
+				ctx->filled = 32;
+				ctx->buffer = encoded[i++];
+			}
+			tmp = ctx->buffer & 0x0001;
+			ctx->buffer = ctx->buffer >> 1;
+			ctx->filled--;
+			if(tmp == 0)
+				break;
+			q++;
+		}
+		decoded[k] = q << param; //x = q * 2^k
+
+		//Read out the last 'param' bits of input
+		for(j = param - 1; j >= 0; j--)
+		{
+			if(ctx->filled == 0)
+			{
+				ctx->filled = 32;
+				ctx->buffer = encoded[i++];
+			}
+			tmp = ctx->buffer & 0x0001;
+			ctx->buffer = ctx->buffer >> 1;
+			ctx->filled--;
+			decoded[k] = decoded[k] | (tmp << j);
+		}
+
+		k++;
+	}
+
+	return(k);
+}
+
+
+//Lookup Tables for 1st, 2nd and higher order quantized reflection coeffs
+static const double
+lookup_1st_order_coeffs[128] =
+{
+	-1, -0.9998779296875, -0.99951171875, -0.9989013671875, -0.998046875, -0.9969482421875, -0.99560546875,
+	-0.9940185546875, -0.9921875, -0.9901123046875, -0.98779296875, -0.9852294921875, -0.982421875, -0.9793701171875,
+	-0.97607421875, -0.9725341796875, -0.96875, -0.9647216796875, -0.96044921875, -0.9559326171875, -0.951171875,
+	-0.9461669921875, -0.94091796875, -0.9354248046875, -0.9296875, -0.9237060546875, -0.91748046875,
+	-0.9110107421875, -0.904296875, -0.8973388671875, -0.89013671875, -0.8826904296875, -0.875, -0.8670654296875,
+	-0.85888671875, -0.8504638671875, -0.841796875, -0.8328857421875, -0.82373046875, -0.8143310546875, -0.8046875,
+	-0.7947998046875, -0.78466796875, -0.7742919921875, -0.763671875, -0.7528076171875, -0.74169921875,
+	-0.7303466796875, -0.71875, -0.7069091796875, -0.69482421875000011, -0.6824951171875, -0.669921875,
+	-0.6571044921875, -0.64404296875, -0.63073730468750011, -0.6171875, -0.6033935546875, -0.58935546875,
+	-0.57507324218750011, -0.56054687500000011, -0.5457763671875, -0.53076171875, -0.5155029296875,
+	-0.50000000000000011, -0.48425292968750011, -0.46826171875, -0.4520263671875, -0.43554687500000011,
+	-0.41882324218750011, -0.40185546875000011, -0.38464355468750011, -0.3671875, -0.34948730468750011,
+	-0.33154296875000011, -0.31335449218750011, -0.29492187500000022, -0.2762451171875, -0.25732421875000011,
+	-0.23815917968750011, -0.21875000000000011, -0.19909667968750022, -0.17919921875, -0.15905761718750011,
+	-0.13867187500000011, -0.11804199218750011, -0.097167968750000222, -0.076049804687500222, -0.054687500000000111,
+	-0.033081054687500111, -0.011230468750000111, 0.010864257812499778, 0.033203125, 0.055786132812499778,
+	0.07861328125, 0.1016845703125, 0.12499999999999978, 0.1485595703125, 0.17236328124999978, 0.1964111328125,
+	0.22070312499999956, 0.24523925781249978, 0.27001953125, 0.29504394531249978, 0.3203125, 0.34582519531249956,
+	0.37158203124999978, 0.3975830078125, 0.42382812499999978, 0.4503173828125, 0.47705078124999956,
+	0.50402832031249978, 0.53125, 0.55871582031249978, 0.58642578125, 0.61437988281249956, 0.64257812499999978,
+	0.6710205078125, 0.69970703124999956, 0.7286376953125, 0.75781249999999956, 0.78723144531249978, 0.81689453125,
+	0.84680175781249956, 0.876953125, 0.90734863281249956, 0.93798828124999978, 0.9688720703125
+};
+
+static const double
+lookup_2nd_order_coeffs[128] =
+{
+	-0.50000000000000011, 0.9998779296875, 0.99951171875, 0.9989013671875, 0.998046875, 0.9969482421875,
+	0.99560546875, 0.9940185546875, 0.9921875, 0.9901123046875, 0.98779296875, 0.9852294921875, 0.982421875,
+	0.9793701171875, 0.97607421875, 0.9725341796875, 0.96875, 0.9647216796875, 0.96044921875, 0.9559326171875,
+	0.951171875, 0.9461669921875, 0.94091796875, 0.9354248046875, 0.9296875, 0.9237060546875, 0.91748046875,
+	0.9110107421875, 0.904296875, 0.8973388671875, 0.89013671875, 0.8826904296875, 0.875, 0.8670654296875,
+	0.85888671875, 0.8504638671875, 0.841796875, 0.8328857421875, 0.82373046875, 0.8143310546875, 0.8046875,
+	0.7947998046875, 0.78466796875, 0.7742919921875, 0.763671875, 0.7528076171875, 0.74169921875, 0.7303466796875,
+	0.71875, 0.7069091796875, 0.69482421875000011, 0.6824951171875, 0.669921875, 0.6571044921875, 0.64404296875,
+	0.63073730468750011, 0.6171875, 0.6033935546875, 0.58935546875, 0.57507324218750011, 0.56054687500000011,
+	0.5457763671875, 0.53076171875, 0.5155029296875, 0.50000000000000011, 0.48425292968750011, 0.46826171875,
+	0.4520263671875, 0.43554687500000011, 0.41882324218750011, 0.40185546875000011, 0.38464355468750011, 0.3671875,
+	0.34948730468750011, 0.33154296875000011, 0.31335449218750011, 0.29492187500000022, 0.2762451171875,
+	0.25732421875000011, 0.23815917968750011, 0.21875000000000011, 0.19909667968750022, 0.17919921875,
+	0.15905761718750011, 0.13867187500000011, 0.11804199218750011, 0.097167968750000222, 0.076049804687500222,
+	0.054687500000000111, 0.033081054687500111, 0.011230468750000111, -0.010864257812499778, -0.033203125,
+	-0.055786132812499778, -0.07861328125, -0.1016845703125, -0.12499999999999978, -0.1485595703125,
+	-0.17236328124999978, -0.1964111328125, -0.22070312499999956, -0.24523925781249978, -0.27001953125,
+	-0.29504394531249978, -0.3203125, -0.34582519531249956, -0.37158203124999978, -0.3975830078125,
+	-0.42382812499999978, -0.4503173828125, -0.47705078124999956, -0.50402832031249978, -0.53125,
+	-0.55871582031249978, -0.58642578125, -0.61437988281249956, -0.64257812499999978, -0.6710205078125,
+	-0.69970703124999956, -0.7286376953125, -0.75781249999999956, -0.78723144531249978, -0.81689453125,
+	-0.84680175781249956, -0.876953125, -0.90734863281249956, -0.93798828124999978, -0.9688720703125
+};
+
+static const double
+lookup_higher_order_coeffs[128] =
+{
+	-1, -0.984375, -0.96875, -0.953125, -0.9375, -0.921875, -0.90625, -0.890625, -0.875, -0.859375, -0.84375,
+	-0.828125, -0.8125, -0.796875, -0.78125, -0.765625, -0.75, -0.734375, -0.71875, -0.703125, -0.6875, -0.671875,
+	-0.65625, -0.640625, -0.625, -0.609375, -0.59375, -0.578125, -0.5625, -0.546875, -0.53125, -0.515625, -0.5,
+	-0.484375, -0.46875, -0.453125, -0.4375, -0.421875, -0.40625, -0.390625, -0.375, -0.359375, -0.34375, -0.328125,
+	-0.3125, -0.296875, -0.28125, -0.265625, -0.25, -0.234375, -0.21875, -0.203125, -0.1875, -0.171875, -0.15625,
+	-0.140625, -0.125, -0.109375, -0.09375, -0.078125, -0.0625, -0.046875, -0.03125, -0.015625, 0, 0.015625, 0.03125,
+	0.046875, 0.0625, 0.078125, 0.09375, 0.109375, 0.125, 0.140625, 0.15625, 0.171875, 0.1875, 0.203125, 0.21875,
+	0.234375, 0.25, 0.265625, 0.28125, 0.296875, 0.3125, 0.328125, 0.34375, 0.359375, 0.375, 0.390625, 0.40625,
+	0.421875, 0.4375, 0.453125, 0.46875, 0.484375, 0.5, 0.515625, 0.53125, 0.546875, 0.5625, 0.578125, 0.59375,
+	0.609375, 0.625, 0.640625, 0.65625, 0.671875, 0.6875, 0.703125, 0.71875, 0.734375, 0.75, 0.765625, 0.78125,
+	0.796875, 0.8125, 0.828125, 0.84375, 0.859375, 0.875, 0.890625, 0.90625, 0.921875, 0.9375, 0.953125, 0.96875,0.984375
+};
+
+/*Checks if every sample of a block is equal or not*/
+int32_t check_if_constant(const int16_t *data,int32_t num_elements)
+{
+	int16_t temp = data[0];
+
+	for(int32_t i = 1; i < num_elements; i++)
+	{
+		if(temp != data[i])
+			return -1;
+	}
+
+	return 0;
+}
+
+/*autocorrelation function*/
+void auto_corr_fun(double *x,int32_t N,int64_t k,int16_t norm,double *rxx)
+{
+	int64_t i, n;
+	double sum = 0,mean = 0;
+
+	for (i = 0; i < N; i++)
+		sum += x[i];
+	mean = sum/N;
+
+	for(i = 0; i <= k; i++)
+	{
+		rxx[i] = 0.0;
+		for (n = i; n < N; n++)
+			rxx[i] += (x[n] - mean) * (x[n-i] - mean);
+	}
+
+	if(norm)
+	{
+		for (i = 1; i <= k; i++)
+			rxx[i] /= rxx[0];
+		rxx[0] = 1.0;
+	}
+
+	return;
+}
+
+/*Levinson recursion algorithm*/
+void levinson(double *autoc,uint8_t max_order,double *ref,double lpc[][MAX_LPC_ORDER])
+{
+	int32_t i, j, i2;
+	double r, err, tmp;
+	double lpc_tmp[MAX_LPC_ORDER];
+
+	for(i = 0; i < max_order; i++)
+		lpc_tmp[i] = 0;
+	err = 1.0;
+	if(autoc)
+		err = autoc[0];
+
+	for(i = 0; i < max_order; i++)
+	{
+		if(ref)
+			r = ref[i];
+		else
+		{
+			r = -autoc[i+1];
+			for(j = 0; j < i; j++)
+				r -= lpc_tmp[j] * autoc[i-j];
+			r /= err;
+			err *= 1.0 - (r * r);
+		}
+
+		i2 = i >> 1;
+		lpc_tmp[i] = r;
+		for(j = 0; j < i2; j++)
+		{
+			tmp = lpc_tmp[j];
+			lpc_tmp[j] += r * lpc_tmp[i-1-j];
+			lpc_tmp[i-1-j] += r * tmp;
+		}
+		if(i & 1)
+			lpc_tmp[j] += lpc_tmp[j] * r;
+
+		for(j = 0; j <= i; j++)
+			lpc[i][j] = -lpc_tmp[j];
+	}
+
+	return;
+}
+
+/*Fixed point levinson function*/
+void levinson_fixed(int64_t *autocorr,uint8_t max_order,int64_t *ref,int64_t lpc[][MAX_LPC_ORDER])
+{
+	int32_t i, j, i2;
+	int64_t r, err, tmp;
+	int64_t lpc_tmp[MAX_LPC_ORDER];
+
+	for(i = 0; i < max_order; i++)
+		lpc_tmp[i] = 0;
+	err = 1.0;
+	if(autocorr)
+		err = autocorr[0];
+
+	for(i = 0; i < max_order; i++)
+	{
+		if(ref)
+			r = ref[i];
+		else
+		{
+			r = -autocorr[i+1];
+			for(j = 0; j < i; j++)
+				r -= lpc_tmp[j] * autocorr[i-j];
+			r /= err;
+			err *= 1.0 - (r * r);
+		}
+
+		i2 = i >> 1;
+		lpc_tmp[i] = r;
+		for(j = 0; j < i2; j++)
+		{
+			tmp = lpc_tmp[j];
+			lpc_tmp[j] += r * lpc_tmp[i-1-j];
+			lpc_tmp[i-1-j] += r * tmp;
+		}
+		if(i & 1)
+			lpc_tmp[j] += lpc_tmp[j] * r;
+
+		for(j = 0; j <= i; j++)
+			lpc[i][j] = -lpc_tmp[j];
+	}
+
+	return;
+}
+
+/*Calculate reflection coefficients*/
+uint8_t compute_ref_coefs(double *autoc,uint8_t max_order,double *ref)
+{
+	int32_t i, j;
+	double error;
+	double gen[2][MAX_LPC_ORDER];
+	uint8_t order_est;
+
+	//Schurr recursion
+	for(i = 0; i < max_order; i++)
+		gen[0][i] = gen[1][i] = autoc[i+1];
+
+	error = autoc[0];
+	ref[0] = -gen[1][0] / error;
+	error += gen[1][0] * ref[0];
+	for(i = 1; i < max_order; i++)
+	{
+		for(j = 0; j < max_order - i; j++)
+		{
+			gen[1][j] = gen[1][j+1] + ref[i-1] * gen[0][j];
+			gen[0][j] = gen[1][j+1] * ref[i-1] + gen[0][j];
+		}
+		ref[i] = -gen[1][0] / error;
+		error += gen[1][0] * ref[i];
+	}
+
+	//Estimate optimal order using reflection coefficients
+	order_est = 1;
+	for(i = max_order - 1; i >= 0; i--)
+	{
+		if(fabs(ref[i]) > 0.05)
+		{
+			order_est = i+1;
+			break;
+		}
+	}
+
+	return(order_est);
+}
+
+/*Quantize reflection coeffs*/
+int32_t qtz_ref_cof(double *par,uint8_t ord,int32_t *q_ref)
+{
+	for(int32_t i = 0; i < ord; i++)
+	{
+		if(i == 0)
+			q_ref[i] = floor(64 * (-1 + (SQRT2 * sqrt(par[i]  + 1))));
+		else if(i == 1)
+			q_ref[i] = floor(64 * (-1 + (SQRT2 * sqrt(-par[i] + 1))));
+		else
+			q_ref[i] = floor(64 * par[i]);
+	}
+
+	return(0);
+}
+
+/*Dequantize reflection coefficients*/
+int32_t dqtz_ref_cof(const int32_t *q_ref,uint8_t ord,double *ref)
+{
+	int32_t spar[MAX_LPC_ORDER];
+	double temp;
+
+	if(ord <= 1)
+	{
+		ref[0] = 0;
+		return(0);
+	}
+
+	for(int32_t i = 0; i < ord; i++)
+	{
+		if(i == 0)
+			ref[i] = lookup_1st_order_coeffs[q_ref[i] + 64];
+		else if(i == 1)
+			ref[i] = lookup_2nd_order_coeffs[q_ref[i] + 64];
+		else
+			ref[i] = lookup_higher_order_coeffs[q_ref[i] + 64];
+	}
+
+	return(0);
+}
+
+/*Calculate residues from samples and lpc coefficients*/
+void calc_residue(const int32_t *samples,int64_t N,int16_t ord,int16_t Q,int64_t *coff,int32_t *residues)
+{
+	int64_t k, i;
+	int64_t corr;
+	int64_t y;
+
+	corr = ((int64_t)1) << (Q - 1);//Correction term
+
+	residues[0] = samples[0];
+	for(k = 1; k <= ord; k++)
+	{
+		y = corr;
+		for(i = 1; i <= k; i++)
+		{
+			y += (int64_t)coff[i] * samples[k-i];
+			if((k - i) == 0)
+				break;
+		}
+		residues[k] = samples[k] - (int32_t)(y >> Q);
+	}
+
+	for(k = ord + 1; k < N; k++)
+	{
+		y = corr;
+
+		y += (int64_t)(coff[0] * samples[k - 0]);
+		y += (int64_t)(coff[1] * samples[k - 1]);
+		y += (int64_t)(coff[2] * samples[k - 2]);
+		y += (int64_t)(coff[3] * samples[k - 3]);
+		y += (int64_t)(coff[4] * samples[k - 4]);
+		y += (int64_t)(coff[5] * samples[k - 5]);
+		y += (int64_t)(coff[6] * samples[k - 6]);
+		y += (int64_t)(coff[7] * samples[k - 7]);
+		y += (int64_t)(coff[8] * samples[k - 8]);
+		y += (int64_t)(coff[9] * samples[k - 9]);
+		y += (int64_t)(coff[10] * samples[k - 10]);
+		y += (int64_t)(coff[11] * samples[k - 11]);
+		y += (int64_t)(coff[12] * samples[k - 12]);
+		y += (int64_t)(coff[13] * samples[k - 13]);
+		y += (int64_t)(coff[14] * samples[k - 14]);
+		y += (int64_t)(coff[15] * samples[k - 15]);
+		y += (int64_t)(coff[16] * samples[k - 16]);
+		y += (int64_t)(coff[17] * samples[k - 17]);
+		y += (int64_t)(coff[18] * samples[k - 18]);
+		y += (int64_t)(coff[19] * samples[k - 19]);
+		y += (int64_t)(coff[20] * samples[k - 20]);
+		y += (int64_t)(coff[21] * samples[k - 21]);
+		y += (int64_t)(coff[22] * samples[k - 22]);
+		y += (int64_t)(coff[23] * samples[k - 23]);
+		y += (int64_t)(coff[24] * samples[k - 24]);
+		y += (int64_t)(coff[25] * samples[k - 25]);
+		y += (int64_t)(coff[26] * samples[k - 26]);
+		y += (int64_t)(coff[27] * samples[k - 27]);
+		y += (int64_t)(coff[28] * samples[k - 28]);
+		y += (int64_t)(coff[29] * samples[k - 29]);
+		y += (int64_t)(coff[30] * samples[k - 30]);
+		y += (int64_t)(coff[31] * samples[k - 31]);
+		y += (int64_t)(coff[32] * samples[k - 32]);
+		y += (int64_t)(coff[33] * samples[k - 33]);
+		y += (int64_t)(coff[34] * samples[k - 34]);
+		y += (int64_t)(coff[35] * samples[k - 35]);
+		y += (int64_t)(coff[36] * samples[k - 36]);
+		y += (int64_t)(coff[37] * samples[k - 37]);
+		y += (int64_t)(coff[38] * samples[k - 38]);
+		y += (int64_t)(coff[39] * samples[k - 39]);
+		y += (int64_t)(coff[40] * samples[k - 40]);
+		y += (int64_t)(coff[41] * samples[k - 41]);
+		y += (int64_t)(coff[42] * samples[k - 42]);
+		y += (int64_t)(coff[43] * samples[k - 43]);
+		y += (int64_t)(coff[44] * samples[k - 44]);
+		y += (int64_t)(coff[45] * samples[k - 45]);
+		y += (int64_t)(coff[46] * samples[k - 46]);
+		y += (int64_t)(coff[47] * samples[k - 47]);
+		y += (int64_t)(coff[48] * samples[k - 48]);
+		y += (int64_t)(coff[49] * samples[k - 49]);
+		y += (int64_t)(coff[50] * samples[k - 50]);
+		y += (int64_t)(coff[51] * samples[k - 51]);
+		y += (int64_t)(coff[52] * samples[k - 52]);
+		y += (int64_t)(coff[53] * samples[k - 53]);
+		y += (int64_t)(coff[54] * samples[k - 54]);
+		y += (int64_t)(coff[55] * samples[k - 55]);
+		y += (int64_t)(coff[56] * samples[k - 56]);
+		y += (int64_t)(coff[57] * samples[k - 57]);
+		y += (int64_t)(coff[58] * samples[k - 58]);
+		y += (int64_t)(coff[59] * samples[k - 59]);
+		y += (int64_t)(coff[60] * samples[k - 60]);
+		y += (int64_t)(coff[61] * samples[k - 61]);
+		y += (int64_t)(coff[62] * samples[k - 62]);
+		y += (int64_t)(coff[63] * samples[k - 63]);
+		y += (int64_t)(coff[64] * samples[k - 64]);
+		y += (int64_t)(coff[65] * samples[k - 65]);
+		y += (int64_t)(coff[66] * samples[k - 66]);
+		y += (int64_t)(coff[67] * samples[k - 67]);
+		y += (int64_t)(coff[68] * samples[k - 68]);
+		y += (int64_t)(coff[69] * samples[k - 69]);
+		y += (int64_t)(coff[70] * samples[k - 70]);
+		y += (int64_t)(coff[71] * samples[k - 71]);
+		y += (int64_t)(coff[72] * samples[k - 72]);
+		y += (int64_t)(coff[73] * samples[k - 73]);
+		y += (int64_t)(coff[74] * samples[k - 74]);
+		y += (int64_t)(coff[75] * samples[k - 75]);
+		y += (int64_t)(coff[76] * samples[k - 76]);
+		y += (int64_t)(coff[77] * samples[k - 77]);
+		y += (int64_t)(coff[78] * samples[k - 78]);
+		y += (int64_t)(coff[79] * samples[k - 79]);
+		y += (int64_t)(coff[80] * samples[k - 80]);
+		y += (int64_t)(coff[81] * samples[k - 81]);
+		y += (int64_t)(coff[82] * samples[k - 82]);
+		y += (int64_t)(coff[83] * samples[k - 83]);
+		y += (int64_t)(coff[84] * samples[k - 84]);
+		y += (int64_t)(coff[85] * samples[k - 85]);
+		y += (int64_t)(coff[86] * samples[k - 86]);
+		y += (int64_t)(coff[87] * samples[k - 87]);
+		y += (int64_t)(coff[88] * samples[k - 88]);
+		y += (int64_t)(coff[89] * samples[k - 89]);
+		y += (int64_t)(coff[90] * samples[k - 90]);
+		y += (int64_t)(coff[91] * samples[k - 91]);
+		y += (int64_t)(coff[92] * samples[k - 92]);
+		y += (int64_t)(coff[93] * samples[k - 93]);
+		y += (int64_t)(coff[94] * samples[k - 94]);
+		y += (int64_t)(coff[95] * samples[k - 95]);
+		y += (int64_t)(coff[96] * samples[k - 96]);
+		y += (int64_t)(coff[97] * samples[k - 97]);
+		y += (int64_t)(coff[98] * samples[k - 98]);
+		y += (int64_t)(coff[99] * samples[k - 99]);
+		y += (int64_t)(coff[100] * samples[k - 100]);
+
+		//for(i = 0; i <= ord; i++)
+			//y += (int64_t)(coff[i] * samples[k-i]);
+		residues[k] = samples[k] - (int32_t)(y >> Q);
+	}
+
+	return;
+}
+
+/*Calculate samples from residues and lpc coefficients*/
+void calc_signal(const int32_t *residues,int64_t N,int16_t ord,int16_t Q,int64_t *coff,int32_t *samples)
+{
+	int64_t k, i;
+	int64_t corr;
+	int64_t y;
+
+	corr = ((int64_t)1) << (Q - 1);//Correction term
+
+	samples[0] = residues[0];
+	for(k = 1; k <= ord; k++)
+	{
+		y = corr;
+		for(i = 1; i <= k; i++)
+		{
+			y -= (int64_t)(coff[i] * samples[k-i]);
+			if((k - i) == 0)
+				break;
+		}
+		samples[k] = residues[k] - (int32_t)(y >> Q);
+	}
+
+	for(k = ord + 1; k < N; k++)
+	{
+		y = corr;
+
+		y -= (int64_t)(coff[0] * samples[k - 0]);
+		y -= (int64_t)(coff[1] * samples[k - 1]);
+		y -= (int64_t)(coff[2] * samples[k - 2]);
+		y -= (int64_t)(coff[3] * samples[k - 3]);
+		y -= (int64_t)(coff[4] * samples[k - 4]);
+		y -= (int64_t)(coff[5] * samples[k - 5]);
+		y -= (int64_t)(coff[6] * samples[k - 6]);
+		y -= (int64_t)(coff[7] * samples[k - 7]);
+		y -= (int64_t)(coff[8] * samples[k - 8]);
+		y -= (int64_t)(coff[9] * samples[k - 9]);
+		y -= (int64_t)(coff[10] * samples[k - 10]);
+		y -= (int64_t)(coff[11] * samples[k - 11]);
+		y -= (int64_t)(coff[12] * samples[k - 12]);
+		y -= (int64_t)(coff[13] * samples[k - 13]);
+		y -= (int64_t)(coff[14] * samples[k - 14]);
+		y -= (int64_t)(coff[15] * samples[k - 15]);
+		y -= (int64_t)(coff[16] * samples[k - 16]);
+		y -= (int64_t)(coff[17] * samples[k - 17]);
+		y -= (int64_t)(coff[18] * samples[k - 18]);
+		y -= (int64_t)(coff[19] * samples[k - 19]);
+		y -= (int64_t)(coff[20] * samples[k - 20]);
+		y -= (int64_t)(coff[21] * samples[k - 21]);
+		y -= (int64_t)(coff[22] * samples[k - 22]);
+		y -= (int64_t)(coff[23] * samples[k - 23]);
+		y -= (int64_t)(coff[24] * samples[k - 24]);
+		y -= (int64_t)(coff[25] * samples[k - 25]);
+		y -= (int64_t)(coff[26] * samples[k - 26]);
+		y -= (int64_t)(coff[27] * samples[k - 27]);
+		y -= (int64_t)(coff[28] * samples[k - 28]);
+		y -= (int64_t)(coff[29] * samples[k - 29]);
+		y -= (int64_t)(coff[30] * samples[k - 30]);
+		y -= (int64_t)(coff[31] * samples[k - 31]);
+		y -= (int64_t)(coff[32] * samples[k - 32]);
+		y -= (int64_t)(coff[33] * samples[k - 33]);
+		y -= (int64_t)(coff[34] * samples[k - 34]);
+		y -= (int64_t)(coff[35] * samples[k - 35]);
+		y -= (int64_t)(coff[36] * samples[k - 36]);
+		y -= (int64_t)(coff[37] * samples[k - 37]);
+		y -= (int64_t)(coff[38] * samples[k - 38]);
+		y -= (int64_t)(coff[39] * samples[k - 39]);
+		y -= (int64_t)(coff[40] * samples[k - 40]);
+		y -= (int64_t)(coff[41] * samples[k - 41]);
+		y -= (int64_t)(coff[42] * samples[k - 42]);
+		y -= (int64_t)(coff[43] * samples[k - 43]);
+		y -= (int64_t)(coff[44] * samples[k - 44]);
+		y -= (int64_t)(coff[45] * samples[k - 45]);
+		y -= (int64_t)(coff[46] * samples[k - 46]);
+		y -= (int64_t)(coff[47] * samples[k - 47]);
+		y -= (int64_t)(coff[48] * samples[k - 48]);
+		y -= (int64_t)(coff[49] * samples[k - 49]);
+		y -= (int64_t)(coff[50] * samples[k - 50]);
+		y -= (int64_t)(coff[51] * samples[k - 51]);
+		y -= (int64_t)(coff[52] * samples[k - 52]);
+		y -= (int64_t)(coff[53] * samples[k - 53]);
+		y -= (int64_t)(coff[54] * samples[k - 54]);
+		y -= (int64_t)(coff[55] * samples[k - 55]);
+		y -= (int64_t)(coff[56] * samples[k - 56]);
+		y -= (int64_t)(coff[57] * samples[k - 57]);
+		y -= (int64_t)(coff[58] * samples[k - 58]);
+		y -= (int64_t)(coff[59] * samples[k - 59]);
+		y -= (int64_t)(coff[60] * samples[k - 60]);
+		y -= (int64_t)(coff[61] * samples[k - 61]);
+		y -= (int64_t)(coff[62] * samples[k - 62]);
+		y -= (int64_t)(coff[63] * samples[k - 63]);
+		y -= (int64_t)(coff[64] * samples[k - 64]);
+		y -= (int64_t)(coff[65] * samples[k - 65]);
+		y -= (int64_t)(coff[66] * samples[k - 66]);
+		y -= (int64_t)(coff[67] * samples[k - 67]);
+		y -= (int64_t)(coff[68] * samples[k - 68]);
+		y -= (int64_t)(coff[69] * samples[k - 69]);
+		y -= (int64_t)(coff[70] * samples[k - 70]);
+		y -= (int64_t)(coff[71] * samples[k - 71]);
+		y -= (int64_t)(coff[72] * samples[k - 72]);
+		y -= (int64_t)(coff[73] * samples[k - 73]);
+		y -= (int64_t)(coff[74] * samples[k - 74]);
+		y -= (int64_t)(coff[75] * samples[k - 75]);
+		y -= (int64_t)(coff[76] * samples[k - 76]);
+		y -= (int64_t)(coff[77] * samples[k - 77]);
+		y -= (int64_t)(coff[78] * samples[k - 78]);
+		y -= (int64_t)(coff[79] * samples[k - 79]);
+		y -= (int64_t)(coff[80] * samples[k - 80]);
+		y -= (int64_t)(coff[81] * samples[k - 81]);
+		y -= (int64_t)(coff[82] * samples[k - 82]);
+		y -= (int64_t)(coff[83] * samples[k - 83]);
+		y -= (int64_t)(coff[84] * samples[k - 84]);
+		y -= (int64_t)(coff[85] * samples[k - 85]);
+		y -= (int64_t)(coff[86] * samples[k - 86]);
+		y -= (int64_t)(coff[87] * samples[k - 87]);
+		y -= (int64_t)(coff[88] * samples[k - 88]);
+		y -= (int64_t)(coff[89] * samples[k - 89]);
+		y -= (int64_t)(coff[90] * samples[k - 90]);
+		y -= (int64_t)(coff[91] * samples[k - 91]);
+		y -= (int64_t)(coff[92] * samples[k - 92]);
+		y -= (int64_t)(coff[93] * samples[k - 93]);
+		y -= (int64_t)(coff[94] * samples[k - 94]);
+		y -= (int64_t)(coff[95] * samples[k - 95]);
+		y -= (int64_t)(coff[96] * samples[k - 96]);
+		y -= (int64_t)(coff[97] * samples[k - 97]);
+		y -= (int64_t)(coff[98] * samples[k - 98]);
+		y -= (int64_t)(coff[99] * samples[k - 99]);
+		y -= (int64_t)(coff[100] * samples[k - 100]);
+
+		//for(i = 0; i <= ord; i++)
+			//y -= (int64_t)(coff[i] * samples[k-i]);
+		samples[k] = residues[k] - (int32_t)(y >> Q);
+	}
+
+	return;
+}
+


[add] Arduino sketch

[add] Arduino sketch

remote: Support for password authentication was removed on August 13, 2021.
remote: Please see https://docs.github.com/en/get-started/getting-started-with-git/about-remote-repositories#cloning-with-https-urls for information on currently recommended modes of authentication.
fatal: Authentication failed for 'https://github.com/tomsimonart/GLM.git/'


Test sketch

Test sketch

error: pathspec 'Peripheral/Test_Sensor.ino/Test_Sensor.ino/Test_Sensor.ino.ino' did not match any file(s) known to git

--- Peripheral/Test_Sensor.ino/Test_Sensor.ino/Test_Sensor.ino.ino
+++ Peripheral/Test_Sensor.ino/Test_Sensor.ino/Test_Sensor.ino.ino
@@ -0,0 +1,77 @@
+/******************************************************************************
+Source: https://learn.sparkfun.com/tutorials/flex-sensor-hookup-guide
+Source: https://www.arduino.cc/en/Reference/CurieBLE
+
+Create a voltage divider circuit combining a flex sensor with a 47k resistor.
+- The resistor should connect from A0 to GND.
+- The flex sensor should connect from A0 to 3.3V
+As the resistance of the flex sensor increases (meaning it's being bent), the
+voltage at A0 should decrease.
+Transmit to BLE centrals
+******************************************************************************/
+// Include BLE files.
+#include <CurieBLE.h>
+
+//###BLE Characteristics###
+BLEPeripheral blePeripheral;       // BLE Peripheral Device (the board you're programming)
+BLEService batteryService("180F"); // BLE "Battery service"
+
+// BLE Battery Level Characteristic
+BLEUnsignedCharCharacteristic batteryLevel("2A19",  // standard 16-bit characteristic UUID
+    BLERead | BLENotify);     // remote clients will be able to get notifications if this characteristic changes
+
+int value = 50;
+
+void setup()
+{
+  Serial.begin(9600);
+  pinMode(13, OUTPUT); // initialize the LED on pin 13 to indicate when a central is connected
+  /* Set a local name for the BLE device
+     This name will appear in advertising packets
+     and can be used by remote devices to identify this BLE device
+     The name can be changed but maybe be truncated based on space left in advertisement packet */
+  blePeripheral.setLocalName("TestSensor1");
+  blePeripheral.setAdvertisedServiceUuid(batteryService.uuid());  // add the service UUID
+  blePeripheral.addAttribute(batteryService);   // Add the BLE Battery service
+  blePeripheral.addAttribute(batteryLevel); // add the battery level characteristic
+  batteryLevel.setValue(value);   // initial value for this characteristic
+  while(!Serial);
+  Serial.println("Configured BLE");
+  /* Now activate the BLE device.  It will start continuously transmitting BLE
+     advertising packets and will be visible to remote BLE central devices
+     until it receives a new connection */
+  blePeripheral.begin();
+  Serial.println("Bluetooth device active, waiting for connections...");
+}
+
+
+// This function is called continuously, after setup() completes.
+void loop()
+{
+  // listen for BLE centrals to connect:
+  BLECentral central = blePeripheral.central();
+
+  // if a central is connected to peripheral:
+  if (central) {
+    Serial.print("Connected to central: ");
+    // print the central's MAC address:
+    Serial.println(central.address());
+    // turn on the LED to indicate the connection:
+    digitalWrite(13, HIGH);
+    while (central.connected()) {
+      if (++value == 100) value = 0;
+      delay(500);
+      /*long currentMillis = millis();
+      // if 200ms have passed, check the flex angle:
+      if (currentMillis - previousMillis >= 200) {
+        previousMillis = currentMillis;
+        updateFlexAngle();
+      }*/
+    }
+    Serial.print("Disconnected from central: ");
+    Serial.println(central.address());
+    // when the central disconnects, turn off the LED:
+    digitalWrite(13, LOW);
+  }
+}
+


v1.01 files added

v1.01 files added

error: pathspec 'arduino/ESPsonoff-v1.01t/ESPsonoff-v1.01t.ino' did not match any file(s) known to git

--- arduino/ESPsonoff-v1.01t/ESPsonoff-v1.01t.ino
+++ arduino/ESPsonoff-v1.01t/ESPsonoff-v1.01t.ino
@@ -0,0 +1,267 @@
+/*
+
+  Copyright (c) 2017 @KmanOz
+
+  Permission is hereby granted, free of charge, to any person obtaining a copy
+  of this software and associated documentation files (the "Software"), to deal
+  in the Software without restriction, including without limitation the rights
+  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+  copies of the Software, and to permit persons to whom the Software is
+  furnished to do so, subject to the following conditions:
+
+  The above copyright notice and this permission notice shall be included in all
+  copies or substantial portions of the Software.
+
+  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+  SOFTWARE.
+
+  ==============================================================================
+  Changes in v1.01
+
+    - Relay state now stored in EEPROM and will power up from power loss with
+      last relay state.
+  ==============================================================================
+
+  **** USE THIS Firmware for: Original Sonof fitted with DHT22 as per Github Instructions ****
+
+*/
+
+#include "DHT.h"
+#include <EEPROM.h>
+#include <ESP8266WiFi.h>
+#include <PubSubClient.h>
+#include <Ticker.h>
+
+#define BUTTON          0                                    // (Don't Change for Sonoff)
+#define RELAY           12                                   // (Don't Change for Sonoff)
+#define LED             13                                   // (Don't Change for Sonoff)
+#define DHTPIN          14                                   // (Don't Change for Sonoff)
+
+#define DHTTYPE         DHT22                                // (11 or 22) DHT Type
+
+#define MQTT_CLIENT     "Sonoff_Living_Room_v1.01t"          // mqtt client_id (Must be unique for each Sonoff)
+#define MQTT_SERVER     "192.168.0.100"                      // mqtt server
+#define MQTT_PORT       1883                                 // mqtt port
+#define MQTT_TOPIC      "home/sonoff/living_room/1"          // mqtt topic (Must be unique for each Sonoff)
+#define MQTT_USER       "user"                               // mqtt user
+#define MQTT_PASS       "pass"                               // mqtt password
+
+#define WIFI_SSID       "homewifi"                           // wifi ssid
+#define WIFI_PASS       "homepass"                           // wifi password
+
+#define VERSION    "\n\n------------------  Sonoff Powerpoint v1.01t  ------------------"
+
+bool rememberRelayState = true;                              // If 'true' remembers the state of the relay before power loss.
+bool sendStatus = false;                                     // (Do not Change)
+bool requestRestart = false;                                 // (Do not Change)
+bool tempReport = false;                                     // (Do not Change)
+
+int kUpdFreq = 1;                                            // Update frequency in Mintes to check for mqtt connection
+int kRetries = 10;                                           // WiFi retry count. Increase if not connecting to router.
+int lastRelayState;                                          // (Do not Change)
+
+unsigned long TTasks;                                        // (Do not Change)
+unsigned long count = 0;                                     // (Do not Change)
+
+DHT dht(DHTPIN, DHTTYPE, 11);                                // (Don't Change for Sonoff)
+
+extern "C" {
+  #include "user_interface.h"
+}
+
+WiFiClient wifiClient;
+PubSubClient mqttClient(wifiClient, MQTT_SERVER, MQTT_PORT);
+Ticker btn_timer;
+
+void callback(const MQTT::Publish& pub) {
+  if (pub.payload_string() == "stat") {
+  }
+  else if (pub.payload_string() == "on") {
+    digitalWrite(LED, LOW);
+    digitalWrite(RELAY, HIGH);
+  }
+  else if (pub.payload_string() == "off") {
+    digitalWrite(LED, HIGH);
+    digitalWrite(RELAY, LOW);
+  }
+  else if (pub.payload_string() == "reset") {
+    requestRestart = true;
+  }
+  sendStatus = true;
+}
+
+void setup() {
+  pinMode(LED, OUTPUT);
+  pinMode(RELAY, OUTPUT);
+  pinMode(BUTTON, INPUT);
+  digitalWrite(LED, HIGH);
+  digitalWrite(RELAY, LOW);
+  Serial.begin(115200);
+  EEPROM.begin(8);
+  lastRelayState = EEPROM.read(0);
+  if (rememberRelayState && lastRelayState == 1) {
+    digitalWrite(LED, LOW);
+    digitalWrite(RELAY, HIGH);
+  }
+  btn_timer.attach(0.05, button);
+  mqttClient.set_callback(callback);
+  WiFi.mode(WIFI_STA);
+  WiFi.begin(WIFI_SSID, WIFI_PASS);
+  Serial.println(VERSION);
+  Serial.print("\nUnit ID: ");
+  Serial.print("esp8266-");
+  Serial.print(ESP.getChipId(), HEX);
+  Serial.print("\nConnecting to "); Serial.print(WIFI_SSID); Serial.print(" Wifi");
+  while ((WiFi.status() != WL_CONNECTED) && kRetries --) {
+    delay(500);
+    Serial.print(" .");
+  }
+  if (WiFi.status() == WL_CONNECTED) {
+    Serial.println(" DONE");
+    Serial.print("IP Address is: "); Serial.println(WiFi.localIP());
+    Serial.print("Connecting to ");Serial.print(MQTT_SERVER);Serial.print(" Broker . .");
+    delay(500);
+    while (!mqttClient.connect(MQTT::Connect(MQTT_CLIENT).set_keepalive(90).set_auth(MQTT_USER, MQTT_PASS)) && kRetries --) {
+      Serial.print(" .");
+      delay(1000);
+    }
+    if(mqttClient.connected()) {
+      Serial.println(" DONE");
+      Serial.println("\n----------------------------  Logs  ----------------------------");
+      Serial.println();
+      mqttClient.subscribe(MQTT_TOPIC);
+      blinkLED(LED, 40, 8);
+      if(digitalRead(RELAY) == HIGH)  {
+        digitalWrite(LED, LOW);
+      } else {
+        digitalWrite(LED, HIGH);
+      }
+    }
+    else {
+      Serial.println(" FAILED!");
+      Serial.println("\n----------------------------------------------------------------");
+      Serial.println();
+    }
+  }
+  else {
+    Serial.println(" WiFi FAILED!");
+    Serial.println("\n----------------------------------------------------------------");
+    Serial.println();
+  }
+}
+
+void loop() {
+  mqttClient.loop();
+  timedTasks();
+  checkStatus();
+  if (tempReport) {
+    getTemp();
+  }
+}
+
+void blinkLED(int pin, int duration, int n) {
+  for(int i=0; i<n; i++)  {
+    digitalWrite(pin, HIGH);
+    delay(duration);
+    digitalWrite(pin, LOW);
+    delay(duration);
+  }
+}
+
+void button() {
+  if (!digitalRead(BUTTON)) {
+    count++;
+  }
+  else {
+    if (count > 1 && count <= 40) {
+      digitalWrite(LED, !digitalRead(LED));
+      digitalWrite(RELAY, !digitalRead(RELAY));
+      sendStatus = true;
+    }
+    else if (count >40){
+      Serial.println("\n\nSonoff Rebooting . . . . . . . . Please Wait");
+      requestRestart = true;
+    }
+    count=0;
+  }
+}
+
+void checkConnection() {
+  if (WiFi.status() == WL_CONNECTED)  {
+    if (mqttClient.connected()) {
+      Serial.println("mqtt broker connection . . . . . . . . . . OK");
+    }
+    else {
+      Serial.println("mqtt broker connection . . . . . . . . . . LOST");
+      requestRestart = true;
+    }
+  }
+  else {
+    Serial.println("WiFi connection . . . . . . . . . . LOST");
+    requestRestart = true;
+  }
+}
+
+void checkStatus() {
+  if (sendStatus) {
+    if(digitalRead(LED) == LOW)  {
+      if (rememberRelayState) {
+        EEPROM.write(0, 1);
+      }
+      mqttClient.publish(MQTT::Publish(MQTT_TOPIC"/stat", "on").set_retain().set_qos(1));
+      Serial.println("Relay . . . . . . . . . . . . . . . . . . ON");
+    } else {
+      if (rememberRelayState) {
+        EEPROM.write(0, 0);
+      }
+      mqttClient.publish(MQTT::Publish(MQTT_TOPIC"/stat", "off").set_retain().set_qos(1));
+      Serial.println("Relay . . . . . . . . . . . . . . . . . . OFF");
+    }
+    if (rememberRelayState) {
+      EEPROM.commit();
+    }
+    sendStatus = false;
+  }
+  if (requestRestart) {
+    blinkLED(LED, 400, 4);
+    ESP.restart();
+  }
+}
+
+void getTemp() {
+  Serial.print("DHT read . . . . . . . . . . . . . . . . . ");
+  float dhtH, dhtT;
+  char message_buff[60];
+  dhtH = dht.readHumidity();
+  dhtT = dht.readTemperature();
+  if(digitalRead(LED) == LOW)  {
+    blinkLED(LED, 100, 1);
+  } else {
+    blinkLED(LED, 100, 1);
+    digitalWrite(LED, HIGH);
+  }
+  if (isnan(dhtH) || isnan(dhtT)) {
+    mqttClient.publish(MQTT::Publish(MQTT_TOPIC"/debug","\"DHT Read Error\"").set_retain().set_qos(1));
+    Serial.println("ERROR");
+    tempReport = false;
+    return;
+  }
+  String pubString = "{\"Temp\": "+String(dhtT)+", "+"\"Humidity\": "+String(dhtH) + "}";
+  pubString.toCharArray(message_buff, pubString.length()+1);
+  mqttClient.publish(MQTT::Publish(MQTT_TOPIC"/temp", message_buff).set_retain().set_qos(1));
+  Serial.println("OK");
+  tempReport = false;
+}
+
+void timedTasks() {
+  if ((millis() > TTasks + (kUpdFreq*60000)) || (millis() < TTasks)) {
+    TTasks = millis();
+    checkConnection();
+    tempReport = true;
+  }
+}


Criação do Sketch para teste do control da fan

Utiliza o Pin 6 para uma frequência de pwm de 980Hz de acordo com a
datasheet do Atmega 328 e do código gerado pelo Arduino IDE para
controlo de sinais PWM.

error: pathspec 'Modules/FanControl/FanControl.ino' did not match any file(s) known to git

--- Modules/FanControl/FanControl.ino
+++ Modules/FanControl/FanControl.ino
@@ -0,0 +1,17 @@
+// The frequency of the PWM signal on most pins is approximately
+// 490 Hz. On the Uno and similar boards, pins 5 and 6 have
+// a frequency of approximately 980 Hz. Pins 3 and 11 on the Leonardo also run at 980 Hz.
+
+const int fanOutput = 6;
+const int PWM = 127; // 127 para o PWM = 50 % do Ciclo
+
+void setup() {
+  // put your setup code here, to run once:
+
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+
+analogWrite(fanOutput,PWM);
+}


IRSensorTest

IRSensorTest

Formula: cm = 10650.08 * pow(sensorValue, -0.935)-10;

error: pathspec 'IRsensortest.ino' did not match any file(s) known to git

--- IRsensortest.ino
+++ IRsensortest.ino
@@ -0,0 +1,14 @@
+float sensorValue, cm;
+
+void setup() {
+  Serial.begin(9600);
+  pinMode(A1, INPUT);
+}
+
+void loop() {
+  sensorValue = analogRead(A1);
+  cm = 10650.08 * pow(sensorValue, -0.935)-10;
+  Serial.print("cm:");
+  Serial.println(cm);
+  delay(1000);
+}


Add SpidEmulator example project.

Add SpidEmulator example project.

error: pathspec 'NexStar/examples/SpidEmulator/SpidEmulator.ino' did not match any file(s) known to git

--- NexStar/examples/SpidEmulator/SpidEmulator.ino
+++ NexStar/examples/SpidEmulator/SpidEmulator.ino
@@ -0,0 +1,111 @@
+// Emulates the SPID Rot2Prog AzEl rotator protocol.
+
+#include <NexStar.h>
+
+#define RXPIN 10
+#define TXPIN 11
+#define SELPIN 12
+
+#define TICKS_PER_DEG 46603.378f
+
+#define DEFAULT_RESOLUTION 10
+
+NexStar ns(TXPIN, RXPIN, SELPIN);
+
+void sendStatus() {
+  float az = (float)ns.azimuthMC->getPosition() / TICKS_PER_DEG;
+  float el = (float)ns.elevationMC->getPosition() / TICKS_PER_DEG;
+
+  int h14 = (az + 360.0f) * 10.0f;
+  int v14 = (el + 360.0f) * 10.0f;
+
+  Serial.write(0x57);
+  Serial.write((byte)(h14 / 1000));
+  Serial.write((byte)(h14 / 100 % 10));
+  Serial.write((byte)(h14 / 10 % 10));
+  Serial.write((byte)(h14 % 10));
+  Serial.write(DEFAULT_RESOLUTION);
+  Serial.write((byte)(v14 / 1000));
+  Serial.write((byte)(v14 / 100 % 10));
+  Serial.write((byte)(v14 / 10 % 10));
+  Serial.write((byte)(v14 % 10));
+  Serial.write(DEFAULT_RESOLUTION);
+  Serial.write(0x20);
+
+  return;
+}
+
+void setup() {
+  Serial.begin(600);
+}
+
+void loop() {
+  static byte i = 0;
+  static int h14 = 0, v14 = 0;
+  static int ph = 0, pv = 0;
+  static byte cmd;
+
+  while (Serial.available() < 1);
+  byte b = Serial.read();
+
+  switch (i) {
+    case 0:
+      if (b != 0x57) return;
+      break;
+    case 1:
+      h14 = (b - '0');
+      break;
+    case 2:
+    case 3:
+    case 4:
+      h14 = h14 * 10 + (b - '0');
+      break;
+    case 5:
+      ph = b;
+      break;
+    case 6:
+      v14 = (b - '0');
+      break;
+    case 7:
+    case 8:
+    case 9:
+      v14 = v14 * 10 + (b - '0');
+      break;
+    case 10:
+      pv = b;
+      break;
+    case 11:
+      cmd = b;
+      break;
+    case 12:
+      if (b != 0x20) {
+        i = 0;
+        return;
+      }
+  }
+
+  if (i == 12) {
+    switch (cmd) {
+      case 0x0F: // Stop
+        ns.azimuthMC->jog(0, FORWARD);
+        ns.elevationMC->jog(0, FORWARD);
+      case 0x1F: // Status
+        sendStatus();
+        break;
+      case 0x2F: // Set
+        if (ph == 0) ph = DEFAULT_RESOLUTION;
+        if (pv == 0) pv = DEFAULT_RESOLUTION;
+
+        float az = (float)h14 / ph - 360.0f;
+        float el = (float)v14 / pv - 360.0f;
+
+        ns.azimuthMC->gotoFast(TICKS_PER_DEG * az);
+        ns.elevationMC->gotoFast(TICKS_PER_DEG * el);
+        break;
+    }
+
+    i = 0;
+  } else {
+    i++;
+  }
+}


Create MPU6050_rs485_Master.ino

Create MPU6050_rs485_Master.ino

error: pathspec 'cc/MPU6050_rs485_Master.ino' did not match any file(s) known to git

--- cc/MPU6050_rs485_Master.ino
+++ cc/MPU6050_rs485_Master.ino
@@ -0,0 +1,196 @@
+
+#include <SimpleModbusMaster.h>
+#include "I2Cdev.h"
+#include "MPU6050.h"
+
+
+#include "Wire.h"
+
+#define baud 9600
+#define timeout 1000
+#define polling 100 // the scan rate
+#define retry_count 10
+
+// used to toggle the receive/transmit pin on the driver
+#define TxEnablePin 2
+
+#define LED 9
+
+// The total amount of available memory on the master to store data
+#define TOTAL_NO_OF_REGISTERS 16
+
+
+enum
+{
+  PACKET1,
+  PACKET2,
+  TOTAL_NO_OF_PACKETS // leave this last entry
+};
+
+// Create an array of Packets to be configured
+Packet packets[TOTAL_NO_OF_PACKETS];
+
+// Masters register array
+unsigned int regs[TOTAL_NO_OF_REGISTERS];
+
+
+
+int val1 = 0;
+int val2 = 0;
+int val3 = 0;
+int val4 = 0;
+long values []= {0,0,0,0};
+
+int accel_angle1;
+
+MPU6050 accelgyro;
+
+int16_t ax, ay, az;
+int16_t gx, gy, gz;
+
+#define LED_PIN 13
+bool blinkState = false;
+
+// accelerometer values
+int accel_reading;
+int accel_corrected;
+int accel_readingy;
+int accel_correctedy;
+int accel_offset = 200;
+float accel_angle;
+float accel_angley;
+float accel_scale = 1; // set to 0.01
+
+// gyro values
+int gyro_offset = 151; // 151
+int gyro_corrected;
+int gyro_reading;
+float gyro_rate;
+float gyro_scale = 0.02; // 0.02 by default - tweak as required
+float gyro_angle;
+float loop_time = 0.05; // 50ms loop
+float angle = 0.00; // value to hold final calculated gyro angle
+
+// time stamp variables
+int last_update;
+int cycle_time;
+long last_cycle = 0;
+
+
+
+
+
+void setup() {
+
+        Wire.begin();
+// initialize serial communication
+Serial.begin(9600);
+
+
+modbus_construct(&packets[PACKET1], 1, READ_HOLDING_REGISTERS, 0, 1, 0);
+  modbus_construct(&packets[PACKET2], 1, PRESET_MULTIPLE_REGISTERS, 0, 2, 0);
+
+  // Initialize the Modbus Finite State Machine
+  modbus_configure(&Serial, baud, SERIAL_8N2, timeout, polling, retry_count, TxEnablePin, packets, TOTAL_NO_OF_PACKETS, regs);
+
+
+// initialize device
+Serial.println("Initializing I2C devices...");
+accelgyro.initialize();
+
+// verify connection
+Serial.println("Testing device connections...");
+Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
+
+
+
+
+
+}
+
+
+byte old_level = 0;
+
+void loop() {
+    // read raw accel/gyro measurements from device
+    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+
+  // accelerometer_X_Axis angle calc
+  accel_reading = ax;
+  accel_corrected = accel_reading - accel_offset;
+  accel_corrected = map(accel_corrected, -16800, 16800, -90, 90);
+  accel_corrected = constrain(accel_corrected, -90, 90);
+  accel_angle = (float)(accel_corrected * accel_scale);
+  Serial.print(accel_angle);
+  Serial.print("\t");
+
+
+  // accelerometer_Y_Axis angle calc
+  accel_readingy = ay;
+  accel_correctedy = accel_readingy - accel_offset;
+  accel_correctedy = map(accel_correctedy, -16800, 16800, -90, 90);
+  accel_correctedy = constrain(accel_correctedy, -90, 90);
+  accel_angley = (float)(accel_correctedy * accel_scale);
+  Serial.print(accel_angley);
+  Serial.print("\t");
+
+
+  // gyro_Y_Axis angle calc
+  gyro_reading = gy;
+  gyro_corrected = (float)((gyro_reading/131) - gyro_offset);  // 131 is sensivity of gyro from data sheet
+  gyro_rate = (gyro_corrected * gyro_scale) * -loop_time;      // loop_time = 0.05 ie 50ms
+  gyro_angle = angle + gyro_rate;
+
+  // print values to serial monitor for checking
+  Serial.print(accel_angley);
+  Serial.print("\t");
+  Serial.print(gyro_corrected);
+  Serial.print("\t");
+  Serial.print(gyro_angle);
+  Serial.print("\t");
+  Serial.println(" ");
+
+  // blink LED to indicate activity
+  blinkState = !blinkState;
+  digitalWrite(LED_PIN, blinkState);
+
+//timestamp
+time_stamp();
+
+
+
+
+if (accel_angle < -1)
+
+{accel_angle = accel_angle*-10;
+modbus_update();
+regs[0] = accel_angle;
+regs[1] = accel_angley;
+
+}
+
+else if (accel_angley < -1)
+{accel_angley = accel_angley*-10;
+modbus_update();
+regs[0] = accel_angle;
+regs[1] = accel_angley;
+}
+
+
+
+else
+{
+modbus_update();
+regs[0] = accel_angle;
+regs[1] = accel_angley;
+}
+}
+
+void time_stamp(){
+  while ((millis() - last_cycle) < 50){
+  delay(1);
+  }
+  // once loop cycle reaches 50ms, reset timer value and continue
+  cycle_time = millis() - last_cycle;
+  last_cycle = millis();
+}