Delete distanceSensor.ino

Delete distanceSensor.ino

--- distanceSensor.ino
+++ distanceSensor.ino
@@ -1,200 +0,0 @@
-/*
-    // ----------------------------------------------------------------------
-    // --- distanceSensor.ino -----------------------------------------------
-    // ----------------------------------------------------------------------
-    // --- Distance Sensor Detector, suited for cars, etc -------------------
-    // --- Author: Jaime Martin Soler ---------------------------------------
-    // --- Date  : 31/08/2016 -----------------------------------------------
-    // ----------------------------------------------------------------------
-*/
-
-// INCLUDES -------------------------------------------------------------
-#include <LiquidCrystal.h>
-#include <C:\Users\Jaime\Documents\Arduino\test\distanceSensor\Ultrasonic.h>
-#include <C:\Users\Jaime\Documents\Arduino\test\distanceSensor\signalFunction.h>
-
-
-
-
-// PARAMETERS -----------------------------------------------------------
-
-// PINs: Ultrasonic
-//const int PIN_ULTRAS_GND = GND;
-const int PIN_ULTRAS_ECH = 13;
-const int PIN_ULTRAS_TRI = 12;
-const int PIN_ULTRAS_VCC = 8;   // BROKEN, ALWAYS VCC
-// PINs: Buzzer
-const int PIN_BUZZER_IN = 10;
-// PINs: LED
-const int PIN_LED_IN = 9; // PWM
-// PINs: Button
-const int PIN_BUTTON_IN = 11;
-// PINs: LCD
-const int PIN_LCD_RS = 7;
-const int PIN_LCD_EN = 6;
-const int PIN_LCD_D4 = 5;
-const int PIN_LCD_D5 = 4;
-const int PIN_LCD_D6 = 3;
-const int PIN_LCD_D7 = 2;
-
-// LCD properties
-const int LCD_ROWS = 2;
-const int LCD_COLS = 16;
-
-// button parameters
-int button_last = LOW;
-
-// centimeters measured
-const int rangesSize = 4;
-const short int ranges[rangesSize]    = {10, 30, 100, 200};  // cm
-const short int rangeOffset = 1;                             // cm
-const short int periods[rangesSize-1] = {200, 600, 1500};    // ms
-int rangeCurrent = -1; // ranges idx [0,rangesSize]
-int cm;                // cm measured
-
-// time
-const unsigned long loopTime = 20;          // ms
-const unsigned long LCDrefreshTime = 200;   // ms
-unsigned long LCDrefreshLast = 0;           // ms
-
-// objects
-LiquidCrystal lcd = LiquidCrystal(PIN_LCD_RS, PIN_LCD_EN, PIN_LCD_D4, PIN_LCD_D5, PIN_LCD_D6, PIN_LCD_D7);
-Ultrasonic us = Ultrasonic(PIN_ULTRAS_TRI, PIN_ULTRAS_ECH); // (Trig PIN,Echo PIN)
-SignalFunction sfBuzzer = SignalFunction();
-SignalFunction sfLED = SignalFunction();
-
-
-
-
-// SETUP ----------------------------------------------------------------
-void setup() {
-
-  //Serial.begin(9600);
-
-  // set LCD
-  lcd.begin(LCD_COLS, LCD_ROWS);
-  lcd.setCursor(0, 0);
-  lcd.print("DISTNACE");
-  lcd.setCursor(0, 1);
-  lcd.print("SENSOR");
-  lcd.display();
-  delay(1000);
-
-  // set Ultrasonic VCC, GND pins
-  pinMode(PIN_ULTRAS_VCC, OUTPUT);
-  digitalWrite(PIN_ULTRAS_VCC, HIGH);
-    //pinMode(PIN_ULTRAS_GND, OUTPUT);
-    //digitalWrite(PIN_ULTRAS_GND, LOW);
-
-  // set sfBuzzer
-  int buz_enable = ENABLE_NORMAL;
-  int buz_type = TYPE_DIGITAL;
-  int buz_period = 1000;
-  const int buz_VTsize = 3;
-  short int buz_Vn[buz_VTsize] = {  0, VnMAX, 0};
-  short int buz_Tn[buz_VTsize] = {0, 60,    80};
-  sfBuzzer.set(buz_enable, PIN_BUZZER_IN, buz_type, buz_period, buz_Vn, buz_Tn, buz_VTsize);
-
-  // set sfLED
-  int led_enable = ENABLE_NORMAL;
-  int led_type = TYPE_ANALOG;
-  int led_period = 1000;
-  const int led_VTsize = 3;
-  short int led_Vn[led_VTsize] = {0,  0, VnMAX};
-  short int led_Tn[led_VTsize] = {0, 60,    80};
-  sfLED.set(led_enable, PIN_LED_IN, led_type, led_period, led_Vn, led_Tn, led_VTsize);
-
-  // set button pin
-  pinMode(PIN_BUTTON_IN, INPUT);
-}
-
-
-
-
-// LOOP -----------------------------------------------------------------
-void loop()
-{
-  // t0 for delay
-  unsigned long t0 = millis();
-
-  // check button pause
-  int button_current = digitalRead(PIN_BUTTON_IN);
-  if (button_last==LOW && button_current==HIGH) {
-    button_last=HIGH;
-  } else if (button_last==HIGH && button_current==LOW) {
-    // pause: manage outputs
-    sfBuzzer.enable = ENABLE_ALWAYS_LOW;
-    sfLED.enable = ENABLE_ALWAYS_LOW;
-    sfBuzzer.refresh();
-    sfLED.refresh();
-    lcd.clear();
-    lcd.setCursor(0, 0);
-    lcd.print("PAUSED");
-    // pause: manage states
-    button_last=LOW;
-    while(true) {
-      delay(5);
-      button_current = digitalRead(PIN_BUTTON_IN);
-      if (button_last==LOW && button_current==HIGH) {
-        button_last=HIGH;
-      } else if (button_last==HIGH && button_current==LOW) {
-        // exit pause
-        button_last=LOW;
-        rangeCurrent=-1;  // force to update rangeCurrent
-        break;
-  } } }
-
-  // get cm
-  cm = us.Ranging(CM); // CM or INC
-
-  // get rangeCurrent, ranges[], periods[]
-  if (cm<=ranges[0]) {
-    if ((rangeCurrent<0) || (rangeCurrent>0 && abs(cm-ranges[0])>=rangeOffset)) {
-      rangeCurrent = 0;
-      sfBuzzer.enable = ENABLE_ALWAYS_HIGH;
-      sfLED.enable = ENABLE_ALWAYS_HIGH;
-    }
-  } else if (cm>ranges[rangesSize-1]) {
-    if ((rangeCurrent<0) || (rangeCurrent<rangesSize && abs(cm-ranges[rangesSize-1])>=rangeOffset)) {
-      rangeCurrent = rangesSize;
-      sfBuzzer.enable = ENABLE_ALWAYS_LOW;
-      sfLED.enable = ENABLE_ALWAYS_LOW;
-    }
-  } else {
-    for (int i=1; i<rangesSize; i++) {
-      if (cm>ranges[i-1] && cm<=ranges[i]) {
-        if ((rangeCurrent<0) || (rangeCurrent<i && abs(cm-ranges[i])>=rangeOffset) || (rangeCurrent>i && abs(cm-ranges[i])>=rangeOffset)) {
-            rangeCurrent = i;
-            sfBuzzer.enable = ENABLE_NORMAL;
-            sfLED.enable = ENABLE_NORMAL;
-            sfBuzzer.period = periods[i-1];
-            sfLED.period = periods[i-1];
-            break;
-  } } } }
-
-  // refresh signalFunctions
-  sfBuzzer.refresh();
-  sfLED.refresh();
-
-  // refresh LCD
-  if ((t0-LCDrefreshLast)>=LCDrefreshTime) {
-    LCDrefreshLast = t0;
-    String line0(""); line0+="Dist : " ; line0+=cm ; line0+=" cm";
-    String line1(""); line1+="Range: " ; line1+=rangeCurrent;
-    lcd.clear();
-    lcd.setCursor(0, 0);
-    lcd.print(line0);
-    lcd.setCursor(0, 1);
-    lcd.print(line1);
-  }
-
-  // delay
-  unsigned long tProcess = millis() - t0;
-  if (tProcess<loopTime) {
-    delay(loopTime-tProcess);
-  }
-
-  //Serial.print(tProcess); Serial.println(" us" );
-  //Serial.println();
-}
-


call the update_results method of ultrasonic_sensors_controller

this will get the results of reading ultrasonic

--- jenny5-firmware/jenny5-firmware.ino
+++ jenny5-firmware/jenny5-firmware.ino
@@ -30,7 +30,7 @@
 //--------------------------------------------------------------------------------------------
 void setup()
 {
-  strcpy(firmware_version, "2016.01.22.2");
+  strcpy(firmware_version, "2016.01.23.0");

   motors_controller.set_num_motors(2);
   ultrasonic_sensors_controller.set_num_sensors(1);
@@ -237,6 +237,12 @@
 void loop()
 {
   char serial_out[100];
+
+// check to see if there are new results from ultrasonic sensor
+  ultrasonic_sensors_controller.update_results(serial_out);
+  if (serial_out[0])
+    Serial.write(serial_out);
+
   if (Serial.available() || current_buffer[0]) {
     int num_read = 0;
     char serial_buffer[65];


preparing for interpreting multiple commands.

--- jenny5-firmware/jenny5-firmware.ino
+++ jenny5-firmware/jenny5-firmware.ino
@@ -14,7 +14,7 @@

 char is_command_running;

-char firmware_version[] = "2015.11.20.0";// year.month.day.version
+char firmware_version[] = "2015.11.20.1";// year.month.day.version

 char current_buffer[65];

@@ -45,6 +45,56 @@
   current_buffer[0] = 0;
   is_command_running = 0;
 }
+
+//--------------------------------------------------------------------------------------------
+void parse_and_execute_commands(char* tmp_str)
+{
+ // int i = 0;
+
+              if (tmp_str[0] == 'M' || tmp_str[0] == 'm'){// moves motor
+                int motor_index, num_steps;
+                sscanf(tmp_str, "%d%d", &motor_index, &num_steps);
+                motors_control.move_motor(motor_index, num_steps);
+                is_command_running = 1;
+              }
+              else
+                if (tmp_str[0] == 'D' || tmp_str[0] == 'd'){// disables motor
+                  int motor_index;
+                  sscanf(tmp_str, "%d", &motor_index);
+                  motors_control.disable_motor(motor_index);
+                }
+                else
+                  if (tmp_str[0] == 'L' || tmp_str[0] == 'l'){// locks motor
+                    int motor_index;
+                    sscanf(tmp_str, "%d", &motor_index);
+                    motors_control.lock_motor(motor_index);
+                  }
+                else
+                  if (tmp_str[0] == 'S' || tmp_str[0] == 's'){// motor speed
+                    int motor_index, motor_speed;
+                    sscanf(tmp_str, "%d%d", &motor_index, &motor_speed);
+                    motors_control.set_motor_speed(motor_index, motor_speed);
+                  }
+                else
+                  if (tmp_str[0] == 'A' || tmp_str[0] == 'a'){// motor acceleration
+                    int motor_index, motor_acceleration;
+                    sscanf(tmp_str, "%d%d", &motor_index, &motor_acceleration);
+                    motors_control.set_motor_acceleration(motor_index, motor_acceleration);
+                  }
+                else
+                  if (tmp_str[0] == 'U' || tmp_str[0] == 'u'){// ultrasonic
+                    int sensor_index;
+                    sscanf(tmp_str, "%d", &sensor_index);
+                      Serial.print(ultrasonic.Ranging());
+                  }
+                else
+                  if (tmp_str[0] == 'P' || tmp_str[0] == 'p'){// ultrasonic
+                    int sensor_index;
+                    sscanf(tmp_str, "%d", &sensor_index);
+                      Serial.print(potentiometers_control.getPotentiometerValue(sensor_index));
+                  }
+
+}
 //--------------------------------------------------------------------------------------------
 //Main loop
 void loop() {
@@ -73,7 +123,7 @@
           for (; j < buffer_length && current_buffer[j] != '#'; j++);// parse until I find the termination char
           if (j < buffer_length){
               char tmp_str[64];
-              strncpy(tmp_str, current_buffer + i + 1, j - i - 1);
+              strncpy(tmp_str, current_buffer + i, j - i);
               tmp_str[j - i - 1] = 0;

               #ifdef DEBUG
@@ -82,48 +132,6 @@
                 Serial.println();
               #endif

-              if (current_buffer[i] == 'M' || current_buffer[i] == 'm'){// moves motor
-                int motor_index, num_steps;
-                sscanf(tmp_str, "%d%d", &motor_index, &num_steps);
-                motors_control.move_motor(motor_index, num_steps);
-                is_command_running = 1;
-              }
-              else
-                if (current_buffer[i] == 'D' || current_buffer[i] == 'd'){// disables motor
-                  int motor_index;
-                  sscanf(tmp_str, "%d", &motor_index);
-                  motors_control.disable_motor(motor_index);
-                }
-                else
-                  if (current_buffer[i] == 'L' || current_buffer[i] == 'l'){// locks motor
-                    int motor_index;
-                    sscanf(tmp_str, "%d", &motor_index);
-                    motors_control.lock_motor(motor_index);
-                  }
-                else
-                  if (current_buffer[i] == 'S' || current_buffer[i] == 's'){// motor speed
-                    int motor_index, motor_speed;
-                    sscanf(tmp_str, "%d%d", &motor_index, &motor_speed);
-                    motors_control.set_motor_speed(motor_index, motor_speed);
-                  }
-                else
-                  if (current_buffer[i] == 'A' || current_buffer[i] == 'a'){// motor acceleration
-                    int motor_index, motor_acceleration;
-                    sscanf(tmp_str, "%d%d", &motor_index, &motor_acceleration);
-                    motors_control.set_motor_acceleration(motor_index, motor_acceleration);
-                  }
-                else
-                  if (current_buffer[i] == 'U' || current_buffer[i] == 'u'){// ultrasonic
-                    int sensor_index;
-                    sscanf(tmp_str, "%d", &sensor_index);
-                      Serial.print(ultrasonic.Ranging());
-                  }
-                else
-                  if (current_buffer[i] == 'P' || current_buffer[i] == 'p'){// ultrasonic
-                    int sensor_index;
-                    sscanf(tmp_str, "%d", &sensor_index);
-                      Serial.print(potentiometers_control.getPotentiometerValue(sensor_index));
-                  }

               // remove the current executed command


removed #include "ultrasonic.h" from the main ino

--- jenny5-firmware/jenny5-firmware.ino
+++ jenny5-firmware/jenny5-firmware.ino
@@ -2,7 +2,6 @@
 #include "potentiometers_controller.h"
 #include "motors_control.h"
 #include "button_controller.h"
-#include "ultrasonic.h"

 byte potentiometers_pins[4] = {0, 1, 2, 3};
 byte ultrasonic_trig_pins[2] = {48, 50};


esp32 speed enhancements that half-work, and flow control enhancements i can't test.

git-svn-id: d8bfcd070c2c81762cca1b3912331324182919ba@16963 0d6f1817-ed0e-0410-87c9-987e46238f29

--- zimodem/serout.ino
+++ zimodem/serout.ino
@@ -132,10 +132,7 @@
       if(rtsActive == 0)
         invertMask = invertMask | UART_INVERSE_CTS;
     }
-    uart_set_line_inverse(UART_NUM_2, invertMask);
-
-    debugPrintf("invert=%d\n",invertMask); //BZ:DELME
-
+    uart_set_line_inverse(UART_NUM_2, invertMask);
     //uart_set_hw_flow_ctrl(UART_NUM_2,UART_HW_FLOWCTRL_DISABLE,0);
     if(pinSupport[pinRTS])
     {


Modified prinout lines. Removed sd.end() because it (no longer?) exists.

--- eeZee_microSD/examples/Arduino/microSD_Test1/microSD_Test1.ino
+++ eeZee_microSD/examples/Arduino/microSD_Test1/microSD_Test1.ino
@@ -25,60 +25,54 @@
 {
  // Open serial communications and wait for port to open:
   Serial.begin(9600);
-   while (!Serial) {
-    ; // wait for serial port to connect. Needed for Leonardo only
-  }
+}

-
-  Serial.print("Initializing SD card...");
+void loop()
+{
   // On the Ethernet Shield, CS is pin 4. It's set as an output by default.
   // Note that even if it's not used as the CS pin, the hardware SS pin
   // (10 on most Arduino boards, 53 on the Mega) must be left as an output
   // or the SD library functions will not work.
   pinMode(10, OUTPUT);

+  delay(5000);
+
   if (!SD.begin(10)) {
-    Serial.println("initialization failed!");
-    return;
+    Serial.println("[FAIL] initialization failed.");
+  } else {
+    Serial.println("[PASS] initialization done.");
+
+    if (SD.exists("example.txt")) {
+      Serial.println("[FAIL] example.txt already exists.");
+    }
+    else {
+      Serial.println("[PASS] no example.txt, good.");
+    }
+
+    // open a new file and immediately close it:
+    myFile = SD.open("example.txt", FILE_WRITE);
+    myFile.close();
+
+    // Check to see if the file exists:
+    if (SD.exists("example.txt")) {
+      Serial.println("[PASS] example.txt created.");
+    }
+    else {
+      Serial.println("[FAIL] example.txt not created.");
+    }
+
+    // delete the file:
+    SD.remove("example.txt");
+
+    if (SD.exists("example.txt")){
+      Serial.println("[FAIL] example.txt removed.");
+    }
+    else {
+      Serial.println("[PASS] example.txt not removed.");
+    }
   }
-  Serial.println("initialization done.");
-
-  if (SD.exists("example.txt")) {
-    Serial.println("example.txt exists.");
-  }
-  else {
-    Serial.println("example.txt doesn't exist.");
-  }
-
-  // open a new file and immediately close it:
-  Serial.println("Creating example.txt...");
-  myFile = SD.open("example.txt", FILE_WRITE);
-  myFile.close();
-
-  // Check to see if the file exists:
-  if (SD.exists("example.txt")) {
-    Serial.println("example.txt exists.");
-  }
-  else {
-    Serial.println("example.txt doesn't exist.");
-  }
-
-  // delete the file:
-  Serial.println("Removing example.txt...");
-  SD.remove("example.txt");
-
-  if (SD.exists("example.txt")){
-    Serial.println("example.txt exists.");
-  }
-  else {
-    Serial.println("example.txt doesn't exist.");
-  }
-}
-
-void loop()
-{
-  // nothing happens after setup finishes.
+
 }

-
+


ManualEthernet.ino edited online with Bitbucket

--- examples/Transports/Advanced/ManualEthernet/ManualEthernet.ino
+++ examples/Transports/Advanced/ManualEthernet/ManualEthernet.ino
@@ -4,7 +4,7 @@
 #include <EthernetClient.h>
 #include <BlynkSimpleEthernet.h>

-// You should get Auth Token in Blynk App. Go to the Project Settings (nut icon)
+// You can get Auth Token for your Project in the Blynk App. Go to the Project Settings (nut icon)
 char auth[] = "YourAuthToken";

 IPAddress server_ip (10, 0, 0, 10);


The write function is currupting something. We are only keeping the read function for now.

--- BackEnd/BackEnd.ino
+++ BackEnd/BackEnd.ino
@@ -469,7 +469,7 @@
 //------------------------------------------------------------
 void initBoardStateFromFile(char* _sFullFilePath) {

-  Serial.println("Init from board file");
+//  Serial.println("Init from board file");
   // Open File
   FILE *fp;
   char sJsonFile[CONFIG_FILE_MAX_SIZE];
@@ -1723,7 +1723,7 @@

   if (millis() - g_last_print > 1000)
   {
-     writeBoardStateToFile(BOARD_CONFIG_FILE_FULL_PATH);
+     //writeBoardStateToFile(BOARD_CONFIG_FILE_FULL_PATH);
     /*
     if(g_iWriteToFile)
     {


CompleteTest.ino updated.

CompleteTest.ino updated.

--- libraries/ParkingShield/examples/CompleteTest/CompleteTest.ino
+++ libraries/ParkingShield/examples/CompleteTest/CompleteTest.ino
@@ -12,7 +12,7 @@
     Serial.begin(9600);
     Scheduler::init();
     Scheduler::addTask(output, 1000);
-    shield.decimalPointOn();
+    shield.setDecimalPoint(true);
     playMelody();
 }

@@ -106,9 +106,9 @@
 {
   for (long i = 0; i < duration * 1000L; i += tone * 2)
   {
-    shield.setBuzzer(HIGH);
+    shield.setBuzzer(true);
     delayMicroseconds(tone);
-    shield.setBuzzer(LOW);
+    shield.setBuzzer(false);
     delayMicroseconds(tone);
   }
 }
@@ -125,4 +125,4 @@
       playTone(tones[i], duration);
   }
 }
-
+


Clean up

Clean up

--- module/module.ino
+++ module/module.ino
@@ -154,7 +154,7 @@

 	// Only here temporarily for debugging
 	if (1) {
-		Serial.print(startTime); Serial.print(" "); Serial.println(elapsedTime);
+		Serial.print(startTime); Serial.print(F(" ")); Serial.println(elapsedTime);
 	}

 	while (1) {
@@ -250,7 +250,6 @@
 	    	}

 	    	digitalWrite(interlock, HIGH);
-			//delay(pollInterval);
 			lastPollTime = millis();
 		}


Testing new interlock

Testing new interlock


CAN DATA READ implemented

CAN DATA READ implemented

--- arduino_control/arduino_control.ino
+++ arduino_control/arduino_control.ino
@@ -31,6 +31,7 @@

 // Statemachine State variable and initial value
 byte State = Startup;
+byte variables = Position;

 // the cs pin of the version after v1.1 is default to D9
 // v0.9b and v1.0 is default D10
@@ -99,31 +100,32 @@
     Serial.println(" Init CAN BUS Shield again");
     delay(100);
   }
-
-  State = Startup;
+
   Serial.println("CAN BUS Shield init ok!");

-//  // TIMER SETUP- the timer interrupt allows precise timed measurements of the reed switch
-//  //for mor info about configuration of arduino timers see http://arduino.cc/playground/Code/Timer1
-//
-//  cli();//stop interrupts
-//
-//  //set timer1 interrupt at 1kHz
-//  TCCR1A = 0;// set entire TCCR1A register to 0
-//  TCCR1B = 0;// same for TCCR1B\
-//  TCNT1  = 0;//initialize counter value to 0
-//  // set timer count for 1khz increments
-//  OCR1A = 1999;// = (16*10^6) / (1000*8) - 1
-//  //had to use 16 bit timer1 for this bc 1999>255, but could switch to timers 0 or 2 with larger prescaler
-//  // turn on CTC mode
-//  TCCR1B |= (1 << WGM12);
-//  // Set CS11 bit for 8 prescaler
-//  TCCR1B |= (1 << CS11);
-//  // enable timer compare interrupt
-//  TIMSK1 |= (1 << OCIE1A);
-//
-//  sei();//allow interrupts
-//  //END TIMER SETUP
+ // TIMER SETUP- the timer interrupt allows precise timed measurements of the reed switch
+ //for mor info about configuration of arduino timers see http://arduino.cc/playground/Code/Timer1
+
+ cli();//stop interrupts
+
+ //set timer1 interrupt at 1kHz
+ TCCR1A = 0;// set entire TCCR1A register to 0
+ TCCR1B = 0;// same for TCCR1B\
+ TCNT1  = 0;//initialize counter value to 0
+ // set timer count for 1khz increments
+
+ // OCR1A 1999 to 1kHZ, 19999 to 0.1kHz
+ OCR1A = 19999;// = (16*10^6) / (1000*8) - 1
+ //had to use 16 bit timer1 for this bc 1999>255, but could switch to timers 0 or 2 with larger prescaler
+ // turn on CTC mode
+ TCCR1B |= (1 << WGM12);
+ // Set CS11 bit for 8 prescaler
+ TCCR1B |= (1 << CS11);
+ // enable timer compare interrupt
+ TIMSK1 |= (1 << OCIE1A);
+
+ sei();//allow interrupts
+ //END TIMER SETUP

   Serial.println("Interrupt init ok!");
 }
@@ -147,7 +149,7 @@
   CAN.sendMsgBuf(0x601, 0, 8, set_max_profile_velocity);
   Serial.println("Max Profile Velocity set as 2000rpm");
   delay(10);
-  State = Operational;
+  State = Pre_Operational;
 }

 //***************
@@ -156,14 +158,14 @@
 void PDOConfig(void) {

   CAN.sendMsgBuf(0x00, 0, 2, set_pre_operational);
-  delay(10);
+  delay(100);
   CAN.sendMsgBuf(0x601, 0, 8, pdo_actual_position_1);
-  delay(10);
+  delay(100);
   CAN.sendMsgBuf(0x601, 0, 8, pdo_actual_position_2);
-  delay(10);
-  CAN.sendMsgBuf(0x601, 0, 8, pdo_actual_velocity_1);
-  delay(10);
-  CAN.sendMsgBuf(0x601, 0, 8, pdo_actual_velocity_2);
+  delay(100);
+  // CAN.sendMsgBuf(0x601, 0, 8, pdo_actual_velocity_1);
+  // delay(10);
+  // CAN.sendMsgBuf(0x601, 0, 8, pdo_actual_velocity_2);
   delay(10);
   CAN.sendMsgBuf(0x601, 0, 8, pdo_actual_current_1);
   delay(10);
@@ -176,7 +178,7 @@
   CAN.sendMsgBuf(0x601, 0, 8, pdo_actual_current_5);
   delay(10);
   CAN.sendMsgBuf(0x00, 0, 2, set_operational);
-  delay(10);
+  delay(100);

   Serial.println("TPDO Configured!");

@@ -193,14 +195,14 @@

 //int tests = 0;

-////******
-//// INTERRUPTION
-////******
-//ISR(TIMER1_COMPA_vect) { //timer1 interrupt 1Hz toggles pin 13 (LED)
-//
-//  tests = tests + 1;
-//
-//}
+//******
+// INTERRUPTION
+//******
+ISR(TIMER1_COMPA_vect) { //timer1 interrupt 1Hz toggles pin 13 (LED)
+
+ sync();
+
+}

 //*******************
 // POSITION SETPOINT
@@ -222,9 +224,9 @@
   delay(10);
 }

-//***************
-// DATA READ
-//***************
+//******************************
+// AMPLIFICATION BOARD DATA READ
+//******************************
 float amplificationBoardDataRead()
 {
   unsigned char len = 0;
@@ -247,16 +249,16 @@
     loadcell_data = loadcell_data | buf[2];
     loadcell_data <<= 8;
     loadcell_data = loadcell_data | buf[3];
-//
-//    if (buf[1] >= 128) {
-//           loadcell_data = loadcell_data - 16777216;
-//    }
-
-  loadcell_data_double = loadcell_data;
-//  loadcell_data_double = loadcell_data_double-4294000000;
-
-//    Serial.print("Encoder: ");
-//    Serial.println(encoder_data, DEC);
+    //
+    //    if (buf[1] >= 128) {
+    //           loadcell_data = loadcell_data - 16777216;
+    //    }
+
+    loadcell_data_double = loadcell_data;
+    //  loadcell_data_double = loadcell_data_double-4294000000;
+
+    //    Serial.print("Encoder: ");
+    //    Serial.println(encoder_data, DEC);

      Serial.print("Loadcell: ");
      Serial.println(loadcell_data_double);
@@ -265,13 +267,16 @@
   }
 }

+//******************************
+// CURRENT DATA READ
+//******************************
 float currentDataRead()
 {
   current_data = 0;
   unsigned char len = 0;
   unsigned char buf[8];

-// clear the string:
+  // clear the string:
   sync();

   if (CAN_MSGAVAIL == CAN.checkReceive())           // check if data coming
@@ -301,7 +306,61 @@

     return(current_data);
   }
-
+}
+
+//****************
+// CAN DATA READ
+//****************
+void CANDataRead()
+{
+  unsigned char len = 0;
+  unsigned char buf[8];
+
+  if(CAN_MSGAVAIL == CAN.checkReceive())            // check if data coming
+  {
+    CAN.readMsgBuf(&len, buf);    // read data,  len: data length, buf: data buf
+    unsigned int canId = CAN.getCanId();
+
+    switch (canId)
+    {
+      case 0x381:
+        Serial.println("-----------------------------");
+        Serial.print("POSICAO");
+        Serial.println(canId, HEX);
+
+        for(int i = 0; i<len; i++)    // print the data
+        {
+            Serial.print(buf[i], HEX);
+            Serial.print("\t");
+        }
+        Serial.println();
+        break;
+      case 0x321:
+        Serial.println("-----------------------------");
+        Serial.print("AMPLIFICACAO");
+        Serial.println(canId, HEX);
+
+        for(int i = 0; i<len; i++)    // print the data
+        {
+            Serial.print(buf[i], HEX);
+            Serial.print("\t");
+        }
+        Serial.println();
+        break;
+      case 0x181:
+        Serial.println("-----------------------------");
+        Serial.print("CORRENTE");
+        Serial.println(canId, HEX);
+
+        for(int i = 0; i<len; i++)    // print the data
+        {
+            Serial.print(buf[i], HEX);
+            Serial.print("\t");
+        }
+        Serial.println();
+        break;
+    }
+  }
 }

 void loop()
@@ -311,10 +370,14 @@
     case Startup:
       doStartup();
       break;
+    case Pre_Operational:
+      PDOConfig();
+      break;
     case Operational:
-//       amplificationBoardDataRead();
-      currentDataRead();
-//       positionSetpoint(encoder_data);
+      // amplificationBoardDataRead();
+//       currentDataRead();
+    CANDataRead();
+      // positionSetpoint(encoder_data);
       break;
   }
   delay(100);


turn tabs to spaces

turn tabs to spaces

--- drone/WebServerESC/WebServerESC.ino
+++ drone/WebServerESC/WebServerESC.ino
@@ -6,7 +6,7 @@
 #include <Servo.h>
 #include <WiServer.h>

-byte thestart = 0;
+byte thestart = 0;

 #define WIRELESS_MODE_INFRA	1
 #define WIRELESS_MODE_ADHOC	2
@@ -25,10 +25,10 @@
 // WEP 128-bit keys
 // sample HEX keys
 prog_uchar wep_keys[] PROGMEM = { 0x83, 0x7A, 0xAF, 0x74, 0xEC,	// Key 0
-				  0x5F, 0xE5, 0x65, 0x9F, 0xF2,	// Key 1
-				  0x69, 0x61, 0x53, 0x1C, 0x51, // Key 2
-				  0xB5, 0x6F, 0xBE, 0xCB, 0x6B,	// Key 3
-				};
+                                  0x5F, 0xE5, 0x65, 0x9F, 0xF2,	// Key 1
+                                  0x69, 0x61, 0x53, 0x1C, 0x51, // Key 2
+                                  0xB5, 0x6F, 0xBE, 0xCB, 0x6B,	// Key 3
+                                };

 // setup the wireless mode
 // infrastructure - connect to AP
@@ -42,36 +42,35 @@
 // This is our page serving function that generates web pages
 boolean sendMyPage(char* URL)
 {
-
     // Check if the requested URL matches "/"
     if (strcmp(URL, "/") == 0) {
         _webpageHelper();
         // URL was recognized
         return true;
     }
-
+
     if (strcmp (URL, "/?OPERATION=OFF") == 0) {
         thestart = 1;
         _webpageHelper();
-	    move_to_start();
-	    return (true);
+        move_to_start();
+        return (true);
     }
-
-
-	if (strcmp (URL, "/?OPERATION=ON") == 0) {
+
+
+    if (strcmp (URL, "/?OPERATION=ON") == 0) {
         thestart = 0;
         _webpageHelper();
-	    move_to_end();
-	    return (true);
+        move_to_end();
+        return (true);
     }

     if (strcmp (URL, "/?OPERATION=90") == 0) {
-	    thestart = 0;
-	    _webpageHelper();
+        thestart = 0;
+        _webpageHelper();
         move_to_ninty();
-	    return (true);
-	}
+        return (true);
+    }

     // URL not found
     return false;
@@ -97,23 +96,18 @@
 }

 void setup() {
-
-  // Put the motor to Arduino pin #9
-  myMotor.attach(8);
-  arm();
-  myMotor.write(70);
-
-  // Required for I/O from Serial monitor
-  Serial.begin(57600);
-  Serial.println("Starting WiFi");
+    WiServer.enableVerboseMode(true);

-  // Initialize WiServer and have it use the sendMyPage function to serve pages
-  WiServer.init(sendMyPage);
-
-  // Enable Serial output and ask WiServer to generate log messages (optional)
- // Serial.begin(9600);  // ...set up the serial ouput on 0004 style
-  //Serial.begin(57600);
-  WiServer.enableVerboseMode(false);
+    // Put the motor to Arduino pin #9
+    myMotor.attach(8);
+    arm();
+    myMotor.write(70);
+    // Required for I/O from Serial monitor
+    Serial.begin(57600);
+     Serial.println("Starting WiFi");
+
+     // Initialize WiServer and have it use the sendMyPage function to serve pages
+    WiServer.init(sendMyPage);
 }

 void loop()
@@ -126,14 +120,15 @@

 void _webpageHelper()
 {
-	WiServer.print ("");
-	WiServer.print ("");
+    WiServer.print ("");
+    WiServer.print ("");
     WiServer.print ("<meta http-equiv=""content-type"" content=""text/html; charset=utf-8"" />");
     //WiServer.print ("<meta http-equiv=""refresh"" content=""25;url=http://192.168.1.60"" />");
     WiServer.print ("<title>OneButtonAlert Device</title>");
     //WiServer.print ("<link rel=""stylesheet"" type=""text/css"" media=""One Button Alert"" href=""http://onebuttonalert.com/buttonpressed/demo.css"" />");

-	WiServer.print ("");
+    WiServer.print ("");
+
     WiServer.print ("
");

@@ -179,7 +174,7 @@
     myMotor.write(80);
     //delay(500); // delay 1000ms, this allows the previous instruction to be finished.
 }
-
+

 void move_to_end()
 {
@@ -190,3 +185,4 @@
 {
     myMotor.write(90);
 }
+


last changes before the Arduino Micro got shorted ..

.. because of incorrect wireing.

TODO:
- check fixed wireing and get a new Ardunio Micro

--- tm.ino
+++ tm.ino
@@ -50,6 +50,10 @@
         Serial.print(", outside");
         st = "outside";
     }
+    else {
+        Serial.print("");
+        st = "unknown";
+    }

     if (s1 == "") {
         s1 = st;


Document more code

Document more code

--- Guzheng/Guzheng.ino
+++ Guzheng/Guzheng.ino
@@ -1,13 +1,16 @@
-/* Code for laser GuZheng
+/* Code for laser guzheng
  * Copyright (C) 2016 Jason Lu
  *
  * Licensed under the MIT License
+ * This code has NO WARRANTY! You are
+ * responsible for anything that happens
  */

 // Port I/O defines
 #define THRESHOLD 700
 #define LASTPORT 7
 #define INSTRUMENTBUTTON 8
+#define NOTEBUTTON 9

 // MIDI property defines
 #define VOLUME 0x7F
@@ -218,10 +221,12 @@
 {
   // Initialize serial at 115200 baud
   Serial.begin(115200);
+  // Set the 74HC4051 command ports
   pinMode(2,OUTPUT);
   pinMode(3,OUTPUT);
   pinMode(4,OUTPUT);
   // Wait 1 second for ttymidi to come up
+  // Could be unnecessary, but better safe than sorry
   delay(1000);
 }

@@ -230,25 +235,33 @@
   // Main loop
   // LASTPORT+1 ports, from 0 to LASTPORT
   for (int i = 0; i <= LASTPORT; i++) {
-    r0 = bitRead(i,0);    // use this with arduino 0013 (and newer versions)
-    r1 = bitRead(i,1);    // use this with arduino 0013 (and newer versions)
-    r2 = bitRead(i,2);    // use this with arduino 0013 (and newer versions)
+    /* The following three lines convert the port we want
+     * into the correct bit selections for the 74HC4051. I'm
+     * not really sure exactly how it works, but I'm pretty
+     * sure it's some bit shifting
+     */
+    r0 = bitRead(i,0);
+    r1 = bitRead(i,1);
+    r2 = bitRead(i,2);
+    // Select the correct input using the values we calculated above
     digitalWrite(2, r0);
     digitalWrite(3, r1);
     digitalWrite(4, r2);
+    // Detect if the instrument selection button is pressed
     if(digitalRead(INSTRUMENTBUTTON) == HIGH) {
-      // Make sure that this is the first time that the button
-      // has been pressed
+      /* Make sure that this is the first time that the button
+       * has been pressed
+       */
       if (instrumentButtonState == 0) {
+        if (currentInstrument >= (NUMINSTRUMENTS - 1)) {
+          // Max number of instruments, so wrap around to 0
+          currentInstrument = 0;
+        } else {
+          // Select next instrument
+          currentInstrument++;
+        }
         // Send MIDI command to change instrument
         Serial.write(CHANGEINSTRUMENT);
-        // Max number of instruments, so wrap around
-        if (currentInstrument >= (NUMINSTRUMENTS - 1)) {
-          currentInstrument = 0;
-        } else {
-          // Increment current instrument
-          currentInstrument++;
-        }
         // Write the current instrument
         Serial.write(instrumentArray[currentInstrument]);
         // Set the button state so this won't fire when holding down
@@ -259,10 +272,13 @@
       // Button is no longer pressed, so reset the button state.
       instrumentButtonState = 0;
     }
-    if(digitalRead(9) == HIGH) {
+    // Detect if note button has been pressed
+    if(digitalRead(NOTEBUTTON) == HIGH) {
       // Make sure that this is the first time that the button
       // has been pressed
       if (noteButtonState == 0) {
+        // Swap the current note set
+        // TODO: FIgure out a better way to do this
         if (noteSet == 1) {
           noteSet = 2;
         } else {


add break and minor fix in current_sketch

--- src/current_sketch.ino
+++ src/current_sketch.ino
@@ -132,6 +132,7 @@

         case 52:
             lcd.leftToRight();
+            break;

         case 53:
             lcd.rightToLeft();
@@ -191,12 +192,15 @@

             // current sensor
         case 86:
-            int cur = analogRead(CURRENT_SENSOR_PIN);
-            Serial.write(analogRead(CURRENT_SENSOR_PIN));
-            break;
+            {int opt = analogRead(CURRENT_SENSOR_PIN);
+            Serial.write(opt & 0x00FF);
+            Serial.write(opt & 0xFF00);
+            break;
+            }

         default:
             Serial.print("ERR");
+            Serial.print(func);
             break;
     }
 }


オリジナルのtensen関数を追加してみた

オリジナルのtensen関数を追加してみた

--- arduino-neopixel.ino
+++ arduino-neopixel.ino
@@ -20,17 +20,36 @@
 void loop() {
   // Some example procedures showing how to display to the pixels:
 //    colorWipe(strip.Color(0, 0, 0), DELAY_TIME); // Black
-  colorWipe(strip.Color(MAX_VAL, 0, 0), DELAY_TIME); // Red
-  colorWipe(strip.Color(0, MAX_VAL, 0), DELAY_TIME); // Green
-  colorWipe(strip.Color(MAX_VAL, MAX_VAL, 0), DELAY_TIME); // Yellow
-  colorWipe(strip.Color(0, 0, MAX_VAL), DELAY_TIME); // Blue
-  colorWipe(strip.Color(MAX_VAL, 0, MAX_VAL), DELAY_TIME); // Purple
-  colorWipe(strip.Color(0, MAX_VAL, MAX_VAL), DELAY_TIME); // Cyan
-  colorWipe(strip.Color(MAX_VAL, MAX_VAL, MAX_VAL), DELAY_TIME); // White
-  rainbow(DELAY_TIME2);
-  rainbowCycle(DELAY_TIME2);
+//  colorWipe(strip.Color(MAX_VAL, 0, 0), DELAY_TIME); // Red
+//  colorWipe(strip.Color(0, MAX_VAL, 0), DELAY_TIME); // Green
+//  colorWipe(strip.Color(MAX_VAL, MAX_VAL, 0), DELAY_TIME); // Yellow
+//  colorWipe(strip.Color(0, 0, MAX_VAL), DELAY_TIME); // Blue
+//  colorWipe(strip.Color(MAX_VAL, 0, MAX_VAL), DELAY_TIME); // Purple
+//  colorWipe(strip.Color(0, MAX_VAL, MAX_VAL), DELAY_TIME); // Cyan
+//  colorWipe(strip.Color(MAX_VAL, MAX_VAL, MAX_VAL), DELAY_TIME); // White
+//  rainbow(DELAY_TIME2);
+//  rainbowCycle(DELAY_TIME2);
+
+  tensen1(strip.Color(MAX_VAL, 0, 0), DELAY_TIME); // Red
+  tensen2(strip.Color(MAX_VAL, MAX_VAL, 0), DELAY_TIME); // Yellow
+  tensen1(strip.Color(0, 0, 0), DELAY_TIME);
+  tensen2(strip.Color(0, 0, 0), DELAY_TIME); // Yellow
 }
-
+
+void tensen1(uint32_t c, uint8_t wait) {
+  for(uint16_t i=0; i<strip.numPixels(); i=i+2) {
+      strip.setPixelColor(i, c);
+      strip.show();
+      delay(wait);
+  }
+}
+void tensen2(uint32_t c, uint8_t wait) {
+  for(uint16_t i=1; i<strip.numPixels(); i=i+2) {
+      strip.setPixelColor(i, c);
+      strip.show();
+      delay(wait);
+  }
+}
 // Fill the dots one after the other with a color
 void colorWipe(uint32_t c, uint8_t wait) {
   for(uint16_t i=0; i<strip.numPixels(); i++) {


Update Computational_Firmware.ino

Update Computational_Firmware.ino

--- Current_Designs/Computational_Datalogger/Firmware/Computational_Firmware/Computational_Firmware.ino
+++ Current_Designs/Computational_Datalogger/Firmware/Computational_Firmware/Computational_Firmware.ino
@@ -1,7 +1,7 @@
 // Firmware for the CIWS Residential Datalogger
 // Arduino IDE ver. 1.8.9
 // Utah Water Research Lab
-// Updated: 9/10/2019
+// Updated: 9/11/2019
 // Daniel Henshaw and Josh Tracy

 /*****************************************************************************************************************\


Discovered the "volatile" keyword and why I should have used it before for my interrupt handling

--- Bike/Bike.ino
+++ Bike/Bike.ino
@@ -56,20 +56,9 @@
 #define changeSecondLine 5

 // Global Vars
-int nbRotation = 0;
-unsigned int rotationCount = 0;
+unsigned int nbRotation = 0;
 unsigned int updateCount = 0;
-// Distance in meters
 unsigned int totalDistance = 0;
-float currentSpeed = 0;
-
-boolean done = false;
-boolean uploaded = false;
-boolean backlight = false;
-boolean paused = false;
-boolean resetRequested = false;
-boolean start = false;
-unsigned long lastReedPress = 0;
 unsigned long lastUpdate = 0;
 unsigned long currentTime = 0;
 unsigned long startTime = 0;
@@ -77,8 +66,17 @@
 unsigned long exitLoop = 0;
 unsigned long time_elasped = 0;
 unsigned long effectiveTime = 0;
+volatile boolean backlight = false;
+volatile boolean paused = false;
+volatile boolean resetRequested = false;
+volatile boolean start = false;
+volatile unsigned long lastReedPress = 0;
+volatile unsigned int rotationCount = 0;
 // For display during init and sending via API
 String startTimeStr;
+float currentSpeed = 0;
+boolean done = false;
+boolean uploaded = false;

 #include "display.h"

@@ -100,7 +98,7 @@
   lcd.print("Getting IP...");

   // Reed switch handling by interrupt
-  attachInterrupt(0, turnCounter, FALLING);
+  attachInterrupt(0, turnCounter, RISING);

   // start Ethernet
   if (Ethernet.begin(mac) == 0) {


More lint

More lint

--- turboencabulator/turboencabulator.ino
+++ turboencabulator/turboencabulator.ino
@@ -59,13 +59,16 @@
 #define PULSEWIDTH_MIN 2
 #define PULSEWIDTH_MAX 300

+// 1 will have pulse readout in ms, 0 is hz
+#define READOUT_MS 1
+
 int clamp_pulse_width(float nominal_width) {
   return (int) constrain(nominal_width, PULSEWIDTH_MIN, PULSEWIDTH_MAX);
 }

 #include "midi_constants.h"

-volatile uint32_t pulse_duty_cycle_setpoint          = 10000;
+volatile uint32_t pulse_duty_cycle_setpoint          = 10000; // NOTE(meawoppl) - units of microseconds?
 volatile uint32_t old_pulse_duty_cycle_setpoint      = 0;
 volatile uint16_t interrupter_pulsewidth_setpoint    = 100;
 volatile uint8_t  system_mode                        = 0;     // System Mode, 2 = USB, 1 = MIDI-RX, 0 = Clock
@@ -88,10 +91,6 @@
 }

 uint8_t note_scheduler[4] = {0, 0, 0, 0};   // Note scheduling array
-
-/*
-    Setup / Init. This runs only once
-*/

 void setup() {
   /* Pullups, impedances etc */
@@ -199,17 +198,22 @@
       vfd.setCursor(8, 1);
       vfd.print("T:      ");
       vfd.setCursor(10, 1);
-
-      vfd.print(pulse_duty_cycle_setpoint / 1000);
-      vfd.print("ms  ");
-
-
-      //vfd.print(( (float) 1 / ((float)(pulse_duty_cycle_setpoint / (float)1000000))  ));
-      //vfd.print("Hz   ");
+
+      // NOTE (meawoppl) - This changes the readout between ms and Hz
+      if (READOUT_MS) {
+        vfd.print(pulse_duty_cycle_setpoint / 1000);
+        vfd.print("ms  ");
+      } else {
+        vfd.print(( (float) 1 / ((float)(pulse_duty_cycle_setpoint / (float) 1000000))  ));
+        vfd.print("Hz   ");
+      }

       pulse_duty_cycle_setpoint = map(analogRead(duty_cycle_pot), 128, 0, 10000, 100000);

-      if( (old_pulse_duty_cycle_setpoint + 2000) < pulse_duty_cycle_setpoint || (old_pulse_duty_cycle_setpoint - 2000) > pulse_duty_cycle_setpoint ) {
+      // NOTE (meawoppl) - The intention of this code appears to be only updating
+      // The duty cycle if it has changed by more than 2000 (ms?)
+      if( (old_pulse_duty_cycle_setpoint + 2000) < pulse_duty_cycle_setpoint ||
+          (old_pulse_duty_cycle_setpoint - 2000) > pulse_duty_cycle_setpoint ) {
         old_pulse_duty_cycle_setpoint = pulse_duty_cycle_setpoint;
         timer_0.end();
         delay(10);
@@ -220,8 +224,6 @@
     timer_0.end();
   };
 };
-
-

 void common() {
   // Read the potentiometers, and set the pulsewidth setpoint


Revert "Power management hardware added"

This reverts commit cb86de2a99a9806fc599f56778dd639688bd66f5.

--- RobotBase.ino
+++ RobotBase.ino
@@ -18,11 +18,6 @@
 #define RC_RX_PIN 12
 #define RC_TX_PIN 11
 RoboClaw roboclaw(RC_RX_PIN, RC_TX_PIN);
-
-//
-#define AUX_POWER_PIN 7
-#define MOTOR_POWER_PIN 8
-#define UPS_POWER_PIN 9

 // common vars //
 unsigned long lastCommandTime;
@@ -55,16 +50,10 @@
   pinMode(4, INPUT_PULLUP);
   pinMode(5, INPUT_PULLUP);
   pinMode(6, INPUT_PULLUP);
+  pinMode(7, INPUT_PULLUP);
+  pinMode(8, INPUT_PULLUP);
+  pinMode(9, INPUT_PULLUP);
   pinMode(10, INPUT_PULLUP);
-
-  pinMode(MOTOR_POWER_PIN, OUTPUT);
-  digitalWrite(MOTOR_POWER_PIN, LOW);
-  pinMode(UPS_POWER_PIN, OUTPUT);
-  digitalWrite(UPS_POWER_PIN, LOW);
-  pinMode(AUX_POWER_PIN, OUTPUT);
-  digitalWrite(AUX_POWER_PIN, HIGH);
-  delay(20);
-  digitalWrite(AUX_POWER_PIN, LOW);

   // start ports
   roboclaw.begin(38400);
@@ -131,7 +120,6 @@

 void CheckCommandTimeout()
 {
-  if (cmdTimeout == 0) return;
   commandTime = millis();
   if (commandTime >= lastCommandTime) commandTime -= lastCommandTime; else lastCommandTime = 0;
   if (commandTime > cmdTimeout) OnDisconnected();
@@ -180,9 +168,7 @@
   else
   if (cmd == 'V' || cmd == 'v') ver();
   else
-  if (cmd == 'P' || cmd == 'p') power();
-  else
-  if (cmd == 'S' || cmd == 's') settings();
+  if (cmd == 'P' || cmd == 'p') param();
   else
   if (cmd == 'B' || cmd == 'b') battery();
   else
@@ -331,21 +317,9 @@
   Serial.println("V2");
 }

-// power control command
-// ex: PU - full shutdown (UPS shutdown)
-void power()
-{
-  cmd = buf[1];
-  if (cmd == 'A' || cmd == 'a') { digitalWrite(AUX_POWER_PIN, HIGH); delay(50); digitalWrite(AUX_POWER_PIN, LOW); delay(2000); digitalWrite(AUX_POWER_PIN, HIGH); delay(50); digitalWrite(AUX_POWER_PIN, LOW); }
-  else
-  if (cmd == 'M' || cmd == 'm') { digitalWrite(MOTOR_POWER_PIN, HIGH); delay(50); digitalWrite(MOTOR_POWER_PIN, LOW); }
-  else
-  if (cmd == 'U' || cmd == 'u') { digitalWrite(UPS_POWER_PIN, HIGH); delay(5500); digitalWrite(UPS_POWER_PIN, LOW); }
-}
-
-// 'settings' command
-// ex: ST2500 - set timeout to 2500ms. 0 to disable
-void settings()
+// 'set parameter' command
+// ex: PT2500
+void param()
 {
   cmd = buf[1];
   // command timeout
@@ -357,7 +331,7 @@
 void battery()
 {
   //unsigned int voltage = (unsigned int)roboclaw.ReadMainBatteryVoltage(RC_ADDR, 0);
-  unsigned int voltage = (unsigned int)analogRead(0) * 312 >> 6;
+  int voltage = analogRead(0) * 4.8828;
   Serial.write('B');
   Serial.println(voltage);
 }


run multiple commands in parallel - not tested

--- jenny5-firmware/jenny5-firmware.ino
+++ jenny5-firmware/jenny5-firmware.ino
@@ -14,7 +14,7 @@

 char is_command_running;

-char firmware_version[] = "2015.11.20.1";// year.month.day.version
+char firmware_version[] = "2015.11.20.2";// year.month.day.version

 char current_buffer[65];

@@ -47,53 +47,64 @@
 }

 //--------------------------------------------------------------------------------------------
-void parse_and_execute_commands(char* tmp_str)
+void parse_and_execute_commands(char* tmp_str, byte str_length)
 {
- // int i = 0;
-
-              if (tmp_str[0] == 'M' || tmp_str[0] == 'm'){// moves motor
-                int motor_index, num_steps;
-                sscanf(tmp_str, "%d%d", &motor_index, &num_steps);
-                motors_control.move_motor(motor_index, num_steps);
-                is_command_running = 1;
-              }
-              else
-                if (tmp_str[0] == 'D' || tmp_str[0] == 'd'){// disables motor
-                  int motor_index;
-                  sscanf(tmp_str, "%d", &motor_index);
-                  motors_control.disable_motor(motor_index);
-                }
-                else
-                  if (tmp_str[0] == 'L' || tmp_str[0] == 'l'){// locks motor
-                    int motor_index;
-                    sscanf(tmp_str, "%d", &motor_index);
-                    motors_control.lock_motor(motor_index);
-                  }
-                else
-                  if (tmp_str[0] == 'S' || tmp_str[0] == 's'){// motor speed
-                    int motor_index, motor_speed;
-                    sscanf(tmp_str, "%d%d", &motor_index, &motor_speed);
-                    motors_control.set_motor_speed(motor_index, motor_speed);
-                  }
-                else
-                  if (tmp_str[0] == 'A' || tmp_str[0] == 'a'){// motor acceleration
-                    int motor_index, motor_acceleration;
-                    sscanf(tmp_str, "%d%d", &motor_index, &motor_acceleration);
-                    motors_control.set_motor_acceleration(motor_index, motor_acceleration);
-                  }
-                else
-                  if (tmp_str[0] == 'U' || tmp_str[0] == 'u'){// ultrasonic
-                    int sensor_index;
-                    sscanf(tmp_str, "%d", &sensor_index);
-                      Serial.print(ultrasonic.Ranging());
-                  }
-                else
-                  if (tmp_str[0] == 'P' || tmp_str[0] == 'p'){// ultrasonic
-                    int sensor_index;
-                    sscanf(tmp_str, "%d", &sensor_index);
-                      Serial.print(potentiometers_control.getPotentiometerValue(sensor_index));
-                  }
-
+  byte i = 0;
+  while (i < str_length){
+    if (current_buffer[i] >= 'A' && current_buffer[i] <= 'Z' || current_buffer[i] >= 'a' && current_buffer[i] <= 'z'){
+      if (tmp_str[i] == 'M' || tmp_str[i] == 'm'){// moves motor
+        int motor_index, num_steps;
+        sscanf(tmp_str + i + 1, "%d%d", &motor_index, &num_steps);
+        motors_control.move_motor(motor_index, num_steps);
+        is_command_running = 1;
+        i += 3;
+      }
+      else
+        if (tmp_str[i] == 'D' || tmp_str[i] == 'd'){// disables motor
+          int motor_index;
+          sscanf(tmp_str + i + 1, "%d", &motor_index);
+          motors_control.disable_motor(motor_index);
+          i++;
+        }
+        else
+          if (tmp_str[i] == 'L' || tmp_str[i] == 'l'){// locks motor
+            int motor_index;
+            sscanf(tmp_str + i + 1, "%d", &motor_index);
+            motors_control.lock_motor(motor_index);
+            i++;
+          }
+        else
+          if (tmp_str[i] == 'S' || tmp_str[i] == 's'){// motor speed
+            int motor_index, motor_speed;
+            sscanf(tmp_str + i + 1, "%d%d", &motor_index, &motor_speed);
+            motors_control.set_motor_speed(motor_index, motor_speed);
+            i += 3;
+          }
+        else
+          if (tmp_str[i] == 'A' || tmp_str[i] == 'a'){// motor acceleration
+            int motor_index, motor_acceleration;
+            sscanf(tmp_str + i + 1, "%d%d", &motor_index, &motor_acceleration);
+            motors_control.set_motor_acceleration(motor_index, motor_acceleration);
+            i += 3;
+          }
+        else
+          if (tmp_str[i] == 'U' || tmp_str[i] == 'u'){// ultrasonic
+            int sensor_index;
+            sscanf(tmp_str + i + 1, "%d", &sensor_index);
+            Serial.print(ultrasonic_sensors_controller.getDistanceForSensor(sensor_index));
+            i++;
+          }
+        else
+          if (tmp_str[i] == 'P' || tmp_str[i] == 'p'){// potentiometer
+            int sensor_index;
+            sscanf(tmp_str + i + 1, "%d", &sensor_index);
+            Serial.print(potentiometers_control.getPotentiometerValue(sensor_index));
+            i++;
+          }
+    }
+    else
+      i++;
+  }
 }
 //--------------------------------------------------------------------------------------------
 //Main loop
@@ -124,7 +135,7 @@
           if (j < buffer_length){
               char tmp_str[64];
               strncpy(tmp_str, current_buffer + i, j - i);
-              tmp_str[j - i - 1] = 0;
+              tmp_str[j - i] = 0;

               #ifdef DEBUG
                 Serial.write("current command is=");
@@ -132,7 +143,7 @@
                 Serial.println();
               #endif

-
+              parse_and_execute_commands(tmp_str, j - i);

               // remove the current executed command
               strcpy(current_buffer, current_buffer + j + 1);// not sure if this is good


Added output pins + flashing of route switch led when it needs switching

--- signalboxes/kelberg/kelberg.ino
+++ signalboxes/kelberg/kelberg.ino
@@ -28,10 +28,11 @@
 // Push button for route fixing
 int PIN_BUTTON_ROUTE_A_FIXATION = 46;
 // Output pins (pull to 0 means go out of the ground position)
-int PIN_OUTPUT_SWITCH_5 = 2;
-int PIN_OUTPUT_SWITCH_1 = 3;
-int PIN_OUTPUT_SIGNAL_A1 = 4;
-int PIN_OUTPUT_SIGNAL_A2 = 5;
+int PIN_OUTPUT_SWITCH_1 = A1;
+int PIN_OUTPUT_SWITCH_5 =A2;
+int PIN_OUTPUT_SIGNAL_A1 = A5;
+int PIN_OUTPUT_SIGNAL_A2 = A4;
+

 // Array index numbers. These are columns in the locking table.
 int SWITCH_5 = 0;
@@ -171,24 +172,56 @@
 }

+void update_outputs() {
+  // Active: pull to zero.
+  if (current_position[SWITCH_1] == PLUS) {
+    digitalWrite(PIN_OUTPUT_SWITCH_1, HIGH);
+  }
+  else {
+    digitalWrite(PIN_OUTPUT_SWITCH_1, LOW);
+  }
+  if (current_position[SWITCH_5] == PLUS) {
+    digitalWrite(PIN_OUTPUT_SWITCH_5, HIGH);
+  }
+  else {
+    digitalWrite(PIN_OUTPUT_SWITCH_5, LOW);
+  }
+  if (current_position[SIGNAL_A1] == PLUS) {
+    digitalWrite(PIN_OUTPUT_SIGNAL_A1, HIGH);
+  }
+  else {
+    digitalWrite(PIN_OUTPUT_SIGNAL_A1, LOW);
+  }
+  if (current_position[SIGNAL_A2] == PLUS) {
+    digitalWrite(PIN_OUTPUT_SIGNAL_A2, HIGH);
+  }
+  else {
+    digitalWrite(PIN_OUTPUT_SIGNAL_A2, LOW);
+  }
+}
+
+
 void change_position(int number,
                      boolean new_position) {
-  // DEBUG LED TO SHOF IF POSITION-CHANGE happens correctly.
-  if (new_position == MINUS) {
-    digitalWrite(13, HIGH);
-  }
-  else {
-    digitalWrite(13, LOW);
-  }

   current_position[number] = new_position;
   position_is_ok[number] = true;
+
   update_requirements();
   update_blocks();
+  update_outputs();

   // Special case: signals going back to PLUS free the route fixation.
+  // It also marks the related route as disallowed so that the handle
+  // shows up as having to change.
   if (number == SIGNAL_A1 || number == SIGNAL_A2) {
     if (new_position == PLUS) {
+      if (current_position[ROUTE_A1] == MINUS) {
+        position_is_ok[ROUTE_A1] = false;
+      }
+      if (current_position[ROUTE_A2] == MINUS) {
+        position_is_ok[ROUTE_A2] = false;
+      }
       change_position(ROUTE_A_FIXATION, PLUS);
     }
   }
@@ -252,7 +285,6 @@
   }

 }
-

 void setup() {
@@ -285,8 +317,17 @@
   pinMode(PIN_LED_ROUTE_BERENBACH, OUTPUT);
   pinMode(PIN_LED_ROUTE_A_FIXATION, OUTPUT);

+  // Attach the output pins
+  pinMode(PIN_OUTPUT_SWITCH_1, OUTPUT);
+  pinMode(PIN_OUTPUT_SWITCH_5, OUTPUT);
+  pinMode(PIN_OUTPUT_SIGNAL_A1, OUTPUT);
+  pinMode(PIN_OUTPUT_SIGNAL_A2, OUTPUT);
+
   // Debug led 13 stuff
   digitalWrite(13, LOW);
+
+  // First make sure the output pins are OK
+  update_outputs();

   // Attach pins to lever Bounce objects.
   levers[SWITCH_5].attach(PIN_LEVER_SWITCH_5);


graph_init runs

graph_init runs

--- hb.ino
+++ hb.ino
@@ -19,7 +19,7 @@

 */

-//#define DUBUG_BUILD true
+#define DUBUG_BUILD true
 #define UNDEF_INT -32768
 #define NAME_SIZE  12
 #define PROCESS_SIZE  12
@@ -54,9 +54,15 @@
 Edge edges[10];

 // hbumtf parser vars.
-// state 0=outside_cmd, 1=loading, 2=nodes, 3=edges
+// state 0=outside_com, 1=loading, 2=nodes, 3=edges
 int state = 0;
-
+short run_state = 0;
+boolean graph_loaded = 0;
+
+
+// message queue
+short message_count = 0;
+String message_str;

 void setup() {
   // initialize serial:
@@ -69,7 +75,107 @@

 void loop() {
   hbumtfSerial();
-}
+  if (run_state) {
+    if (message_count > 0) {
+      process_message();
+    }
+    #ifdef DUBUG_BUILD
+    Serial.println(message_str);
+    delay(2000);
+    #endif
+  }
+}
+
+void process_message() {
+  String msg = deq_message();
+  int e_index;
+  int n0_index, n1_index;
+  if (msg[0] == '\0')  {
+    #ifdef DUBUG_BUILD
+    debug("no edge 1");
+    #endif
+    set_run_state(0);
+    return;
+  }
+  e_index = get_edge("msg", msg);
+  if (e_index < 0) {
+    e_index = get_edge("flo", msg);
+  }
+  if (e_index < 0) {
+    #ifdef DUBUG_BUILD
+    debug("no edge 2");
+    #endif
+    set_run_state(0);
+    return;
+  }
+  else {
+    #ifdef DUBUG_BUILD
+    debug("test of process edge (type) ");
+    debug(edges[e_index].type);
+    #endif
+    n0_index = get_node("id", edges[e_index].from);
+    n1_index = get_node("id", edges[e_index].to);
+    run_node(n1_index);
+  }
+}
+
+// kindof a cheat for now (only one place in the queue)
+void enq_message(String msg) {
+  if (message_count == 0) {
+    message_count++;
+    message_str = msg;
+  }
+}
+String deq_message() {
+  if (message_count > 0) {
+    message_count--;
+    return message_str;
+  }
+}
+
+// get edge
+int get_edge(String e_type, String msg) {
+  int i;
+  String t, n;
+  for (i = 0; i < 10; i++) {
+    t = edges[i].type;
+    n = edges[i].name;
+    if (t == e_type && n == msg) {
+      return i;
+    }
+  }
+  return -1;
+}
+
+// get node
+int get_node(String field, int id) {
+  return id;
+}
+
+// run_node(n1);
+void run_node(int n_index) {
+  #ifdef DUBUG_BUILD
+  debug("test of run_node");
+  debug(nodes[n_index].name);
+  #endif
+}
+
+void set_run_state(short new_state) {
+  short previous_state = run_state;
+  if (graph_loaded) {
+    run_state = new_state;
+  }
+  // next line... jsut for fun (remove later ;)
+  //digitalWrite(led, (int)run_state);
+  if (!previous_state && run_state) { // rising edge
+    enq_message("graph_init");
+
+    #ifdef DUBUG_BUILD
+    debug("sent msg graph_init");
+    #endif
+  }
+}
+

 /*
   hbumtfSerial
@@ -107,6 +213,12 @@
     if (inputString == "rfi") {
       com_rfi();
     }
+    if (inputString == "set run_state 1") {
+      set_run_state(1);
+    }
+    if (inputString == "set run_state 0") {
+      set_run_state(0);
+    }
   }
   if (state == 1 && inputString == NODES_HBG) {
     next_state = 2;
@@ -119,6 +231,7 @@
   if (state == 3 && inputString == END_HBG) {
     next_state = 0;
     state = -1;
+    graph_loaded = 1;
     com_graph();
   }
   // processing for states 2 and 3
@@ -246,6 +359,10 @@
   Serial.println(HB_VERSION);
   Serial.print("Serial state: ");
   Serial.println(state);
+  Serial.print("Graph loaded: ");
+  Serial.println(graph_loaded);
+  Serial.print("Run state: ");
+  Serial.println(run_state);
   com_graph();
 }


Updated the getTemp method

Updated the getTemp method

--- src/SerialPrint/SerialPrint.ino
+++ src/SerialPrint/SerialPrint.ino
@@ -29,10 +29,8 @@
 }

 String getTemp(DeviceAddress address) {
+  sensors.requestTemperatures();
   float tempC = sensors.getTempC(address);
-  if(tempC != -127.00) {
-    return String(tempC);
-  } else {
-    return "Error";
-  }
+  return (tempC != -127.00) ? String(tempC) : "Error";
 }
+


set USE_ARDUINO_INTERRUPTS to false

set USE_ARDUINO_INTERRUPTS to false

--- examples/TwoPulseSensors_On_OneArduino_Alternative/TwoPulseSensors_On_OneArduino_Alternative.ino
+++ examples/TwoPulseSensors_On_OneArduino_Alternative/TwoPulseSensors_On_OneArduino_Alternative.ino
@@ -35,7 +35,7 @@
    call pulse.sawNewSample() at least once every 2 milliseconds
    to accurately read the PulseSensor signal.
 */
-#define USE_ARDUINO_INTERRUPTS true
+#define USE_ARDUINO_INTERRUPTS false
 #include <PulseSensorPlayground.h>


Added a current read-out. Smoothed the current sampling in the ardurino. Added beagle CPU utilization.

--- arduino/OpenROV/OpenROV.ino
+++ arduino/OpenROV/OpenROV.ino
@@ -17,6 +17,16 @@
 // array[0] will be the number of arguments in the command
 int array[MAX_ARGS];

+// Define the number of samples to keep track of.  The higher the number,
+// the more the readings will be smoothed, but the slower the output will
+// respond to the input.  Using a constant rather than a normal variable lets
+// use this value to determine the size of the readings array.
+const int numReadings = 10;
+
+int readings[numReadings];      // the readings from the analog input
+int index = 0;                  // the index of the current reading
+int total = 0;                  // the running total
+int average = 0;                // the average

 void setup(){

@@ -29,6 +39,10 @@
   motors.reset();

   time.reset();
+
+// initialize all the readings to 0:
+  for (int thisReading = 0; thisReading < numReadings; thisReading++)
+    readings[thisReading] = 0;
 }

 void loop(){
@@ -62,10 +76,30 @@
     }
   }

+  if (time.elapsed (100)) {
+    // subtract the last reading:
+    total= total - readings[index];
+    // read from the sensor:
+    readings[index] = iout.read();
+    delay(1);
+    // add the reading to the total:
+    total= total + readings[index];
+    // advance to the next position in the array:
+    index = index + 1;
+
+    // if we're at the end of the array...
+    if (index >= numReadings)
+      // ...wrap around to the beginning:
+      index = 0;
+
+    // calculate the average:
+    average = total / numReadings;
+  }
+
   // send voltage and current
   if (time.elapsed(1000)) {
     vout.send(vout.read());
-    iout.send(iout.read());
+    iout.send(average);
     Serial.print("time:");
     Serial.println(millis());
   }


Tweak watchdog settings.

Tweak watchdog settings.

--- arduino/monitor/monitor.ino
+++ arduino/monitor/monitor.ino
@@ -44,7 +44,8 @@

 void setup()
 {
-  wdt_enable(WDTO_2S);
+  wdt_reset();
+  wdt_enable(WDTO_8S);

   pinMode(DIO_RAIN, INPUT_PULLUP);
   attachInterrupt(0, IRQ_rain, FALLING);


update

update

--- arduino/opencr_arduino/opencr/libraries/OpenManipulator/example/Arduino/Test/kinematics/kinematics.ino
+++ arduino/opencr_arduino/opencr/libraries/OpenManipulator/example/Arduino/Test/kinematics/kinematics.ino
@@ -26,12 +26,12 @@

   Manipulator manipulator;
   manipulator.addWorld(WORLD, COMP1);
-  manipulator.addComponent(COMP1, WORLD, COMP2, MATH::makeVector3(-0.100, 0.0, 0.0), IDENTITY_MATRIX, 1, Z_AXIS, 0.5, MATH::makeMatrix3(1, 1, 1, 1, 1, 1, 3, 1, 1));
-  manipulator.addComponent(COMP2, COMP1, COMP3, MATH::makeVector3(0.0, 0.0, 0.050), IDENTITY_MATRIX, 2, Y_AXIS, 1.5);
-  manipulator.addComponentChild(COMP2, COMP4);
-  manipulator.addComponent(COMP3, COMP2, TOOL, MATH::makeVector3(0.0, 0.050, 0.0), IDENTITY_MATRIX, 3, Y_AXIS, 2.0);
-  manipulator.addComponent(COMP4, COMP2, TOOL, MATH::makeVector3(0.0, 0.0, 0.050), IDENTITY_MATRIX, NONE);
-  manipulator.addTool(TOOL, COMP3, MATH::makeVector3(0.0, 0.0, 0.025), IDENTITY_MATRIX, 4, 0.1);
+  manipulator.addComponent(COMP1, WORLD, COMP2, MATH::makeVector3(-0.100, 0.0, 0.0), IDENTITY_MATRIX, Z_AXIS, 1, 0.5, 1.0);
+  manipulator.addComponent(COMP2, COMP1, COMP4, MATH::makeVector3(0.0, 0.0, 0.050), IDENTITY_MATRIX, Y_AXIS, 2, -1.5, 1.5);
+  manipulator.addComponentChild(COMP2, COMP3);
+  manipulator.addComponent(COMP3, COMP2, TOOL, MATH::makeVector3(0.0, 0.050, 0.0), IDENTITY_MATRIX);
+  manipulator.addComponent(COMP4, COMP2, TOOL, MATH::makeVector3(0.050, 0.0, 0.0), IDENTITY_MATRIX, Y_AXIS, 3, 2.0);
+  manipulator.addTool(TOOL, COMP4, MATH::makeVector3(0.025, 0.0, 0.0), IDENTITY_MATRIX, 4, 1.0, 0.5);

   Eigen::MatrixXf jacobian = KINEMATICS::CHAIN::jacobian(&manipulator, TOOL);
   LOG::INFO("Jacobian : ");
@@ -41,9 +41,9 @@
   manipulator.checkManipulatorSetting();

   Pose target;
-  target.position = ZERO_VECTOR;
+  target.position = MATH::makeVector3(-0.043, 0.0, 0.086);
   target.orientation = IDENTITY_MATRIX;
-  Eigen::VectorXf joint_angle = KINEMATICS::CHAIN::inverse(&manipulator, TOOL, target);
+  std::vector<float> joint_angle = KINEMATICS::CHAIN::inverse(&manipulator, TOOL, target);
   LOG::INFO("Result of inverse : ");
   PRINT::VECTOR(joint_angle);
 }


comments

comments

--- mutually-assured-destruction.ino
+++ mutually-assured-destruction.ino
@@ -8,7 +8,7 @@

 //////////////////////////////////////////////////
-// Defines
+// Pins
 //////////////////////////////////////////////////

 #define LED 13
@@ -26,6 +26,10 @@
 // #define NEOPIXEL_PIN_A 4
 // #define NEOPIXEL_PIN_B -1
 // #define NEOPIXEL_COUNT 120
+
+//////////////////////////////////////////////////
+// Logging
+//////////////////////////////////////////////////

 // Logging
 #define MAD_LOGGING 1
@@ -112,9 +116,9 @@

   accelerometer->loop();
   // accelerometer->log();
-  RemoteCommand last_command = remote->loop();
-
-  // Process remote command:
+  RemoteCommand last_command = remote->loop();  // get the latest remote control command
+
+  // Process remote commands for stop/start
   if ( last_command == RemoteCommandStop ) {

     stopped = 1;
@@ -128,22 +132,20 @@

   if (!stopped) {

-    if ( last_command == RemoteCommandLeft ) {
+    if ( last_command == RemoteCommandLeft ) {            // Left
 #if MAD_LOGGING
       Serial.println(F("Relay 0 on"));
 #endif
       r0->setOnWithPattern(current_burst_pattern());
-//      r1->off();
-
-    } else if ( last_command == RemoteCommandRight ) {
+
+    } else if ( last_command == RemoteCommandRight ) {    // Right

 #if MAD_LOGGING
       Serial.println(F("Relay 1 on"));
 #endif
-//      r0->off();
       r1->setOnWithPattern(current_burst_pattern());

-    } else if ( last_command == RemoteCommandBoth ) {
+    } else if ( last_command == RemoteCommandBoth ) {    // Middle (Both)

 #if MAD_LOGGING
       Serial.println(F("Both Relays on"));
@@ -151,12 +153,12 @@
       r0->setOnWithPattern(current_burst_pattern());
       r1->setOnWithPattern(current_burst_pattern());

-    } else if ( last_command == RemoteCommandDigit9 ) {
+    } else if ( last_command == RemoteCommandDigit9 ) {  // 9

       r0->setOnWithPattern(JAWS_LEFT);
       r1->setOnWithPattern(JAWS_RIGHT);

-    } else if ( last_command == RemoteCommandDigit8 ) {
+    } else if ( last_command == RemoteCommandDigit8 ) {  // 8... and so on...

       r0->setOnWithPattern(LRM_A);
       r1->setOnWithPattern(LRM_B);
@@ -192,10 +194,10 @@

       burst_mode = 1;

-    } else if ( last_command == RemoteCommandToggleAccelerometer ) {
+    } else if ( last_command == RemoteCommandToggleAccelerometer ) {    // deprecate?
       accel_enabled = !accel_enabled;

-    } else {
+    } else {
       // only do accelerometer based stuff if the remote isn't active (?)

       if ( accelerometer->position_changed) {


Added a function to change the name of the SSID.


add tool control

add tool control

--- arduino/opencr_arduino/opencr/libraries/OpenManipulator/example/Arduino/Chain/d_open_manipulator_chain/d_open_manipulator_chain.ino
+++ arduino/opencr_arduino/opencr/libraries/OpenManipulator/example/Arduino/Chain/d_open_manipulator_chain/d_open_manipulator_chain.ino
@@ -65,13 +65,19 @@
       }
      break;

-     case 2:
+    case 2:
       if (chain.moving() == false)
       {
         chain.setMove(CHAIN, TOOL, OM_MATH::makeVector3(0.0, 0.0, -0.050), 1.0f);

         seq = 3;
       }
+     break;
+
+    case 3:
+      chain.toolMove(CHAIN, TOOL, OM_MATH::map(0.060f, 0.020f, 0.070f, 0.907f, -1.13f));
+      seq = 4;
+     break;

     default:
      break;


fix sampling rate conversion of incoming data

--- ble_heart_rate_template.ino
+++ ble_heart_rate_template.ino
@@ -252,14 +252,11 @@
 		        		Serial.println(F("SAMPLING_RATE: "));
 		        		printData(aci_evt);

-						uint32_t value32;
-
 						// 4 Bytes
  						// big-endian !!!!
-						value32 = aci_evt->params.data_received.rx_data.aci_data[3] + ((uint32_t)aci_evt->params.data_received.rx_data.aci_data[2] << 8)
-						+ ((uint32_t)aci_evt->params.data_received.rx_data.aci_data[1] << 16) + ((uint32_t)aci_evt->params.data_received.rx_data.aci_data[0] << 24) ;
-						Serial.print(F("Value: "));
-		        		Serial.println(value32);
+						value = aci_evt->params.data_received.rx_data.aci_data[1] + ((uint16_t)aci_evt->params.data_received.rx_data.aci_data[0] << 8);
+		        		Serial.print(F("Value: "));
+		        		Serial.println(value);
 		        		break;
 		        	default:
 		        		printData(aci_evt);


Adjusted alarm frequency.

Adjusted alarm frequency.

--- Alicorn.ino
+++ Alicorn.ino
@@ -122,7 +122,7 @@
 // Buzzer
 #define BUZZER_REMOTE_TONE     3000
 #define BUZZER_REMOTE_DURATION 100
-#define BUZZER_ALARM_TONE      2000
+#define BUZZER_ALARM_TONE      4000

 // DHT22
 DHT dht(PIN_DHT, DHT22);


Add mavlink Precise unit if not si in variable names for clarity

--- Arduino/MPU9150_AHRS/MPU9150_AHRS.ino
+++ Arduino/MPU9150_AHRS/MPU9150_AHRS.ino
@@ -58,6 +58,11 @@

 Distributed as-is; no warranty is given.
 *****************************************************************/
+
+#define USE_MAVLINK //Uncomment this line to use mavlink
+#ifdef USE_MAVLINK
+  #include "/home/bat/sketchbook/libraries/mavlink/ardupilotmega/mavlink.h"        // Mavlink interface
+#endif

 #include <Wire.h>
 #include "I2Cdev.h"
@@ -99,9 +104,13 @@
 uint16_t lastUpdate = 0; // used to calculate integration interval
 uint16_t now = 0;        // used to calculate integration interval

-float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values
+float ax_g, ay_g, az_g, gx_degps, gy_degps, gz_degps, mx, my, mz; // variables to hold latest sensor data values
 float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};    // vector to hold quaternion
 float eInt[3] = {0.0f, 0.0f, 0.0f};       // vector to hold integral error for Mahony method
+
+uint8_t system_id = 100;
+uint8_t component_id = 200;
+long time_boot_ms;

 void setup()
 {
@@ -161,14 +170,14 @@
             mcount++;
            // read the raw sensor data
             mpu.getAcceleration  ( &a1, &a2, &a3  );
-            ax = a1*2.0f/32768.0f; // 2 g full range for accelerometer
-            ay = a2*2.0f/32768.0f;
-            az = a3*2.0f/32768.0f;
+            ax_g = a1*2.0f/32768.0f; // 2 g full range for accelerometer
+            ay_g = a2*2.0f/32768.0f;
+            az_g = a3*2.0f/32768.0f;

             mpu.getRotation  ( &g1, &g2, &g3  );
-            gx = g1*250.0f/32768.0f; // 250 deg/s full range for gyroscope
-            gy = g2*250.0f/32768.0f;
-            gz = g3*250.0f/32768.0f;
+            gx_degps = g1*250.0f/32768.0f; // 250 deg/s full range for gyroscope
+            gy_degps = g2*250.0f/32768.0f;
+            gz_degps = g3*250.0f/32768.0f;
 //  The gyros and accelerometers can in principle be calibrated in addition to any factory calibration but they are generally
 //  pretty accurate. You can check the accelerometer by making sure the reading is +1 g in the positive direction for each axis.
 //  The gyro should read zero for each axis when the sensor is at rest. Small or zero adjustment should be needed for these sensors.
@@ -187,6 +196,7 @@
          }

   now = micros();
+  time_boot_ms = now/1000;
   deltat = ((now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update
   lastUpdate = now;
   // Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of the magnetometer;
@@ -196,69 +206,89 @@
   // in the LSM9DS0 sensor. This rotation can be modified to allow any convenient orientation convention.
   // This is ok by aircraft orientation standards!
   // Pass gyro rate as rad/s
-   MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
-// MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
-
-    // Serial print and/or display at 0.5 s rate independent of data rates
+   MadgwickQuaternionUpdate(ax_g, ay_g, az_g, gx_degps*PI/180.0f, gy_degps*PI/180.0f, gz_degps*PI/180.0f,  my,  mx, mz);
+// MahonyQuaternionUpdate(ax_g, ay, az, gx_degps*PI/180.0f, gy_degps*PI/180.0f, gz_degps*PI/180.0f, my, mx, mz);
+
+    // Serial print and/or display at 0.05 s rate independent of data rates
     delt_t = millis() - count;
-    if (delt_t > 500) { // update LCD once per half-second independent of read rate
-
-    Serial.print("ax = "); Serial.print((int)1000*ax);
-    Serial.print(" ay = "); Serial.print((int)1000*ay);
-    Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg");
-    Serial.print("gx = "); Serial.print( gx, 2);
-    Serial.print(" gy = "); Serial.print( gy, 2);
-    Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s");
-    Serial.print("mx = "); Serial.print( (int)mx );
-    Serial.print(" my = "); Serial.print( (int)my );
-    Serial.print(" mz = "); Serial.print( (int)mz ); Serial.println(" mG");
+    if (delt_t > 50) { //update at 20Hz
+
+    // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
+    // In this coordinate system, the positive z-axis is down toward Earth.
+    // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
+    // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
+    // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
+    // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
+    // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
+    // applied in the correct order which for this configuration is yaw, pitch, and then roll.
+    // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
+      yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
+      pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+      roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
+      yaw = yaw + LOCAL_DECLINATION_DEG*PI/180.0f; // Declination is difference between magnetic and true north
+
+#ifdef USE_MAVLINK
+      // Initialize the required buffers
+      mavlink_message_t msg;
+      uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+      mavlink_msg_heartbeat_pack(system_id, component_id, &msg, MAV_TYPE_KITE, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
+      uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
+      Serial.write(buf, len);
+
+      //mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
+      // uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag)
+      mavlink_msg_raw_imu_pack(system_id, component_id, &msg, time_boot_ms, a1, a2, a3, g1, g2, g3, m1, m2, m3);
+      len = mavlink_msg_to_send_buffer(buf, &msg);
+      Serial.write(buf, len);
+
+      mavlink_msg_scaled_imu_pack(system_id, component_id, &msg, time_boot_ms, ax_g*1000, ay_g*1000, az_g*1000, gx_degps*PI/180.0f*1000, gy_degps*PI/180.0f*1000, gz_degps*PI/180.0f*1000, mx, my, mz);
+      len = mavlink_msg_to_send_buffer(buf, &msg);
+      Serial.write(buf, len);
+      //static inline uint16_t mavlink_msg_attitude_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
+      //  uint32_t time_boot_ms, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed)
+      mavlink_msg_attitude_pack(system_id, component_id, &msg, time_boot_ms, roll, pitch, yaw, gx_degps*PI/180.0f, gy_degps*PI/180.0f, gz_degps*PI/180.0f);
+      len = mavlink_msg_to_send_buffer(buf, &msg);
+      Serial.write(buf, len);
+
+#else
+      Serial.print("ax = "); Serial.print((int)1000*ax_g);
+      Serial.print(" ay = "); Serial.print((int)1000*ay_g);
+      Serial.print(" az = "); Serial.print((int)1000*az_g); Serial.println(" mg");
+      Serial.print("gx = "); Serial.print( gx_degps, 2);
+      Serial.print(" gy = "); Serial.print( gy_degps, 2);
+      Serial.print(" gz = "); Serial.print( gz_degps, 2); Serial.println(" deg/s");
+      Serial.print("mx = "); Serial.print( (int)mx );
+      Serial.print(" my = "); Serial.print( (int)my );
+      Serial.print(" mz = "); Serial.print( (int)mz ); Serial.println(" mG");
+
+      Serial.print("q0 = "); Serial.print(q[0]);
+      Serial.print(" qx = "); Serial.print(q[1]);
+      Serial.print(" qy = "); Serial.print(q[2]);
+      Serial.print(" qz = "); Serial.println(q[3]);
+      Serial.print("Yaw, Pitch, Roll: ");
+      Serial.print(yaw*180.0f / PI, 2);
+      Serial.print(", ");
+      Serial.print(pitch*180.0f / PI, 2);
+      Serial.print(", ");
+      Serial.println(roll*180.0f / PI, 2);
+
+      Serial.print("rate = "); Serial.print((float)1.0f/deltat, 2); Serial.println(" Hz");
+#endif
+

-    Serial.print("q0 = "); Serial.print(q[0]);
-    Serial.print(" qx = "); Serial.print(q[1]);
-    Serial.print(" qy = "); Serial.print(q[2]);
-    Serial.print(" qz = "); Serial.println(q[3]);
-
-
-  // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
-  // In this coordinate system, the positive z-axis is down toward Earth.
-  // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
-  // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
-  // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
-  // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
-  // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
-  // applied in the correct order which for this configuration is yaw, pitch, and then roll.
-  // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
-    yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
-    pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
-    roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
-    pitch *= 180.0f / PI;
-    yaw   *= 180.0f / PI + LOCAL_DECLINATION_DEG; // Declination is difference between magnetic and true north
-    roll  *= 180.0f / PI;
-
-    Serial.print("Yaw, Pitch, Roll: ");
-    Serial.print(yaw, 2);
-    Serial.print(", ");
-    Serial.print(pitch, 2);
-    Serial.print(", ");
-    Serial.println(roll, 2);
-
-    Serial.print("rate = "); Serial.print((float)1.0f/deltat, 2); Serial.println(" Hz");
-
-
-
-    // With these settings the filter is updating at a ~145 Hz rate using the Madgwick scheme and
-    // >200 Hz using the Mahony scheme even though the display refreshes at only 2 Hz.
-    // The filter update rate is determined mostly by the mathematical steps in the respective algorithms,
-    // the processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR:
-    // an ODR of 10 Hz for the magnetometer produce the above rates, maximum magnetometer ODR of 100 Hz produces
-    // filter update rates of 36 - 145 and ~38 Hz for the Madgwick and Mahony schemes, respectively.
-    // This is presumably because the magnetometer read takes longer than the gyro or accelerometer reads.
-    // This filter update rate should be fast enough to maintain accurate platform orientation for
-    // stabilization control of a fast-moving robot or quadcopter. Compare to the update rate of 200 Hz
-    // produced by the on-board Digital Motion Processor of Invensense's MPU6050 6 DoF and MPU9150 9DoF sensors.
-    // The 3.3 V 8 MHz Pro Mini is doing pretty well!
-
-    count = millis();
+      // With these settings the filter is updating at a ~145 Hz rate using the Madgwick scheme and
+      // >200 Hz using the Mahony scheme even though the display refreshes at only 2 Hz.
+      // The filter update rate is determined mostly by the mathematical steps in the respective algorithms,
+      // the processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR:
+      // an ODR of 10 Hz for the magnetometer produce the above rates, maximum magnetometer ODR of 100 Hz produces
+      // filter update rates of 36 - 145 and ~38 Hz for the Madgwick and Mahony schemes, respectively.
+      // This is presumably because the magnetometer read takes longer than the gyro or accelerometer reads.
+      // This filter update rate should be fast enough to maintain accurate platform orientation for
+      // stabilization control of a fast-moving robot or quadcopter. Compare to the update rate of 200 Hz
+      // produced by the on-board Digital Motion Processor of Invensense's MPU6050 6 DoF and MPU9150 9DoF sensors.
+      // The 3.3 V 8 MHz Pro Mini is doing pretty well!
+
+      count = millis();
     }
 }


Add protocol for motor control

Add protocol for motor control

--- Arduino/drive_motors/drive_motors.ino
+++ Arduino/drive_motors/drive_motors.ino
@@ -1,15 +1,16 @@
 #include <Servo.h>

-Servo leftMotor;        // create servo for the left motor
-int leftMotorPin = 11;  // Left Motor on pin 11
-Servo rightMotor;       // create servo for the right motor
-int rightMotorPin = 12; // Right Motor on pin 12
-int inByte = 0;         // incoming serial byte
-int onMessage = 49;     // expected "on" message.  This is a "1" character.
-int offMessage = 48;    // expected "off" message.  This is a "0" character.
+Servo leftMotor;          // create servo for the left motor
+int leftMotorPin = 11;    // Left Motor on pin 11
+Servo rightMotor;         // create servo for the right motor
+int rightMotorPin = 12;   // Right Motor on pin 12
+int inByte = 0;           // incoming serial byte
+int onMessage = '1';      // expected "on" message.
+int offMessage = '0';     // expected "off" message.
+int leftMotorValue = 90;  // start off at 0%
+int rightMotorValue = 90; // start off at 0%

-void setup()
-{
+void setup() {
   leftMotor.attach(leftMotorPin);
   rightMotor.attach(rightMotorPin);

@@ -23,24 +24,48 @@
                       // been sent across it.
 }

-void loop()
-{
+void loop() {
   // if we get a valid byte, read analog ins:
   if (Serial.available() > 0) {
     // get incoming byte:
     inByte = Serial.read();
-    if(inByte == onMessage){
-      turnOn();
-    }
-    if(inByte == offMessage){
-      turnOff();
-    }
+    handleCommand(inByte);
   }
+//  drive();
+}
+
+void drive() {
+  debug();
+}
+
+void debug() {
+  Serial.print(leftMotorValue);
+  Serial.println(rightMotorValue);
+}
+
+void handleCommand(char inByte) {
+  debug();
+  if(inByte <= 'u' && inByte >= 'a'){
+    Serial.println("aaa");
+    updateLeftMotor(inByte);
+  }
+  if(inByte <= 'U' && inByte >= 'A'){
+    Serial.println("bbb");
+    updateRightMotor(inByte);
+  }
+}
+
+void updateLeftMotor(char value){
+  leftMotorValue = leftMotorValueMapped(value);
+}
+
+void updateRightMotor(char value){
+  rightMotorValue = rightMotorValueMapped(value);
 }

 void establishContact() {
   while (Serial.available() <= 0) {
-    Serial.print('A');   // send a capital A
+    Serial.print(rightMotorValueMapped('B')); // send a capital A
     delay(300);
   }
 }
@@ -54,3 +79,98 @@
   leftMotor.write(90);
   rightMotor.write(90);
 }
+
+// Protocol for motor speeds:
+int leftMotorValueMapped(char encoded){
+  switch(encoded){
+    case 'a':
+      return 0;   // -100%
+    case 'b':
+      return 9;   // -90%
+    case 'c':
+      return 18;  // -80%
+    case 'd':
+      return 27;  // -70%
+    case 'e':
+      return 36;  // -60%
+    case 'f':
+      return 45;  // -50%
+    case 'g':
+      return 54;  // -40%
+    case 'h':
+      return 63;  // -30%
+    case 'i':
+      return 72;  // -20%
+    case 'j':
+      return 81;  // -10%
+    case 'k':
+      return 90;  // 0%
+    case 'l':
+      return 99;  // 10%
+    case 'm':
+      return 108; // 20%
+    case 'n':
+      return 117; // 30%
+    case 'o':
+      return 126; // 40%
+    case 'p':
+      return 135; // 50%
+    case 'q':
+      return 144; // 60%
+    case 'r':
+      return 153; // 70%
+    case 's':
+      return 162; // 80%
+    case 't':
+      return 171; // 90%
+    case 'u':
+      return 180; // 100%
+  }
+}
+
+int rightMotorValueMapped(char encoded){
+  switch(encoded){
+    case 'A':
+      return 0;   // -100%
+    case 'B':
+      return 9;   // -90%
+    case 'C':
+      return 18;  // -80%
+    case 'D':
+      return 27;  // -70%
+    case 'E':
+      return 36;  // -60%
+    case 'F':
+      return 45;  // -50%
+    case 'G':
+      return 54;  // -40%
+    case 'H':
+      return 63;  // -30%
+    case 'I':
+      return 72;  // -20%
+    case 'J':
+      return 81;  // -10%
+    case 'K':
+      return 90;  // 0%
+    case 'L':
+      return 99;  // 10%
+    case 'M':
+      return 108; // 20%
+    case 'N':
+      return 117; // 30%
+    case 'O':
+      return 126; // 40%
+    case 'P':
+      return 135; // 50%
+    case 'Q':
+      return 144; // 60%
+    case 'R':
+      return 153; // 70%
+    case 'S':
+      return 162; // 80%
+    case 'T':
+      return 171; // 90%
+    case 'U':
+      return 180; // 100%
+  }
+}


Changed Telnet Port to port 23

Changed Telnet Port to port 23

Best to stick to standard port numbers
https://en.wikipedia.org/wiki/List_of_TCP_and_UDP_port_numbers

--- hardware/esp8266com/esp8266/libraries/ESP8266WiFi/examples/WiFiTelnetToSerial/WiFiTelnetToSerial.ino
+++ hardware/esp8266com/esp8266/libraries/ESP8266WiFi/examples/WiFiTelnetToSerial/WiFiTelnetToSerial.ino
@@ -25,7 +25,7 @@
 const char* ssid = "**********";
 const char* password = "**********";

-WiFiServer server(21);
+WiFiServer server(23);
 WiFiClient serverClients[MAX_SRV_CLIENTS];

 void setup() {
@@ -45,7 +45,7 @@

   Serial1.print("Ready! Use 'telnet ");
   Serial1.print(WiFi.localIP());
-  Serial1.println(" 21' to connect");
+  Serial1.println(" 23' to connect");
 }

 void loop() {