Inclusão de calibração do PID.

Inclusão de calibração do PID.

--- quad/quad.ino
+++ quad/quad.ino
@@ -1,14 +1,24 @@
 #include <PID_v1.h>
 #include <Servo.h>
+#include <EEPROM.h>
 #include "I2Cdev.h"
 #include "MPU6050_6Axis_MotionApps20.h"
 #include "Wire.h"

+void setupRC();
+void setupMotors();
+void setupPID();
+void setupMPU();
 void readRC();
 void normalizeRC();
 void readOrientation();
 void writeMotor();
 void telemetry();
+void getZeroValuesFromRc();
+void getPIDParametersAndSetTunnings();
+void setPIDParameters(char param);
+void writePIDParameters();
+void PIDCalibration();
 String padding(String text, int size);

 /*********
@@ -58,6 +68,9 @@
 const int MAX_RC_DIMMER = 1018;
 const int MIN_RC_DIMMER = 2006;

+/* Valores médios para roll, pitch e yaw */
+unsigned long rcRollZero, rcPitchZero, rcYawZero = 0;
+
 /* Armazena a duração dos canais do controle (em us). */
 unsigned long channels[6];

@@ -88,7 +101,6 @@
 const int DMP_PITCH = 1;
 const int DMP_ROLL = 2;

-
 /****************
  * Interrupções *
  ****************/
@@ -111,14 +123,14 @@
 };

 /* Estrutura com os parâmetros dos PIDs */
-struct {
-  kpkikd pid_pitch_rate;
-  kpkikd pid_roll_rate;
-  kpkikd pid_yaw_rate;
-  kpkikd pid_pitch_stab;
-  kpkikd pid_roll_stab;
-  kpkikd pid_yaw_stab;
-} pid_params;
+struct pidParams {
+  kpkikd PIDPitchRate;
+  kpkikd PIDRollRate;
+  kpkikd PIDYawRate;
+  kpkikd PIDPitchStab;
+  kpkikd PIDRollStab;
+  kpkikd PIDYawStab;
+} pidParams;

 /* Variáveis de saída do PID para a taxa de rotação */
 double pitch_rate_output;
@@ -132,15 +144,30 @@

 /* PIDs para a taxa de rotação. Entrada: taxa de rotação do giroscópio. Objetivo: Taxa de giro proporcional ao erro. Saída: Potência para o motor. */
 /*                         ENTRADA                         SAÍDA                  OBJETIVO        P   D  I  DIREÇÃO */
-//PID pid_pitch_rate (/* taxa de giro */, &pitch_rate_output, &pitch_stab_output, 1, 2, 3, DIRECT);
-//PID pid_roll_rate  (/* taxa de giro */, &roll_rate_output, &roll_stab_output, 1, 2, 3, DIRECT);
-//PID pid_yaw_rate   (/* taxa de giro */, &yaw_rate_output, &yaw_stab_output, 1, 2, 3, DIRECT);
+//PID PIDPitchRate (/* taxa de giro */, &pitch_rate_output, &pitch_stab_output, 1, 2, 3, DIRECT);
+//PID PIDRollRate  (/* taxa de giro */, &roll_rate_output, &roll_stab_output, 1, 2, 3, DIRECT);
+//PID PIDYawRate   (/* taxa de giro */, &yaw_rate_output, &yaw_stab_output, 1, 2, 3, DIRECT);

 /* PIDs para a taxa de estabilização. Entrada: posição do quadricóptero através do MPU. Objetivo: Posição indicada através do controle. Saída: Taxa de giro proporcional ao erro. */
 /*                        ENTRADA                 SAÍDA          OBJETIVO  P  I  D  DIREÇÃO */
-PID pid_pitch_stab (&ypr_degree[DMP_PITCH], &pitch_stab_output, &rcPitch, 10, 4, 0, DIRECT);
-PID pid_roll_stab  (&ypr_degree[DMP_ROLL],  &roll_stab_output,  &rcRoll,  10, 4, 0, DIRECT);
-PID pid_yaw_stab   (&ypr_degree[DMP_YAW],   &yaw_stab_output,   &rcYaw,   10, 4, 0, DIRECT);
+PID PIDPitchStab (&ypr_degree[DMP_PITCH], &pitch_stab_output, &rcPitch, 5, 1, 0, DIRECT);
+PID PIDRollStab  (&ypr_degree[DMP_ROLL],  &roll_stab_output,  &rcRoll,  5, 1, 0, DIRECT);
+PID PIDYawStab   (&ypr_degree[DMP_YAW],   &yaw_stab_output,   &rcYaw,   5, 1, 0, DIRECT);
+
+/* Configuração dos endereços da EEPROM para armazenar os PIDs */
+const int EEPROM_PITCH_P = 0;
+const int EEPROM_PITCH_I = 1;
+const int EEPROM_PITCH_D = 2;
+const int EEPROM_ROLL_P = 3;
+const int EEPROM_ROLL_I = 4;
+const int EEPROM_ROLL_D = 5;
+const int EEPROM_YAW_P = 6;
+const int EEPROM_YAW_I = 7;
+const int EEPROM_YAW_D = 8;
+
+/* Variável que indica se houve calibração. Se houve e finalizou, grava a EEPROM. */
+bool pidChanged = false;
+
 /* Fim do PID */

 /*************************
@@ -153,7 +180,8 @@
 const int MOTOR_PIN_BL = 11; /* To-do test this pin */
 const int MOTOR_PIN_BR = 12; /* To-do test this pin */

-/* Entidades que representam os motores. Responsáveis por acionar os ESCs realizando um PPM entre 1ms e 2ms. */
+/* Entidades que representam os motores.
+ * Responsáveis por acionar os ESCs realizando um PPM entre 1ms e 2ms. */
 Servo motorFL;
 Servo motorFR;
 Servo motorBL;
@@ -172,6 +200,128 @@
   D(Serial.begin(115200));
   D(while (Serial.available() && Serial.read())); // Limpa o buffer

+  D(Serial.println("Iniciando controle remoto"));
+  setupRC();
+
+  D(Serial.println("Iniciando motores"));
+  setupMotors();
+
+  D(Serial.println("Iniciando PID"));
+  setupPID();
+
+  D(Serial.println("Iniciando MPU"));
+  setupMPU();
+
+  /* Configura a led para output de atividade */
+  pinMode(LED_PIN, OUTPUT);
+
+  D(Serial.println("Fim do setup"));
+}
+
+/* Loop infinito do Arduino. */
+void loop() {
+  static float yaw_target = 0;
+
+  /* Se a programação do DMP falhou, não faz nada */
+  if (!dmpReady) {
+    return;
+  }
+
+  readOrientation();
+
+  readRC();
+
+  PIDCalibration();
+
+  /* Voe Forest, Voe! */
+  if(rcThrottle > MIN_RC_THROTTLE + 100) {  // Throttle raised, turn on stablisation.
+    PIDPitchStab.Compute();
+    PIDRollStab.Compute();
+    PIDYawStab.Compute();
+
+    // is pilot asking for yaw change - if so feed directly to rate pid (overwriting yaw stab output)
+    if(abs(rcYaw ) > 5) {
+      yaw_stab_output = rcYaw;
+      yaw_target = ypr_degree[DMP_YAW];   // remember this yaw for when pilot stops
+    }
+
+    motors[MOTOR_FL] = rcThrottle + roll_stab_output + pitch_stab_output - yaw_stab_output;
+    motors[MOTOR_BL] = rcThrottle + roll_stab_output - pitch_stab_output + yaw_stab_output;
+    motors[MOTOR_FR] = rcThrottle - roll_stab_output + pitch_stab_output + yaw_stab_output;
+    motors[MOTOR_BR] = rcThrottle - roll_stab_output - pitch_stab_output - yaw_stab_output;
+
+  } else {
+    /* Motores desligados */
+    motors[MOTOR_FL] = motors[MOTOR_FR] = motors[MOTOR_BL] = motors[MOTOR_BR] = 1000;
+
+    // reset yaw target so we maintain this on takeoff
+    yaw_target = ypr_degree[DMP_YAW];
+  }
+
+  writeMotor();
+
+  D(telemetry());
+
+  /* Pisca a led para indicar atividade */
+  blinkState = !blinkState;
+  digitalWrite(LED_PIN, blinkState);
+}
+
+/* Efetua a leitura dos canais dos controles. */
+void readRC() {
+  channels[0] = pulseIn(CH1, HIGH, 20000); /* Roll (horizontal da direita) */
+  channels[1] = pulseIn(CH2, HIGH, 20000); /* Pitch (vertical da direita) */
+  channels[2] = pulseIn(CH3, HIGH, 20000); /* Throttle (vertical da esquerda) */
+  channels[3] = pulseIn(CH4, HIGH, 20000); /* Yaw (horizontal da esquerda */
+  channels[4] = pulseIn(CH5, HIGH, 20000); /* ON/OFF */
+  channels[5] = pulseIn(CH6, HIGH, 20000); /* Dimmer */
+
+  normalizeRC();
+}
+
+/* Obtem os valores dos PIDs da EEPROM */
+void getPIDParametersAndSetTunnings() {
+  D(char tempString[10]);
+
+  pidParams.PIDPitchStab.kp = (float)EEPROM.read(EEPROM_PITCH_P) / 10;
+  pidParams.PIDPitchStab.ki = (float)EEPROM.read(EEPROM_PITCH_I) / 10;
+  pidParams.PIDPitchStab.kd = (float)EEPROM.read(EEPROM_PITCH_D) / 10;
+  PIDPitchStab.SetTunings(pidParams.PIDPitchStab.kp, pidParams.PIDPitchStab.ki, pidParams.PIDPitchStab.kd);
+
+  pidParams.PIDRollStab.kp = (float)EEPROM.read(EEPROM_ROLL_P) / 10;
+  pidParams.PIDRollStab.ki = (float)EEPROM.read(EEPROM_ROLL_I) / 10;
+  pidParams.PIDRollStab.kd = (float)EEPROM.read(EEPROM_ROLL_D) / 10;
+  PIDRollStab.SetTunings(pidParams.PIDRollStab.kp, pidParams.PIDRollStab.ki, pidParams.PIDRollStab.kd);
+
+
+  pidParams.PIDYawStab.kp = (float)EEPROM.read(EEPROM_YAW_P) / 10;
+  pidParams.PIDYawStab.ki = (float)EEPROM.read(EEPROM_YAW_I) / 10;
+  pidParams.PIDYawStab.kd = (float)EEPROM.read(EEPROM_YAW_D) / 10;
+  PIDYawStab.SetTunings(pidParams.PIDYawStab.kp, pidParams.PIDYawStab.ki, pidParams.PIDYawStab.kd);
+
+  D(Serial.print("Valores do PID: ("));
+  D(dtostrf(pidParams.PIDPitchStab.kp, 2, 1, tempString));
+  D(Serial.print(String(tempString) + ", "));
+  D(dtostrf(pidParams.PIDPitchStab.ki, 2, 1, tempString));
+  D(Serial.print(String(tempString) + ", "));
+  D(dtostrf(pidParams.PIDPitchStab.kd, 2, 1, tempString));
+  D(Serial.print(String(tempString) + "), ("));
+  D(dtostrf(pidParams.PIDRollStab.kp, 2, 1, tempString));
+  D(Serial.print(String(tempString) + ", "));
+  D(dtostrf(pidParams.PIDRollStab.ki, 2, 1, tempString));
+  D(Serial.print(String(tempString) + ", "));
+  D(dtostrf(pidParams.PIDRollStab.kd, 2, 1, tempString));
+  D(Serial.print(String(tempString) + "), ("));
+  D(dtostrf(pidParams.PIDYawStab.kp, 2, 1, tempString));
+  D(Serial.print(String(tempString) + ", "));
+  D(dtostrf(pidParams.PIDYawStab.ki, 2, 1, tempString));
+  D(Serial.print(String(tempString) + ", "));
+  D(dtostrf(pidParams.PIDYawStab.kd, 2, 1, tempString));
+  D(Serial.println(String(tempString) + ")"));
+}
+
+/* Efetua o setup do controle remoto */
+void setupRC() {
   /* Bind dos canais de entrada do controle */
   pinMode(CH1, INPUT);
   pinMode(CH2, INPUT);
@@ -180,29 +330,49 @@
   pinMode(CH4, INPUT);
   pinMode(CH6, INPUT);

+  /* Zera os valores do Pitch, Roll e Yaw */
+  getZeroValuesFromRc();
+}
+
+/* Efetua o setup dos motores */
+void setupMotors() {
   /* Bind dos canais de saída dos motores (ESCs) */
   motorFL.attach(MOTOR_PIN_FL);
   motorFR.attach(MOTOR_PIN_FR);
   motorBL.attach(MOTOR_PIN_BL);
   motorBR.attach(MOTOR_PIN_BR);

+  /* Desliga os motores */
+  motorFL.writeMicroseconds(1000);
+  motorFR.writeMicroseconds(1000);
+  motorBL.writeMicroseconds(1000);
+  motorBR.writeMicroseconds(1000);
+}
+
+/* Efetua o setup do PID */
+void setupPID() {
   /* Inicializa o PID */
-  pid_pitch_stab.SetMode(AUTOMATIC);
-  pid_roll_stab.SetMode(AUTOMATIC);
-  pid_yaw_stab.SetMode(AUTOMATIC);
+  getPIDParametersAndSetTunnings();
+
+  PIDPitchStab.SetMode(AUTOMATIC);
+  PIDRollStab.SetMode(AUTOMATIC);
+  PIDYawStab.SetMode(AUTOMATIC);

   /* Limita o quanto será enviado ao motor: range máximo de 1000ms. */
-  pid_pitch_stab.SetOutputLimits(-500, 500);
-  pid_roll_stab.SetOutputLimits(-500, 500);
-  pid_yaw_stab.SetOutputLimits(-500, 500);
-
+  PIDPitchStab.SetOutputLimits(-500, 500);
+  PIDRollStab.SetOutputLimits(-500, 500);
+  PIDYawStab.SetOutputLimits(-500, 500);
+}
+
+/* Efetua o setup do MPU6050 */
+void setupMPU() {
   /* Inicializa o barramento I2C (a biblioteca I2Cdev não faz isso automaticamente) */
   Wire.begin();
   TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)

   /* Inicializa a MPU e seu respectivo DMP */
   mpu.initialize();
-  D(Serial.println(F("Inicializando DMP...")));
+  D(Serial.println("A"));
   dmpStatus = mpu.dmpInitialize();

   /* Define os offsets do MPU. Utilizado o programa MPU6050_calibration para chegar a esses números. */
@@ -215,21 +385,21 @@

   /* Só prossegue somente se o houve a inicialização correta do DMP */
   if (dmpStatus == 0) {
+    D(Serial.println("B"));
     /* Liga o DMP. Agora o MPU está pronto! */
     mpu.setDMPEnabled(true);

     /* Habilita a detecção de interrupção do Arduino */
-    D(Serial.println(F("Habilitando interrupcao 0 (pino 2)...")));
     attachInterrupt(0, dmpDataReady, RISING);
     mpuIntStatus = mpu.getIntStatus();

+D(Serial.println("C"));
     /* Inicializa a flag de controle do DMP para o loop() saber que está tudo certo */
-    D(Serial.println(F("DMP pronto! Iniciando programa...")));
     dmpReady = true;

     /* Obtem o tamanho do pacote do DMP */
     packetSize = mpu.dmpGetFIFOPacketSize();
-    D(Serial.println(F("Tamanho do pacote do DMP obtido...")));
+    D(Serial.println("D"));
   }
   else {
     // ERRO!
@@ -239,115 +409,102 @@
     D(Serial.print(dmpStatus));
     D(Serial.println(F(")")));
   }
-
-  // configure LED for output
-  pinMode(LED_PIN, OUTPUT);
-}
-
-/* Loop infinito do Arduino. */
-void loop() {
-  static float yaw_target = 0;
-
-  /* Se a programação do DMP falhou, não faz nada */
-  // D(Serial.println("DMP rdy"));
-  if (!dmpReady) {
-    return;
-  }
-
-  D(Serial.println(" "));
-  D(Serial.println("Red Ori"));
-  readOrientation();
-
-  // D(Serial.println("Read RC"));
-  readRC();
-
-  /* Voe Forest, Voe! */
-  if(rcThrottle > MIN_RC_THROTTLE + 100) {  // Throttle raised, turn on stablisation.
-    pid_pitch_stab.Compute();
-    pid_roll_stab.Compute();
-    pid_yaw_stab.Compute();
-
-    // is pilot asking for yaw change - if so feed directly to rate pid (overwriting yaw stab output)
-    if(abs(rcYaw ) > 5) {
-      yaw_stab_output = rcYaw;
-      yaw_target = ypr_degree[DMP_YAW];   // remember this yaw for when pilot stops
-    }
-
-    // pid_pitch_rate.calculate()
-    // pid_roll_rate.calculate()
-    // pid_yaw_rate.calculate()
-
-    //  // Stablise PIDS                                                     'setpoint' 'input'
-    // float pitch_stab_output = constrain(pids[PID_PITCH_STAB].get_pid((float)rcpit - pitch, 1), -250, 250);
-    // float roll_stab_output = constrain(pids[PID_ROLL_STAB].get_pid((float)rcroll - roll, 1), -250, 250);
-    // float yaw_stab_output = constrain(pids[PID_YAW_STAB].get_pid(wrap_180(yaw_target - yaw), 1), -360, 360);
-
-    // // is pilot asking for yaw change - if so feed directly to rate pid (overwriting yaw stab output)
-    // if(abs(rcyaw ) > 5) {
-    //   yaw_stab_output = rcyaw;
-    //   yaw_target = yaw;   // remember this yaw for when pilot stops
-    // }
-
-    // // rate PIDS                                                       'setpoint'         'input'
-    // long pitch_output =  (long) constrain(pids[PID_PITCH_RATE].get_pid(pitch_stab_output - gyroPitch, 1), - 500, 500);
-    // long roll_output =  (long) constrain(pids[PID_ROLL_RATE].get_pid(roll_stab_output - gyroRoll, 1), -500, 500);
-    // long yaw_output =  (long) constrain(pids[PID_YAW_RATE].get_pid(yaw_stab_output - gyroYaw, 1), -500, 500);
-
-    // // mix pid outputs and send to the motors.
-    // hal.rcout->write(MOTOR_FL, rcthr + roll_output + pitch_output - yaw_output);
-    // hal.rcout->write(MOTOR_BL, rcthr + roll_output - pitch_output + yaw_output);
-    // hal.rcout->write(MOTOR_FR, rcthr - roll_output + pitch_output + yaw_output);
-    // hal.rcout->write(MOTOR_BR, rcthr - roll_output - pitch_output - yaw_output);
-
-    motors[MOTOR_FL] = rcThrottle + roll_stab_output + pitch_stab_output - yaw_stab_output;
-    motors[MOTOR_BL] = rcThrottle + roll_stab_output - pitch_stab_output + yaw_stab_output;
-    motors[MOTOR_FR] = rcThrottle - roll_stab_output + pitch_stab_output + yaw_stab_output;
-    motors[MOTOR_BR] = rcThrottle - roll_stab_output - pitch_stab_output - yaw_stab_output;
-
+}
+
+/* Define os parâmetros dos PIDs com base no dimmer do controle. O range possível é de 0 a 25.5.
+ * Atualmente, os valores do PID são compartilhados entre o roll, pitch e yaw.
+ * O parâmetro sendo calibrado é definido de forma hard-coded. */
+void setPIDParameters(char param) {
+  if (param == 'P') {
+    pidParams.PIDPitchStab.kp = pidParams.PIDRollStab.kp = pidParams.PIDYawStab.kp = rcDimmer/10;
+  } else if (param == 'I') {
+    pidParams.PIDPitchStab.ki = pidParams.PIDRollStab.ki = pidParams.PIDYawStab.ki = rcDimmer/10;
   } else {
-    /* Motores desligados */
-    motors[MOTOR_FL] = motors[MOTOR_FR] = motors[MOTOR_BL] = motors[MOTOR_BR] = 1000;
-
-    // reset yaw target so we maintain this on takeoff
-    yaw_target = ypr_degree[DMP_YAW];
-
-    // reset PID integrals whilst on the ground
-    // for(int i=0; i<6; i++)
-    //   pids[i].reset_I();
-
-    // }
-  }
-
-  // D(Serial.println("Wrt mtr"));
-  writeMotor();
-
-  // D(telemetry());
-
-  /* Pisca a led para indicar atividade */
-  blinkState = !blinkState;
-  digitalWrite(LED_PIN, blinkState);
-}
-
-/* Efetua a leitura dos canais dos controles. */
-void readRC() {
-  channels[0] = pulseIn(CH1, HIGH); /* Roll (horizontal da direita) */
-  channels[1] = pulseIn(CH2, HIGH); /* Pitch (vertical da direita) */
-  channels[2] = pulseIn(CH3, HIGH); /* Throttle (vertical da esquerda) */
-  channels[3] = pulseIn(CH4, HIGH); /* Yaw (horizontal da esquerda */
-  channels[4] = pulseIn(CH5, HIGH); /* ON/OFF */
-  channels[5] = pulseIn(CH6, HIGH); /* Dimmer */
-
-  normalizeRC();
+    pidParams.PIDPitchStab.kd = pidParams.PIDRollStab.kd = pidParams.PIDYawStab.kd = rcDimmer/10;
+  }
+
+  PIDPitchStab.SetTunings(pidParams.PIDPitchStab.kp, pidParams.PIDPitchStab.ki, pidParams.PIDPitchStab.kd);
+  PIDRollStab.SetTunings(pidParams.PIDRollStab.kp, pidParams.PIDRollStab.ki, pidParams.PIDRollStab.kd);
+  PIDYawStab.SetTunings(pidParams.PIDYawStab.kp, pidParams.PIDYawStab.ki, pidParams.PIDYawStab.kd);
+
+  pidChanged = true;
+}
+
+/* Escreve os parâmetros do PID na EEPROM */
+void writePIDParameters() {
+  EEPROM.write(EEPROM_PITCH_P, (int)(pidParams.PIDPitchStab.kp * 10));
+  EEPROM.write(EEPROM_PITCH_I, (int)(pidParams.PIDPitchStab.ki * 10));
+  EEPROM.write(EEPROM_PITCH_D, (int)(pidParams.PIDPitchStab.kd * 10));
+
+  EEPROM.write(EEPROM_ROLL_P, (int)(pidParams.PIDRollStab.kp * 10));
+  EEPROM.write(EEPROM_ROLL_I, (int)(pidParams.PIDRollStab.ki * 10));
+  EEPROM.write(EEPROM_ROLL_D, (int)(pidParams.PIDRollStab.kd * 10));
+
+  EEPROM.write(EEPROM_YAW_P, (int)(pidParams.PIDYawStab.kp * 10));
+  EEPROM.write(EEPROM_YAW_I, (int)(pidParams.PIDYawStab.ki * 10));
+  EEPROM.write(EEPROM_YAW_D, (int)(pidParams.PIDYawStab.kd * 10));
+}
+
+/* Efetua a calibração se a calibração estiver ativa (controle ON/OFF ligado) e grava a EEPROM ao final da calibração. Escolha do parâmetro de forma hard-coded. */
+void PIDCalibration() {
+  if (rcOnOff > 10) {
+    setPIDParameters('P');
+  } else if (pidChanged) {
+    D(Serial.println("PIDs gravados na EEPROM"));
+    writePIDParameters();
+    pidChanged = false;
+  }
 }

 /* Normaliza os valores do controle para ranges pré definidos. */
 void normalizeRC() {
-  rcRoll     = map(channels[0], MIN_RC_ROLL, MAX_RC_ROLL, -45, 45);
-  rcPitch    = map(channels[1], MIN_RC_PITCH, MAX_RC_PITCH, 45, -45);
+
+  /* Leva em consideração o zero do roll controle */
+  if (rcRoll > rcRollZero) {
+    rcRoll = map(channels[0], rcRollZero, MAX_RC_ROLL, 0, 45);
+  } else {
+    rcRoll = map(channels[0], MIN_RC_ROLL, rcRollZero, -45, 0);
+  }
+
+  /* Leva em consideração o zero do roll controle */
+  if (rcPitch > rcRollZero) {
+    rcPitch = map(channels[1], rcPitchZero, MAX_RC_PITCH, 0, 45);
+  } else {
+    rcPitch = map(channels[1], MIN_RC_PITCH, rcPitchZero, -45, 0);
+  }
+
+  /* Leva em consideração o zero do yaw controle */
+  if (rcYaw > rcYawZero) {
+    rcYaw = map(channels[3], rcYawZero, MAX_RC_YAW, 0, 150);
+  } else {
+    rcYaw = map(channels[3], MIN_RC_YAW, rcYawZero, -150, 0);
+  }
+
   rcThrottle = channels[2];
-  rcYaw      = map(channels[3], MIN_RC_YAW, MAX_RC_YAW, -150, 150);
   rcOnOff    = map(channels[4], MIN_RC_ON_OFF, MAX_RC_ON_OFF, 0, 100);
-  rcDimmer   = map(channels[5], MIN_RC_DIMMER, MAX_RC_DIMMER, 0, 1000);
+  rcDimmer   = map(channels[5], MIN_RC_DIMMER, MAX_RC_DIMMER, 0, 255);
+}
+
+/* Define os valores referentes ao zero do controle para Roll, Yaw e Pitch com base em 10 leituras do controle. */
+void getZeroValuesFromRc() {
+
+  /* Roll (horizontal da direita) */
+  for (int i = 0; i < 10; i++) {
+    rcRollZero += pulseIn(CH1, HIGH, 20000);
+  }
+  rcRollZero /= 10;
+
+  /* Pitch (vertical da direita) */
+  for (int i = 0; i < 10; i++) {
+    rcPitchZero += pulseIn(CH2, HIGH, 20000);
+  }
+  rcPitchZero /= 10;
+
+  /* Yaw (horizontal da esquerda */
+  for (int i = 0; i < 10; i++) {
+    rcYawZero += pulseIn(CH4, HIGH, 20000);
+  }
+  rcYawZero /= 10;
 }

 /* Efetua a escrita da potência dos motores entre 1ms e 2ms */
@@ -362,7 +519,7 @@
 /* Efetua a leitura da orientação a ser armazenada na variável "ypr" */
 void readOrientation() {
   bool fifoOverflow = false; // Marca se houve overflow da FIFO durante a execução
-  D(Serial.println("init RO"));
+  // D(Serial.println("init RO"));

   /* Le somente se houve interrupção */
   if (mpuInterrupt) {
@@ -371,16 +528,16 @@

     /* Obtém o byte INT_STATUS do MPU */
     mpuIntStatus = mpu.getIntStatus();
-    D(Serial.println("getIntStatus: " + String(mpuIntStatus)));
+    // D(Serial.println("getIntStatus: " + String(mpuIntStatus)));

     /* Obtém a contagem da FIFO */
     fifoCount = mpu.getFIFOCount();
-    D(Serial.println("getFIFOCount: " + String(fifoCount)));
+    // D(Serial.println("getFIFOCount: " + String(fifoCount)));

     /* Verifica se houve overflow da FIFO. Quanto mais ineficiente o código, mais irá ocorrer. */
     if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
       // Reseta a FIFO
-      D(Serial.println("resetFIFO: " + String(fifoCount)));
+      // D(Serial.println("resetFIFO: " + String(fifoCount)));
       mpu.resetFIFO();
       fifoOverflow = true;
       return;
@@ -399,16 +556,16 @@
       fifoCount = mpu.getFIFOCount();

       /* Aguarda encher a FIFO caso não esteja com os todos dados completos (pacote completo) */
-      Serial.println("Agu pac");
+      // Serial.println("Agu pac");
       int DMPwatchdog = 0;
       while (fifoCount < packetSize && DMPwatchdog < 5) {
         fifoCount = mpu.getFIFOCount();
-        D(Serial.println("Count: " + String(fifoCount)));
+        // D(Serial.println("Count: " + String(fifoCount)));
         DMPwatchdog++;
       }

       if (DMPwatchdog < 5) {
-        D(Serial.println("Pac rec...."));
+        // D(Serial.println("Pac rec...."));

         /* Dado está pronto para ser lido do MPU */
         mpu.getFIFOBytes(fifoBuffer, packetSize);
@@ -417,7 +574,7 @@
         mpu.dmpGetQuaternion(&q, fifoBuffer);
         mpu.dmpGetGravity(&gravity, &q);
         mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
-        D(Serial.println("Fim lei"));
+        // D(Serial.println("Fim lei"));

         /* Converte para graus/segundo */
         for (int i = 0; i < 3; i++) {
@@ -454,7 +611,6 @@
  * - Canal 6 (em us - entre ~1000 a ~2000)
  * - Intervalo do loop (em us)
  */
-
 D(void telemetry() {
   /* Calcula a frequência do loop */
   tmInterval = micros() - tmCheckpoint;
@@ -467,6 +623,9 @@
   tmValues += padding("RCR=" + String(int(rcRoll)), 8) + ";";
   tmValues += padding("RCO=" + String(int(rcOnOff)), 8) + ";";
   tmValues += padding("RCD=" + String(int(rcDimmer)), 8) + ";";
+  tmValues += padding("ZRP=" + String(int(rcPitchZero)), 9) + ";";
+  tmValues += padding("ZRR=" + String(int(rcRollZero)), 9) + ";";
+  tmValues += padding("ZRY=" + String(int(rcYawZero)), 9) + ";";
   tmValues += padding("DMP=" + String(int(ypr_degree[DMP_PITCH])), 10) + ";";
   tmValues += padding("DMR=" + String(int(ypr_degree[DMP_ROLL])), 10) + ";";
   tmValues += padding("DMY=" + String(int(ypr_degree[DMP_YAW])), 10) + ";";
@@ -478,9 +637,9 @@
   tmValues += padding("MBL=" + String(int(motors[MOTOR_BL])), 8) + ";";
   tmValues += padding("MBR=" + String(int(motors[MOTOR_BR])), 8) + ";";
   tmValues += padding("INT=" + String(int(tmInterval)), 10) + ";";
-  for (int i=0; i < 6; i++) {
-   tmValues += padding(String(channels[i]), 8) + ";";
-  }
+  // for (int i=0; i < 6; i++) {
+  //  tmValues += padding(String(channels[i]), 8) + ";";
+  // }

   countToSend++;
   if (countToSend > 4) {
@@ -489,6 +648,7 @@
   }
 }

+/* Cria padding em uma string preenchendo-a com espaços à direita */
 String padding(String text, int size){
   String newString = String(text);
   if (text.length() < size) {


updated top speed

updated top speed

--- arduino/main_control/main_control.ino
+++ arduino/main_control/main_control.ino
@@ -53,6 +53,13 @@
 //controls the acceleration/deccelration of the robot. -100 is full break to 100 full gas
 void set_throttle_position(int sp){
   int map_throttle = map(sp, -100, 100, 1000, 2000);
+  //set minimum is 1400 for reverse and 1550 for slow forward
+  if (map_throttle < 1400){
+    map_throttle = 1400;
+  }
+  if(map_throttle > 1550){
+    map_throttle = 1550;
+  }
   throttle_servo.writeMicroseconds(map_throttle);
 }

@@ -152,7 +159,7 @@
   while (!TX_found){
       //read in the from the controller
       throttle_input = pulseIn(THROTTLE_IN, HIGH, 25000);
-      Serial.println(throttle_input);
+      //Serial.println(throttle_input);

       //toggle red led while waiting to arm
       toggle_LED(LED_RED, true);
@@ -180,13 +187,13 @@
         }
         toggle_LED(LED_GREEN, true);

-          //WARNING: Uncommenting this will lanuch the robot
-        launch();
+         //WARNING: Uncommenting this will lanuch the robot
+        //launch();
         //WHOAH stop this bad boy (NOTICE, setting the throttle position to -100 is full breaks,
         //to go in reverse, use the reverse method
-        set_throttle_position(-100);
-        delay(3000);
-        set_throttle_position(20);
+        //set_throttle_position(-100);
+        //delay(3000);
+        set_throttle_position(10);
       }
   }//end while init transmitter


Update bare_transmitter.ino

Update bare_transmitter.ino

--- bare_transmitter/bare_transmitter.ino
+++ bare_transmitter/bare_transmitter.ino
@@ -10,12 +10,8 @@

 ArduinoNunchuk nunchuk = ArduinoNunchuk();

-class RF24Test: public RF24
-{
-  public: RF24Test(int a, int b): RF24(a, b) {}
-};

-RF24Test radio(9, 10);
+RF24 radio(9, 10);

 // Radio pipe addresses for the 2 nodes to communicate.
 const uint64_t pipes[2] = { 0xF0F0F0F0E1LL, 0xF0F0F0F0D2LL };
@@ -32,20 +28,20 @@
   radio.openWritingPipe(pipes[0]);
   radio.openReadingPipe(1, pipes[1]);

-  radio.printDetails();
+
 }

 void loop()
 {
   nunchuk.update();

-  byte analogX  = nunchuk.analogX;
-  byte analogY  = nunchuk.analogY;
-  byte accelX   = nunchuk.accelX;
-  byte accelY   = nunchuk.accelY;
-  byte accelZ   = nunchuk.accelZ;
-  byte zButton  = nunchuk.zButton;
-  byte cButton  = nunchuk.cButton;
+  byte analogX = nunchuk.analogX;
+  byte analogY = nunchuk.analogY;
+  byte accelX = nunchuk.accelX;
+  byte accelY = nunchuk.accelY;
+  byte accelZ = nunchuk.accelZ;
+  byte zButton = nunchuk.zButton;
+  byte cButton = nunchuk.cButton;
   byte car_data[7];

   car_data[0] = analogX;


Played around a lot with the Blend Power saving settings - now the goal is to have a push button that enables the BLE function for a give time (say, 60 seconds) - push button works, still need to tweak the rest.


some bugs are found when going through the code with Sean

--- glass/glass.ino
+++ glass/glass.ino
@@ -56,8 +56,8 @@
 void loop() {
   if (isWaitingReply) {
     if (XBee.available()) {
-      if (XBeeReturnCount >= 1) {
-        XBeeReturnIDs[XBeeReturnCount*3-2] = ':';
+      if (XBeeReturnCount > 1) {
+        XBeeReturnIDs[XBeeReturnCount*3-1] = ':';
       }
       delay(10);
       readStringfromSerial(&XBee, message);
@@ -78,7 +78,7 @@
       DEBUG_PRINT("IDs counts:");
       DEBUG_PRINTLN(XBeeReturnCount);
       BT.println(XBeeReturnIDs);
-      isWaitingReply = false;
+      isWaitingReply = false;
     }
   }

@@ -119,7 +119,7 @@
   // check message format
   // IDXVARVAL
   if (strlen(message) >= 9 && isFuncValid(message) &&
-      (message[1] <= '9' && message[1] >= '0') &&
+      (message[0] <= '9' && message[0] >= '0') &&
       (message[1] <= '9' && message[1] >= '0'))
     return true;
   return false;


fixed warning panel

fixed warning panel

--- ArduCAM_OSD/OSD_Panels.ino
+++ ArduCAM_OSD/OSD_Panels.ino
@@ -632,7 +632,7 @@
 // Staus  : done

 void panWarn(int first_col, int first_line){
-const char* warning_string[] = {
+static const char* warning_string[] = {
   "              ",    // clear
   "  no GPS fix  ",    //
   "     Stall    ",    //
@@ -643,15 +643,19 @@
   "No communicat."
 };

-	osd.setPanel(first_col, first_line);
-	osd.openPanel();
-
-	if (blinker) {	// every 2 seconds
-		uint8_t x = warning_type + 1;		// start with next we have to rotate all warns
+static long next_warn_check = 0;
+
+	osd.setPanel(first_col, first_line);
+	osd.openPanel();
+
+	if (millis() > next_warn_check) {	// every 2 seconds
+		uint8_t x = warning_type;		// start with next we have to rotate all warns

 		warning = 0;						// if we are lucky

-		while (x != warning_type) {			// full cycle done
+		do {			// full cycle done
+			++x;
+
 			switch (x) {
 			case 0:		break;			// skip
 			case 1:		if (osd_fix_type < 2) warning = 1;
@@ -676,14 +680,19 @@
 				break;
 			}

-			++x;
-		}
-	}
-
+		} while (x != warning_type);
+
+		if (warning == 0) {	// no warning
+			warning_type = 0;		// cancel
+		}
+
+		next_warn_check = millis() + 2000;
+	}
+
 	if (blinker)
-		osd.printf(warning_string[0]);
+		osd.printf(warning_string[warning_type]);
 	else
-		osd.printf(warning_string[warning_type]);
+		osd.printf(warning_string[0]);	// let's blink

 	osd.closePanel();
 }


+ Tuned the Mike.ino settings for peak detection.

--- ClydeClock/Mike.ino
+++ ClydeClock/Mike.ino
@@ -9,11 +9,11 @@
 uint16_t signal_min = 1024;
 uint8_t sample_time = 20;              // sample time in ms.
 uint16_t NOISE_THRESHOLD = 500;
-uint16_t NOISE_THRESHOLD_DIFF = 200;   // the difference between max and min signal to trigger
+uint16_t NOISE_THRESHOLD_DIFF = 180;   // the difference between max and min signal to trigger
 bool is_loud = false;                  // is true as long as the noise is above threshold.
 uint32_t peak_start_millis;            // the time (ms) when the start of a new peak has been detected.
 uint32_t last_peak_millis;             // the time when the last peak was detected.
-uint16_t PEAK_DURATION = 200;          // the maximal duration (in ms) of a short noise peak (clap).
+uint16_t PEAK_DURATION = 100;          // the maximal duration (in ms) of a short noise peak (clap).
 uint8_t clap_count = 0;                // count claps.
 uint16_t PEAK_COUNT_WINDOW = 1000;     // the time window in which we count claps.
 uint16_t TRIGGER_DELAY = 500;          // the delay after the last clap that we wait for any potentially other noise peak.
@@ -37,7 +37,7 @@
   }
 }

-/*
+/*
  * Listen for claps and trigger an event if (more than) one was detected.
  */
 void listenForClaps(){
@@ -72,7 +72,7 @@
 /* how to detect a clap: -> check the ClydeDev TouchyFeely...
  * - measure noise level, keep record of previous level.
  * - if previous noise level was below thresh and current is above -> record clap_start
- * - if previous noise level was above
+ * - if previous noise level was above
  */
 void detectPeakStart(){
   // use the signal_min and signal_max...


Ghost notes that triggers inbetween the two switches, and velocity sensitivity added. Also the F# and G notes codes where added, so as a debug flag for some tests.

--- Pianoduino.ino
+++ Pianoduino.ino
@@ -1,58 +1,60 @@
 /**
- *
Pianoduino

- * 61 key MIDI Keyboard with Arduino Uno Code, 24 shiftout outputs, 24 shiftin inputs
- * 1 resistive touchscreen and 16 analog I/O.
- * @author Malcolm Davis
- * @version 0.1
- * @since 2017-02-03
- */
- //Arduino Pins
- static const int muxAPin = 5;
- static const int muxBPin = 4;
- static const int muxCPin = 3;
- static const int muxReadPin = A0;
- static const int inClkPin = 7; //Shift in clock
- static const int loadPin = 6; //Shift in or load input. When high data, shifted. When Low data is loaded from parallel inputs.
- static const int inEnPin = 8; //Active low
- static const int inReadPin = 9;
- static const int dataPin = 11;
- static const int outClkPin = 12;
- static const int latchPin = 10;
- //If a Analog mux is added, just connect the control pins in parallel and add the En pin to the list.
- static const int muxEn [] = {2};
-
- //Constants
- int mask[] = { B00000001, B00000010, B00000100, B00001000, B00010000, B00100000, B01000000, B10000000 };//To read the columns values
- int blank = B00000000;//To send blank to the register.
- byte noteOn = B10010000;
- byte noteOff = B10000000;
- byte pitchBend = B11100001;
- static const byte  keyMidi[144]={0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-                                  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-                                  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-                                  B00101000, B00101000, B00101001, B00101001, B00101010, B00101010, B00101011, B00101011, B00101100, B00101100, B00100111, B00100111,
-                                  B00101110, B00101110, B00101111, B00101111, B00110000, B00110000, 0, 0, 0, 0, B00101101, B00101101,
-                                  B00111010, B00111010, B00111011, B00111011, 0, 0, B00110111, B00110111, B00111000, B00111000, B00111001, B00111001,
-                                  B00110100, B00110100, B00110101, B00110101, B00110110, B00110110, B00110001, B00110001, B00110010, B00110010, B00110011, B00110011,
-                                  B01010010, B01010010, B01010011, B01010011, B01010100, B01010100, B01001111, B01001111, B01010000, B01010000, B01010001, B01010001,
-                                  B01001100, B01001100, B01001101, B01001101, B01001110, B01001110, B01001001, B01001001, B01001010, B01001010, B01001011, B01001011,
-                                  B01000110, B01000110, B01000111, B01000111, B01001000, B01001000, B01000011, B01000011, B01000100, B01000100, B01000101, B01000101,
-                                  B01000000, B01000000, B01000001, B01000001, B01000010, B01000010, B00111101, B00111101, B00111110, B00111110, B00111111, B00111111,
-                                  0, 0, 0, 0, B00111100, B00111100, 0, 0, 0, 0, 0, 0
+
Pianoduino

+   61 key MIDI Keyboard with Arduino Uno Code, 24 shiftout outputs, 24 shiftin inputs
+   1 resistive touchscreen and 16 analog I/O.
+   @author Malcolm Davis
+   @version 0.1
+   @since 2017-02-03
+*/
+
+//Arduino Pins
+static const int muxAPin = 5;
+static const int muxBPin = 4;
+static const int muxCPin = 3;
+static const int muxReadPin = A0;
+static const int inClkPin = 7; //Shift in clock
+static const int loadPin = 6; //Shift in or load input. When high data, shifted. When Low data is loaded from parallel inputs.
+static const int inEnPin = 8; //Active low
+static const int inReadPin = 9;
+static const int dataPin = 11;
+static const int outClkPin = 12;
+static const int latchPin = 10;
+//If a Analog mux is added, just connect the control pins in parallel and add the En pin to the list.
+static const int muxEn [] = {2};
+
+//Constants
+int mask[] = { B00000001, B00000010, B00000100, B00001000, B00010000, B00100000, B01000000, B10000000 };//To read the columns values
+int blank = B00000000;//To send blank to the register.
+byte noteOn = B10010000;
+byte noteOff = B10000000;
+byte pitchBend = B11100001;
+static const short sensitivity = 1000;
+static bool debug = false;
+static const byte  keyMidi[144] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+                                   0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+                                   0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+                                   B00101000, B00101000, B00101001, B00101001, B00101010, B00101010, B00101011, B00101011, B00101100, B00101100, B00100111, B00100111,
+                                   B00101110, B00101110, B00101111, B00101111, B00110000, B00110000, B00101011, B00101011, B00101100, B00101100, B00101101, B00101101,
+                                   B00111010, B00111010, B00111011, B00111011, 0, 0, B00110111, B00110111, B00111000, B00111000, B00111001, B00111001,
+                                   B00110100, B00110100, B00110101, B00110101, B00110110, B00110110, B00110001, B00110001, B00110010, B00110010, B00110011, B00110011,
+                                   B01010010, B01010010, B01010011, B01010011, B01010100, B01010100, B01001111, B01001111, B01010000, B01010000, B01010001, B01010001,
+                                   B01001100, B01001100, B01001101, B01001101, B01001110, B01001110, B01001001, B01001001, B01001010, B01001010, B01001011, B01001011,
+                                   B01000110, B01000110, B01000111, B01000111, B01001000, B01001000, B01000011, B01000011, B01000100, B01000100, B01000101, B01000101,
+                                   B01000000, B01000000, B01000001, B01000001, B01000010, B01000010, B00111101, B00111101, B00111110, B00111110, B00111111, B00111111,
+                                   0, 0, 0, 0, B00111100, B00111100, 0, 0, 0, 0, 0, 0
                                   };
-
- //Variables
- int actualPin = 0;
- static bool sValue[] = {0, 0, 0}; // S0, S1, S2 values
- int noteVelocity [144];
- bool keyState[144];
- bool rowsRead []= {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-
- /**
-  * Method to setup the pins and stuffs of the pianoduino.
-  * @param none
-  * @return void
-  */
+
+//Variables
+static bool sValue[] = {0, 0, 0}; // S0, S1, S2 values
+long noteTime [72];
+bool keyState[144];
+bool rowsRead [] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+
+/**
+   Method to setup the pins and stuffs of the pianoduino.
+   @param none
+   @return void
+*/
 void setup() {
   int i;
   Serial.begin(115200);
@@ -66,31 +68,31 @@
   pinMode(dataPin, OUTPUT);
   pinMode(latchPin, OUTPUT);
   pinMode(outClkPin, OUTPUT);
-  for (i=0; i< sizeof(muxEn)/sizeof(int); i++) {
+  for (i = 0; i < sizeof(muxEn) / sizeof(int); i++) {
     pinMode(muxEn[i], OUTPUT);
   }
-  for (i=0; i<sizeof(noteVelocity)/sizeof(int);i++){
-    noteVelocity[i]=127;
-  }
-  for (i=0; i<sizeof(keyState)/sizeof(bool);i++){
-    noteVelocity[i]=0;
-  }
-  digitalWrite(loadPin,HIGH); //On HIGH is read ready
+  for (i = 0; i < sizeof(noteTime) / sizeof(long); i++) {
+    noteTime[i] = 0;
+  }
+  for (i = 0; i < sizeof(keyState) / sizeof(bool); i++) {
+    keyState[i] = 0;
+  }
+  digitalWrite(loadPin, HIGH); //On HIGH is read ready
   delay(1000);
 }

 /**
- * Method to select a pin of a mux
- * @param pPin: the mux pin that you want to write or read.
- * @param pMuxEn: the pin of the mux that you want to read from.
- * @return void
- */
-void muxToPin(int pPin, int pMuxEn){
+   Method to select a pin of a mux
+   @param pPin: the mux pin that you want to write or read.
+   @param pMuxEn: the pin of the mux that you want to read from.
+   @return void
+*/
+void muxToPin(int pPin, int pMuxEn) {
   sValue[0] = bitRead(pPin, 0);
   sValue[1] = bitRead(pPin, 1);
   sValue[2] = bitRead(pPin, 2);
-  for(int i=0; i < sizeof(muxEn)/sizeof(int);i++){
+  for (int i = 0; i < sizeof(muxEn) / sizeof(int); i++) {
     digitalWrite(muxEn[i], HIGH);
   }
   digitalWrite(pMuxEn, LOW);
@@ -100,19 +102,19 @@
 }

 /**
- * Method to shift the registers to a matrix column
- * @param pColumn the column you wish to shift to.
- * @return void
- */
-void shiftToColumn(int pColumn){
+   Method to shift the registers to a matrix column
+   @param pColumn the column you wish to shift to.
+   @return void
+*/
+void shiftToColumn(int pColumn) {
   digitalWrite(latchPin, LOW);
-  if(pColumn <8){
+  if (pColumn < 8) {
     shiftOut(dataPin, outClkPin, MSBFIRST, mask[pColumn]);
     shiftOut(dataPin, outClkPin, MSBFIRST, blank);
     shiftOut(dataPin, outClkPin, MSBFIRST, mask[pColumn]);
-  }else{
-    shiftOut(dataPin, outClkPin, MSBFIRST, mask[pColumn%8]);
-    shiftOut(dataPin, outClkPin, MSBFIRST, mask[pColumn%8]);
+  } else {
+    shiftOut(dataPin, outClkPin, MSBFIRST, mask[pColumn % 8]);
+    shiftOut(dataPin, outClkPin, MSBFIRST, mask[pColumn % 8]);
     shiftOut(dataPin, outClkPin, MSBFIRST, blank);
   }
   digitalWrite(latchPin, HIGH);
@@ -120,10 +122,10 @@
 }

 /**
- * Main loop method
- * @param none
- * @return void
- */
+   Main loop method
+   @param none
+   @return void
+*/
 void loop() {
   /**for(int i=0; i<8; i++){
     Serial.print("Y");
@@ -137,35 +139,57 @@
     Serial.print(sValue[1]);
     Serial.print(" ");
     Serial.println(sValue[0]);
-  }*/
-  for (int i =0; i<16; i++){
+    }*/
+  for (int i = 0; i < 16; i++) {
     shiftToColumn(i);
     scanRows();
-    for(int j = 0; j < sizeof(rowsRead)/sizeof(byte);j++){
-      if(i<12){
-        if(rowsRead[j]&&!keyState[i+12*j]){
-            keyState[i+12*j] = 1;
-            if((i+12*j)%2==0){
-              keyState[i+12*j+1]=1;
+    for (int j = 0; j < sizeof(rowsRead) / sizeof(bool); j++) {
+      if (i < 12 and j < 12) {
+        if (rowsRead[j] && !keyState[i + 12 * j]) {
+          keyState[i + 12 * j] = 1;
+          /*if((i+12*j)%2==0){
+            keyState[i+12*j+1]=1;
             }else{
-              keyState[i+12*j-1]=1;
-            }
-            updateVelocity(i+12*j);
-            midiSend(noteOn, keyMidi[i+12*j], 127);
-        }else{
-          if(keyState[i+12*j]&&!rowsRead[j]){
-            keyState[i+12*j] = 0;
-            midiSend(noteOff, keyMidi[i+12*j], 0);
-            noteVelocity[i+12*j]=0;
-            if((i+12*j)%2==0){
-              noteVelocity[i+12*j+1]=0;
-            }else{
-              noteVelocity[i+12*j-1]=0;
+            keyState[i+12*j-1]=1;
+            }*/
+          if ((i + 12 * j) % 2 == 0) {
+            if (keyState[i + 12 * j + 1] == 1) {
+              midiSend(noteOn, keyMidi[i + 12 * j], getVelocity((i + 12 * j) / 2));
+            } else {
+              updateVelocity((i + 12 * j) / 2, (byte)1);
+            }
+          } else {
+            if (keyState[i + 12 * j - 1] == 1) {
+              midiSend(noteOn, keyMidi[i + 12 * j], getVelocity((i + 12 * j) / 2));
+            }
+            else {
+              updateVelocity((i + 12 * j) / 2, (byte)1);
+            }
+          }
+          /*Serial.print("Column ");
+            Serial.println(i);
+            Serial.print("Row ");
+            Serial.println(j);*/
+        } else {
+          if (keyState[i + 12 * j] && !rowsRead[j]) {
+            /*keyState[i + 12 * j] = 0;
+              midiSend(noteOff, keyMidi[i + 12 * j], 0);*/
+            keyState[i + 12 * j] = 0;
+            if ((i + 12 * j) % 2 == 0) {
+              if (keyState[i + 12 * j + 1] == 0) {
+                updateVelocity((i + 12 * j) / 2, (byte)0);
+                midiSend(noteOff, keyMidi[i + 12 * j], 0);
+              }
+            } else {
+              if (keyState[i + 12 * j - 1] == 0) {
+                updateVelocity((i + 12 * j) / 2, (byte)0);
+                midiSend(noteOff, keyMidi[i + 12 * j], 0);
+              }
             }
           }
         }
       }
-      else{
+      else {
         //The other buttons..
       }
     }
@@ -173,20 +197,63 @@
 }

 /**
- * Method that  updates the velocity of a note.
- * @param pPos the position of the note in the array.
- */
-void updateVelocity(int pPos){
-  return ;
-}
-
-/**
- * Method to scan the values of all the inputs of the shiftregisters
- */
-void scanRows(){
+   Method to update the velocity of a note
+   @param pPos the position of the note in the array.
+   @param pMode 0 to turn off the note, 1 to update.
+*/
+void updateVelocity(int pPos, byte pMode) {
+  if (pMode == 0) {
+    noteTime[pPos] = 0;
+  } else {
+    getVelocity(pPos);
+  }
+}
+
+/**
+   Method that  updates the velocity of a note.
+   @param pPos the position of the note in the array.
+*/
+byte getVelocity(int pPos) {
+  if (noteTime[pPos] == 0) {
+    noteTime[pPos] = millis();
+    /* Serial.println("Time");
+      Serial.println(noteTime[pPos], DEC);
+      Serial.println("Velocity");
+      Serial.println(127, DEC);
+      Serial.println("Pos");
+      Serial.println(pPos, DEC);
+      delay(10);*/
+    return 127;
+  } else {
+    int tempVelocity = map(abs(millis() - noteTime[pPos]), 0, sensitivity, 127, 10);
+    noteTime[pPos] = millis();
+    if (tempVelocity < 10) {
+      tempVelocity = 10;
+    }
+    else {
+      if (tempVelocity > 127) {
+        tempVelocity = 127;
+      }
+    }
+    /*Serial.println("Velocity 2");
+      Serial.print(tempVelocity, DEC);
+      Serial.print(" ");
+      Serial.println("Time 2");
+      Serial.println(abs(millis()-noteTime[pPos]), DEC);
+      Serial.println("Pos 2");
+      Serial.println(pPos, DEC);
+      delay(10);*/
+    return tempVelocity;
+  }
+}
+
+/**
+   Method to scan the values of all the inputs of the shiftregisters
+*/
+void scanRows() {
   digitalWrite(loadPin, LOW); //On Low the parallel data is collected
-  delayMicroseconds(5);
-  digitalWrite(loadPin,HIGH); //On HIGH is read ready
+  delayMicroseconds(5);
+  digitalWrite(loadPin, HIGH); //On HIGH is read ready
   delayMicroseconds(5);
   digitalWrite(inEnPin, LOW); //Active Low
   shiftIn(inReadPin, inClkPin);
@@ -194,30 +261,38 @@
 }

 /**
- * Shift in modification method to read varius shift registers.
- * @param pDatapin the pin to read.
- * @param pClk the shift registers clock pin.
- */
- void shiftIn(int pDatapin, int pClk){
-  for (int i = sizeof(rowsRead)/sizeof(byte)-1; i >=0 ;i--)
+   Shift in modification method to read varius shift registers.
+   @param pDatapin the pin to read.
+   @param pClk the shift registers clock pin.
+*/
+void shiftIn(int pDatapin, int pClk) {
+  for (int i = sizeof(rowsRead) / sizeof(byte) - 1; i >= 0 ; i--)
   {
     digitalWrite(inClkPin, LOW);
     delayMicroseconds(5);
     rowsRead[i] = digitalRead(inReadPin);
     digitalWrite(inClkPin, HIGH);
   }
- }
-
-
-/**
- * Method to send the midi messega.
- * @param pCmd the midi command code.
- * @param pPitch the pitch of the note.
- * @param pVelocity of the note.
- * @return void
- */
+}
+
+
+/**
+   Method to send the midi messega.
+   @param pCmd the midi command code.
+   @param pPitch the pitch of the note.
+   @param pVelocity of the note.
+   @return void
+*/
 void midiSend(byte pCmd, byte pPitch, byte pVelocity) {
-  Serial.write(pCmd);
-  Serial.write(pPitch);
-  Serial.write(pVelocity);
+  if (!debug) {
+    Serial.write(pCmd);
+    Serial.write(pPitch);
+    Serial.write(pVelocity);
+  } else {
+    Serial.println("Begin Message.");
+    Serial.println(pCmd, BIN);
+    Serial.println(pPitch, BIN);
+    Serial.println(pVelocity, BIN);
+    Serial.println("End Message.");
+  }
 }


initialise the global to a nullptr

initialise the global to a nullptr

--- SonoffBoilerplate.ino
+++ SonoffBoilerplate.ino
@@ -77,7 +77,7 @@

 #include "MQTTOutbound.h"
-MQTTOutbound * mqttOutbound;
+MQTTOutbound * mqttOutbound = nullptr;


implemented working http post of temperature

--- SpaceAPI/SpaceAPI.ino
+++ SpaceAPI/SpaceAPI.ino
@@ -11,9 +11,9 @@
 // CONFIG:
 #define BAUDRATE 9600
 //const char api[]      PROGMEM = "spaceapi.net";
-//const char endpoint[] PROGMEM = "new/space/losfuzzyslab/sensor/set";
+//const char endpoint[] PROGMEM = "/new/space/losfuzzyslab/sensor/set";
 const char api[]      PROGMEM = "think.2904.cc";
-const char endpoint[] PROGMEM = "losfuzzys/index.php";
+const char endpoint[] PROGMEM = "/losfuzzys/index.php";

 #define PIN_ONEWIRE 2
 #define TEMPERATURE_PRECISION 9 // Lower resolution
@@ -26,7 +26,7 @@

 static byte session;
 byte Ethernet::buffer[700];
-Stash stash;
+char sendBuffer[128];

 float oldTemp = 0;
 int numberOfDevices;
@@ -41,73 +41,21 @@

   if (DEBUG) return;

-  byte sd = stash.create();
+  snprintf(sendBuffer, sizeof sendBuffer, "key=%s&temp=%d \0", TOKEN, (int)temp);
+  ether.httpPost(endpoint, api, NULL, sendBuffer, NULL);

-  ether.dnsLookup(api);
-
-  stash.print("key=");
-  stash.print(TOKEN);
-  stash.print("&temp=");
-  stash.println(42.0);
-  stash.save();
-  int stash_size = stash.size();
-
-  Stash::prepare(PSTR("POST http://$F/$F HTTP/1.0" "\r\n"
-                      "Host: $F" "\r\n"
-                      "Content-Type: application/x-www-form-urlencoded \r\n"
-                      "Content-Length: $D" "\r\n"
-                      "\r\n"
-                      "$H"),
-                 api, endpoint, api, stash_size, sd);
-
-  Serial.println(Stash::length());
-  for (word i = 0, n = Stash::length(); i < n; ++i)
-  {
-    char c;
-    Stash::extract(i, 1, &c);
-    Serial.print(c);
-  }
-  Serial.println();
-
-  // send the packet - this also releases all stash buffers once done
-  // Save the session ID so we can watch for it in the main loop.
-  session = ether.tcpSend();
-
-  Serial.println("[>] Done sending temperature!");
+//  Serial.println("[>] Done sending temperature!");
 }

 static void updateDoorstatus(int isOpen) {
   Serial.println("[>] Sending new doorstatus ...");
-
+
   if (DEBUG) return;

-  byte sd = stash.create();
-
-  //const char s_error[]  = "%7B%22state%22%3A%7B%22open%22%3Anull%7D%7D";
-  //const char s_open[]  = "%7B%22state%22%3A%7B%22open%22%3Atrue%7D%7D";
-  //const char s_close[] = "%7B%22state%22%3A%7B%22open%22%3Afalse%7D%7D";
-  stash.print("key=");
-  stash.print(TOKEN);
-  //stash.print("&sensors=");
-  //stash.println(isOpen == DOOR_OPEN ? s_open : (isOpen == DOOR_CLOSED ? s_close : s_error));
-  stash.print("&door=");
-  stash.println(isOpen == DOOR_OPEN ? "true" : (isOpen == DOOR_CLOSED ? "false" : "null"));
-  stash.save();
-  int stash_size = stash.size();
-
-  Stash::prepare(PSTR("POST /$F HTTP/1.0" "\r\n"
-                      "Host: $F" "\r\n"
-                      "Content-type: application/x-www-form-urlencoded" "\r\n"
-                      "Content-length: $D" "\r\n"
-                      "\r\n"
-                      "$H"),
-                 endpoint, api, stash_size, sd);
-
-  // send the packet - this also releases all stash buffers once done
-  // Save the session ID so we can watch for it in the main loop.
-  session = ether.tcpSend();
-
-  Serial.println("[>] Done sending doorstatus!");
+  snprintf(sendBuffer, sizeof sendBuffer, "key=%s&door=%d \0", TOKEN, isOpen);
+  ether.httpPost(endpoint, api, NULL, sendBuffer, NULL);
+
+//  Serial.println("[>] Done sending doorstatus!");
 }

 void findTempSensor() {
@@ -116,17 +64,17 @@
     numberOfDevices = sensors.getDeviceCount();

     // locate devices on the bus
-    Serial.print("[T] Locating one-wire devices on port ");
-    Serial.println(PIN_ONEWIRE);
-
+    //    Serial.print("[T] Locating one-wire devices on port ");
+    //    Serial.println(PIN_ONEWIRE);
+    //
     Serial.print("[T] Found ");
     Serial.print(numberOfDevices, DEC);
     Serial.println(" one-wire devices.");
-
-    // report parasite power requirements
-    Serial.print("[T] Parasite power is: ");
-    if (sensors.isParasitePowerMode()) Serial.println("ON");
-    else Serial.println("OFF");
+    //
+    //    // report parasite power requirements
+    //    Serial.print("[T] Parasite power is: ");
+    //    if (sensors.isParasitePowerMode()) Serial.println("ON");
+    //    else Serial.println("OFF");

     if (numberOfDevices < 1) delay(500);
   }
@@ -135,42 +83,16 @@
   {
     if (sensors.getAddress(tempDeviceAddress, i))
     {
-      Serial.print("[T] Found device ");
-      Serial.println(i, DEC);
+      //      Serial.print("[T] Found device ");
+      //      Serial.println(i, DEC);
       sensors.setResolution(tempDeviceAddress, TEMPERATURE_PRECISION);
     } else
     {
-      Serial.print("[T] Found ghost device at ");
-      Serial.print(i, DEC);
-      Serial.print(" but could not detect address. Check power and cabling");
+      //      Serial.print("[T] Found ghost device at ");
+      //      Serial.print(i, DEC);
+      //      Serial.print(" but could not detect address. Check power and cabling");
     }
   }
-}
-
-static void sendToTwitter () {
-  Serial.println("Sending tweet...");
-  byte sd = stash.create();
-
-  const char tweet[] = "@solarkennedy the test Twitter sketch works!";
-  stash.print("token=");
-  stash.print(TOKEN);
-  stash.print("&status=");
-  stash.println(tweet);
-  stash.save();
-  int stash_size = stash.size();
-
-  // Compose the http POST request, taking the headers below and appending
-  // previously created stash in the sd holder.
-  Stash::prepare(PSTR("POST http://$F/update HTTP/1.0" "\r\n"
-    "Host: $F" "\r\n"
-    "Content-Length: $D" "\r\n"
-    "\r\n"
-    "$H"),
-  api, api, stash_size, sd);
-
-  // send the packet - this also releases all stash buffers once done
-  // Save the session ID so we can watch for it in the main loop.
-  session = ether.tcpSend();
 }

 void setup () {
@@ -190,7 +112,7 @@
     if (!ether.dhcpSetup())
       Serial.println(F("[N] DHCP failed"));

-    ether.printIp("[N] IP:  ", ether.myip);
+    //    ether.printIp("[N] IP:  ", ether.myip);
     ether.printIp("[N] GW:  ", ether.gwip);
     ether.printIp("[N] DNS: ", ether.dnsip);

@@ -199,7 +121,6 @@

     ether.printIp("[N] SRV: ", ether.hisip);
   }
-  sendToTwitter();
 }

 float getTemp() {
@@ -209,35 +130,29 @@
   return temp;
 }

+static uint32_t timer;
 void loop () {
   ether.packetLoop(ether.packetReceive());

-  // TODO: check doorsensor
-  int newDoorstatus = DOOR_ERROR;
-  float newTemp = getTemp();
+  if (millis() > timer) {
+    timer = millis() + 5000;

-  if (newDoorstatus != oldDoorstatus) {
-    Serial.println(newDoorstatus == DOOR_OPEN ? "[!] DOOR OPENED!" : "[!] DOOR CLOSED!");
-    updateDoorstatus(newDoorstatus);
-    oldDoorstatus = newDoorstatus;
+    // TODO: check doorsensor
+    int newDoorstatus = DOOR_ERROR;
+    float newTemp = getTemp();

-  } else if (newTemp != oldTemp) { // comparing floats? well ...
-    Serial.print("[!] Temp updated: ");
-    Serial.println(newTemp);
-    updateTemperature(newTemp);
-    //oldTemp = newTemp;
-  } else Serial.println("[*] Nothing to do.");
+    if (newDoorstatus != oldDoorstatus) {
+      Serial.println(newDoorstatus == DOOR_OPEN ? "[!] DOOR OPENED!" : "[!] DOOR CLOSED!");
+      updateDoorstatus(newDoorstatus);
+      oldDoorstatus = newDoorstatus;

-  // HTTP response handling (mostly for debug)
-  // (this may need all your dynamic memory)
+    } else if (newTemp != oldTemp) { // comparing floats? well ...
+      Serial.print("[!] Temp updated: ");
+      Serial.println(newTemp);
+      updateTemperature(newTemp);
+      //oldTemp = newTemp;
+    } else Serial.println("[*] Nothing to do.");

-  //  const char* reply = ether.tcpReply(session);
-  //  if (reply != 0) {
-  //    Serial.println("[<] Got a response:");
-  //    Serial.println(reply);
-  //  }
-
-  delay(5000);
-
+  }
 }


Update TODO

Update TODO

--- LoaderTest/LoaderTest.ino
+++ LoaderTest/LoaderTest.ino
@@ -18,7 +18,6 @@
 // For example:
 //   python sendGt1.py life3.gt1

-// XXX Add commands for sending game controller signals
 // XXX Better check if Gigatron is running OK (judge signals better)
 // XXX Try higher than 9600 transfer rates again
 // XXX Wild idea: let the ROM communicate back by modulating vPulse


Various updates

Various updates

Changed capitalization for comment

Enabled support for 8 laser beams

Changed inttomask to an array, which offers better performance. This also
saves us the cost of having to calculate i + 1, since arrays are also 0
indexed.

--- Guzheng/Guzheng.ino
+++ Guzheng/Guzheng.ino
@@ -1,4 +1,4 @@
-/* Code for laser guzheng
+/* Code for laser GuZheng
  * Copyright (C) 2016 Jason Lu
  *
  * Licensed under the MIT License
@@ -12,12 +12,12 @@

 // Port I/O defines
 #define THRESHOLD 500
-#define LASTPORT 4
+#define LASTPORT 7
 #define BUTTONPORT 2

 // MIDI property defines
 #define VOLUME 0x7F
-#define DELAY 10
+#define DELAY 0

 // MIDI command defines
 #define NOTEON 0x90
@@ -41,14 +41,7 @@
  * work together with the lastNote variable to keep track of blocked
  * lasers. Here, I use an AND operator to read if the value has been
  * set, and OR to set the value. */
-uint8_t laser1 = 0x01;
-uint8_t laser2 = 0x02;
-uint8_t laser3 = 0x04;
-uint8_t laser4 = 0x08;
-uint8_t laser5 = 0x10;
-uint8_t laser6 = 0x20;
-uint8_t laser7 = 0x40;
-uint8_t laser8 = 0x80;
+uint8_t laserArray[8] = {0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80};

 // Variable to keep track of which set of notes (lower or upper 4)
 // Needed to work around limited lasers
@@ -144,29 +137,6 @@
   }
 }

-uint8_t inttomask(int i) {
-  switch (i) {
-    case 1:
-      return laser1;
-    case 2:
-      return laser2;
-    case 3:
-      return laser3;
-    case 4:
-      return laser4;
-    case 5:
-      return laser5;
-    case 6:
-      return laser6;
-    case 7:
-      return laser7;
-    case 8:
-      return laser8;
-    default:
-      return 0x00;
-  }
-}
-
 void loop()
 {
   // Main loop
@@ -174,26 +144,26 @@
   for (int i = 0; i <= LASTPORT; i++) {
     if(analogRead(i) > THRESHOLD) {
 #ifndef WIND
-      if (!(lastNote & (inttomask(i+1)))) {
+      if (!(lastNote & (laserArray[i]))) {
 #endif
         // Beam has been cut
         // i+1 is because i is 0 indexed, but the ID starts with 1
         musicOn(i + 1);
 #ifndef WIND
-        lastNote |= (inttomask(i+1));
+        lastNote |= (laserArray[i]);
       }
 #endif
     } else {
 #ifndef WIND
-      if ((lastNote & (inttomask(i+1)))) {
-        lastNote &= ~(inttomask(i+1));
+      if ((lastNote & (laserArray[i]))) {
+        lastNote &= ~(laserArray[i]);
       }
 #endif
       // Hand is gone. Stop the sound
       musicOff(i + 1);
     }
     // Delay 10 to prevent weird analog readings
-    // delay(DELAY);
+    delay(DELAY);
   }
 }


Fix: position is a boolean instead of an int, now

--- signalboxes/kelberg/kelberg.ino
+++ signalboxes/kelberg/kelberg.ino
@@ -172,7 +172,8 @@

 void change_position(int number,
-                     int new_position) {
+                     boolean new_position) {
+  // DEBUG LED TO SHOF IF POSITION-CHANGE happens correctly.
   if (new_position == MINUS) {
     digitalWrite(13, HIGH);
   }
@@ -195,7 +196,7 @@

 void react_to_movement(int number,
-                       int new_position) {
+                       boolean new_position) {
   if (movement_allowed(number)) {
     change_position(number, new_position);
   }


Fixed a bug in wrap around where it was using the wrong upper bound.

--- InfinityGoggles.ino
+++ InfinityGoggles.ino
@@ -111,7 +111,7 @@
     if (value < 0) {
         return value + maxValue;
     }
-    if (value > NUM_LEDS - 1) {
+    if (value > maxValue - 1) {
         return value - maxValue;
     }
     return value;


Enable KIT neopixel in main loop() - NOT TESTED!

--- dayrider/dayrider.ino
+++ dayrider/dayrider.ino
@@ -2,12 +2,19 @@
 #include <Mailbox.h>

 #include <Dagu4Motor.h>
-
+#include <Adafruit_NeoPixel.h>
+#include "NeoPixel.h"
                  // pwm, dir, cur(analog), encA, encB
 Dagu4Motor left_front(5, 4, 0, 0, 0);
 Dagu4Motor left_back(3, 2, 0, 0, 0);
 Dagu4Motor right_front(11, 12, 0, 0, 0);
 Dagu4Motor right_back(9, 8, 0, 0, 0);
+
+// NeoPixel - KIT emulation :D
+#define PIXEL_PIN 6
+#define PIXEL_COUNT 8
+#define KIT_LIGHT_DELAY 40 // in milliseconds
+NeoPixel pixel = NeoPixel(PIXEL_COUNT, PIXEL_PIN, NEO_GRB + NEO_KHZ800);

 void setup() {
   //Serial.begin(9600);
@@ -26,6 +33,8 @@
   right_back.begin();
   right_back.stopMotors();

+  pixel.begin();
+  pixel.reset();
 }

 #define LEFT 1
@@ -41,6 +50,7 @@
 #define MOTOR_SPEED 100

 int current_direction = 0;
+int prevMillis = 0;

 void left_motor(int dir, int spd) {
      left_front.setMotorDirection(dir);
@@ -149,7 +159,11 @@
 }

 void loop() {
-
+  unsigned long curMillis = millis();
+  if (curMillis - prevMillis >= KIT_LIGHT_DELAY) {
+    prevMillis = curMillis;
+    pixel.kitLightNoTimer();
+  }
   poll_mailbox();

   delay(10);


update controller

update controller

--- document/arduino/motor_control/motor_control.ino
+++ document/arduino/motor_control/motor_control.ino
@@ -16,18 +16,19 @@
 #include <MsTimer2.h>

 // Pin define
-const int out1_pin = 5;
-const int out2_pin = 6;
-const int out3_pin = 9;
-const int out4_pin = 10;
-const int decoder_pin_1 = 3;
-const int decoder_pin_2 = 2;
+static const int out1_pin = 5;
+static const int out2_pin = 6;
+static const int out3_pin = 9;
+static const int out4_pin = 10;
+static const int decoder_pin_1 = 3;
+static const int decoder_pin_2 = 2;

 // Timer internal
-const int timer_period = 50; // ms
-const float timer_hz = 1000.0/( (float)timer_period ); // hz
-const int filter_size = 5;
-const int counter_protect = 200;
+static const int timer_period = 50; // ms
+static const float timer_hz = 1000.0/( (float)timer_period ); // hz
+static const float timer_dt = timer_period/1000.0; // sec
+static const int filter_size = 5;
+static const int counter_protect = 200;

 // encoder
 volatile int decoder_pin_1_counter = 0;
@@ -41,8 +42,15 @@
 // controller
 volatile float WL_ref = 0.0; //reference speed for left wheel (deg/sec)
 volatile float WR_ref = 0.0;
-volatile float Kp_L = 0.2; //0.11 //without loading
-volatile float Kp_R = 0.2;
+static const float Kp_L = 0.2; //0.11 //without loading
+static const float Kp_R = 0.2;
+static const float Ki_L = 0.2;
+static const float Ki_R = 0.2;
+volatile float integralL = 0.0;
+volatile float integralR = 0.0;
+static const float integral_dec = 0.99; // for position control, it should be 1.0
+static const float integral_max = 400;
+static const float integral_min = -400;

 void setup() {
   // Set 4 pwm channel pin to output
@@ -133,7 +141,13 @@
   if(WL_ref > 0.0)
   {
     error = WL_ref - average_counter_pin1*(float)(encoder_res)*timer_hz; // deg/sec
-    cmd = feedforward(WL_ref) + error*Kp_L;
+    integralL = integralL*integral_dec + error*timer_dt;
+    if(integralL > integral_max)
+      integralL = integral_max;
+    if(integralL < integral_min)
+      integralL = integral_min;
+    cmd = feedforward(WL_ref) + error*Kp_L + integralL*Ki_L; // controller
+    Serial.println(cmd);
     if(cmd>=0.0)
     {
       analogWrite(out1_pin, (int)cmd);
@@ -148,7 +162,12 @@
   else if(WL_ref < 0.0) // the encoder has no capability of distinguishing the cw/ccw rotation
   {
     error = -WL_ref - average_counter_pin1*(float)(encoder_res)*timer_hz; // deg/sec
-    cmd = feedforward(-WL_ref) + error*Kp_L;
+    integralL = integralL*integral_dec + error*timer_dt;
+    if(integralL > integral_max)
+      integralL = integral_max;
+    if(integralL < integral_min)
+      integralL = integral_min;
+    cmd = feedforward(-WL_ref) + error*Kp_L + integralL*Ki_L; // controller
     if(cmd>=0.0)
     {
       analogWrite(out2_pin, (int)cmd);
@@ -169,7 +188,12 @@
   if(WR_ref > 0.0)
   {
     error = WR_ref - average_counter_pin2*(float)(encoder_res)*timer_hz; // deg/sec
-    cmd = feedforward(WR_ref) + error*Kp_R;
+    integralR = integralR*integral_dec + error*timer_dt;
+    if(integralR > integral_max)
+      integralR = integral_max;
+    if(integralR < integral_min)
+      integralR = integral_min;
+    cmd = feedforward(WR_ref) + error*Kp_R + integralR*Ki_R; // controller
     if(cmd>=0.0)
     {
       analogWrite(out3_pin, (int)cmd);
@@ -184,7 +208,12 @@
   else if(WR_ref < 0.0)// the encoder has no capability of distinguishing the cw/ccw rotation
   {
     error = -WR_ref - average_counter_pin2*(float)(encoder_res)*timer_hz; // deg/sec
-    cmd = feedforward(-WR_ref) + error*Kp_R;
+    integralR = integralR*integral_dec + error*timer_dt;
+    if(integralR > integral_max)
+      integralR = integral_max;
+    if(integralR < integral_min)
+      integralR = integral_min;
+    cmd = feedforward(-WR_ref) + error*Kp_R + integralR*Ki_R; // controller
     if(cmd>=0.0)
     {
       analogWrite(out4_pin, (int)cmd);


Update normal.ino

Update normal.ino

--- examples/normal/normal.ino
+++ examples/normal/normal.ino
@@ -54,8 +54,8 @@

   if(Device.Connect(mac, httpServer))					          // connect the device with the IOT platform.
   {
-    Device.AddAsset(sensorId, F("Sensor_name"), F("your sensor description"), false, F("int"));
-    Device.AddAsset(actuatorId, F("actuator_name"), F("your actuator description"), true, F("bool"));
+    Device.AddAsset(sensorId, F("YOUR_SENSOR_NAME_HERE"), F("YOUR_SENSOR_DESCRIPTION_HERE"),false, F("int"));
+    Device.AddAsset(actuatorId, F("YOUR_ACTUATOR_NAME_HERE"), F("YOUR_ACTUATOR_DESCRIPTION_HERE"), true, F("bool"));
     Device.Subscribe(pubSub);						        // make certain that we can receive message from the iot platform (activate mqtt)
   }
   else


Remove debug

Remove debug

--- firmware4/src/IoTBox4.ino
+++ firmware4/src/IoTBox4.ino
@@ -99,16 +99,12 @@
     // Update button state
     button.Update();

-
     // Save click codes in function, as click codes are reset at next Update()
     if (button.clicks != 0) {
         function = button.clicks;
-        Serial.print("found click: ");
-        Serial.println(function);
     }

     if (function == 2 || function == -2) { // Double (LONG) click
-        Serial.println("startAuthentication");
         visualLEDs.setColor(VisualLEDs::Color::green);
         pinManager.startAuthentication();
     }
@@ -138,7 +134,7 @@
                 startOpenOperation(3);
                 break;
             case AUTHENTICATED_4:
-                visualLEDs.setColor(VisualLEDs::Color::disabled);
+                visualLEDs.setColor(VisualLEDs::Color::disabled);
                 startOpenOperation(4);
                 break;
             case INVALID:


Enhanced MultipleChannels example.

Enhanced MultipleChannels example.

--- examples/MultipleChannels/MultipleChannels.ino
+++ examples/MultipleChannels/MultipleChannels.ino
@@ -21,22 +21,41 @@
 #include <MidiSoftPedal.h>
 #include <MidiSostenutoPedal.h>

+// Program configuration
 #define DATA_PIN 2          // LED strip data pin
 #define STATUS_LED_PIN 13   // Teensy 3.1 onboard LED
 #define NOTE_MIN 0x15       // note 21 (first note on standard 88 keys keyboard)
 #define NOTE_MAX 0x6C       // note 108 (last note on standard 88 keys keyboard)
-#define CHANNEL_MIN 1       // from MIDI Channel 1
-#define CHANNEL_MAX 8       // to MIDI Channel 8
 #define TIME_RANGE 5000     // Time range for setting parameters from MIDI control messages
-#define NUM_LEDS (NOTE_MAX - NOTE_MIN + 1)
-#define NUM_CHANNELS (CHANNEL_MAX - CHANNEL_MIN + 1)
-#define ML_NUM (channel - CHANNEL_MIN)
+
+// MIDI channels to listen (for now this is tricky, be careful when setting this section)
+#define NUM_CHANNELS 9  // The number of total channels you will listen for (depends on RAM!)
+#define CHANNELS 0b0000001011111111 // From right-to-left, put 1s or 0s to map MIDI channels
+const size_t ML_INDEX[16] = { // From left-to-right put a correlative index (starts with 0)
+  0, 1, 2, 3, 4, 5, 6, 7, -1, 8, -1, -1, -1, -1, -1 // you can use -1 to mark unused channels
+};
+
+// MIDI Control Change (CC) control bytes definitions
+#define CC_COLOR_MAPPER           0x14
+#define CC_NOTE_COLOR_MAP         0x15
+#define CC_FIXED_HUE              0x16
+#define CC_ATTACK_TIME            0x17
+#define CC_DECAY_TIME             0x18
+#define CC_SUSTAIN_LEVEL          0x19
+#define CC_RELEASE_TIME           0x1A
+#define CC_IGNORE_VELOCITY        0x1B
+#define CC_BASE_BRIGHTNESS        0x1C
+#define CC_ALL_NOTES_OFF          0x7B
+#define CC_RESET_ALL_CONTROLLERS  0x79
+#define CC_DAMPER_PEDAL           0x40
+#define CC_SOFT_PEDAL             0x42
+#define CC_SOSTENUTO_PEDAL        0x43

 //***********************************************************************
 // Global objects

 elapsedMillis elapsedTime;
-CRGB leds[NUM_LEDS];
+CRGB leds[NOTE_MAX - NOTE_MIN + 1];
 MidiLeds midiLeds[NUM_CHANNELS];
 MidiDamperPedal damperPedal;
 MidiDamperPedal softPedal;
@@ -52,7 +71,7 @@
     pinMode(STATUS_LED_PIN, OUTPUT);

     // Init FastLED
-    FastLED.addLeds<WS2812B, DATA_PIN, GRB>(leds, NUM_LEDS);
+    FastLED.addLeds<WS2812B, DATA_PIN, GRB>(leds, NOTE_MAX - NOTE_MIN + 1);
     FastLED.setDither(0);
     FastLED.setCorrection(TypicalSMD5050);

@@ -97,88 +116,80 @@
 }

 void sostenutoNoteOn(uint8_t channel, uint8_t note, uint8_t velocity) {
-    midiLeds[channel & 0xF].noteOn(note, velocity);
+    midiLeds[ML_INDEX[channel]].noteOn(note, velocity);
 }

 void sostenutoNoteOff(uint8_t channel, uint8_t note) {
-    midiLeds[channel & 0xF].noteOff(note);
+    midiLeds[ML_INDEX[channel]].noteOff(note);
 }

 //***********************************************************************
 // Message handlers for MIDI input (MIDI channel comes in range 1..16)

 void onNoteOn(uint8_t channel, uint8_t note, uint8_t velocity) {
-    if (channel < CHANNEL_MIN || channel > CHANNEL_MAX)
+    if (!bitRead(CHANNELS, channel - 1))
         return;
     damperPedal.noteOn(channel - 1, note, velocity);
     digitalWrite(STATUS_LED_PIN, LOW);
 }

 void onNoteOff(uint8_t channel, uint8_t note, uint8_t velocity) {
-    if (channel < CHANNEL_MIN || channel > CHANNEL_MAX)
+    if (!bitRead(CHANNELS, channel - 1))
         return;
     damperPedal.noteOff(channel - 1, note);
     digitalWrite(STATUS_LED_PIN, LOW);
 }

 void onControlChange(uint8_t channel, uint8_t control, uint8_t value)  {
-    if (channel < CHANNEL_MIN || channel > CHANNEL_MAX)
+    if (!bitRead(CHANNELS, channel - 1))
         return;
+    size_t mlIndex = ML_INDEX[channel - 1];
     switch (control) {
-        case 0x00: // Bank Select
+        case CC_COLOR_MAPPER:
             switch (value) {
-                case 0x00: midiLeds[ML_NUM].setColorMapper(MidiColorMapper::COLOR_MAP); initNotes(ML_NUM); break;
-                case 0x01: midiLeds[ML_NUM].setColorMapper(MidiColorMapper::RAINBOW); initNotes(ML_NUM); break;
-                case 0x02: midiLeds[ML_NUM].setColorMapper(MidiColorMapper::FIXED_COLOR); initNotes(ML_NUM); break;
+                case 0x00: midiLeds[mlIndex].setColorMapper(MidiColorMapper::COLOR_MAP); initNotes(mlIndex); break;
+                case 0x01: midiLeds[mlIndex].setColorMapper(MidiColorMapper::RAINBOW); initNotes(mlIndex); break;
+                case 0x02: midiLeds[mlIndex].setColorMapper(MidiColorMapper::FIXED_COLOR); initNotes(mlIndex); break;
             }
             break;
-        case 0x20: // Bank Select (fine)
+        case CC_NOTE_COLOR_MAP:
             switch (value) {
-                case 0x00: midiLeds[ML_NUM].setNoteColorMap(MidiNoteColors::AEPPLI_1940); initNotes(ML_NUM); break;
-                case 0x01: midiLeds[ML_NUM].setNoteColorMap(MidiNoteColors::BELMONT_1944); initNotes(ML_NUM); break;
-                case 0x02: midiLeds[ML_NUM].setNoteColorMap(MidiNoteColors::BERTRAND_1734); initNotes(ML_NUM); break;
-                case 0x03: midiLeds[ML_NUM].setNoteColorMap(MidiNoteColors::BISHOP_1893); initNotes(ML_NUM); break;
-                case 0x04: midiLeds[ML_NUM].setNoteColorMap(MidiNoteColors::FIELD_1816); initNotes(ML_NUM); break;
-                case 0x05: midiLeds[ML_NUM].setNoteColorMap(MidiNoteColors::HELMHOLTZ_1910); initNotes(ML_NUM); break;
-                case 0x06: midiLeds[ML_NUM].setNoteColorMap(MidiNoteColors::JAMESON_1844); initNotes(ML_NUM); break;
-                case 0x07: midiLeds[ML_NUM].setNoteColorMap(MidiNoteColors::KLEIN_1930); initNotes(ML_NUM); break;
-                case 0x08: midiLeds[ML_NUM].setNoteColorMap(MidiNoteColors::NEWTON_1704); initNotes(ML_NUM); break;
-                case 0x09: midiLeds[ML_NUM].setNoteColorMap(MidiNoteColors::RIMINGTON_1893); initNotes(ML_NUM); break;
-                case 0x0A: midiLeds[ML_NUM].setNoteColorMap(MidiNoteColors::SCRIABIN_1911); initNotes(ML_NUM); break;
-                case 0x0B: midiLeds[ML_NUM].setNoteColorMap(MidiNoteColors::SEEMANN_1881); initNotes(ML_NUM); break;
-                case 0x0C: midiLeds[ML_NUM].setNoteColorMap(MidiNoteColors::ZIEVERINK_2004); initNotes(ML_NUM); break;
+                case 0x00: midiLeds[mlIndex].setNoteColorMap(MidiNoteColors::AEPPLI_1940); initNotes(mlIndex); break;
+                case 0x01: midiLeds[mlIndex].setNoteColorMap(MidiNoteColors::BELMONT_1944); initNotes(mlIndex); break;
+                case 0x02: midiLeds[mlIndex].setNoteColorMap(MidiNoteColors::BERTRAND_1734); initNotes(mlIndex); break;
+                case 0x03: midiLeds[mlIndex].setNoteColorMap(MidiNoteColors::BISHOP_1893); initNotes(mlIndex); break;
+                case 0x04: midiLeds[mlIndex].setNoteColorMap(MidiNoteColors::FIELD_1816); initNotes(mlIndex); break;
+                case 0x05: midiLeds[mlIndex].setNoteColorMap(MidiNoteColors::HELMHOLTZ_1910); initNotes(mlIndex); break;
+                case 0x06: midiLeds[mlIndex].setNoteColorMap(MidiNoteColors::JAMESON_1844); initNotes(mlIndex); break;
+                case 0x07: midiLeds[mlIndex].setNoteColorMap(MidiNoteColors::KLEIN_1930); initNotes(mlIndex); break;
+                case 0x08: midiLeds[mlIndex].setNoteColorMap(MidiNoteColors::NEWTON_1704); initNotes(mlIndex); break;
+                case 0x09: midiLeds[mlIndex].setNoteColorMap(MidiNoteColors::RIMINGTON_1893); initNotes(mlIndex); break;
+                case 0x0A: midiLeds[mlIndex].setNoteColorMap(MidiNoteColors::SCRIABIN_1911); initNotes(mlIndex); break;
+                case 0x0B: midiLeds[mlIndex].setNoteColorMap(MidiNoteColors::SEEMANN_1881); initNotes(mlIndex); break;
+                case 0x0C: midiLeds[mlIndex].setNoteColorMap(MidiNoteColors::ZIEVERINK_2004); initNotes(mlIndex); break;
             }
             break;
-        case 0x4D: // Vibrato Depth
-            midiLeds[ML_NUM].setFixedHue(round(0xFF * (value * 1.0f / 0x7F))); initNotes(ML_NUM); break;
-        case 0x49: // Attack Time
-            midiLeds[ML_NUM].setAttackTime(round(TIME_RANGE * (value * 1.0f / 0x7F))); break;
-        case 0x4B: // Decay Time
-            midiLeds[ML_NUM].setDecayTime(round(TIME_RANGE * (value * 1.0f / 0x7F))); break;
-        case 0x46: // Sound Variation
-            midiLeds[ML_NUM].setSustainLevel(1.0f * (value * 1.0f / 0x7F)); break;
-        case 0x48: // Release Time
-            midiLeds[ML_NUM].setReleaseTime(round(TIME_RANGE * (value * 1.0f / 0x7F))); break;
-        case 0x47: // Timbre Intensity
-            midiLeds[ML_NUM].setIgnoreVelocity(value < 0x40 ? false : true); break;
-        case 0x4A: // Brightness
-            midiLeds[ML_NUM].setBaseBrightness(value); initNotes(ML_NUM); break;
-        case 0x7B: // All Notes Off
-            midiLeds[ML_NUM].allNotesOff(); break;
-        case 0x79: // Reset All Controllers
-            midiLeds[ML_NUM].resetAllControllers(); break;
-        case 0x40: // Damper Pedal
+        case CC_FIXED_HUE: midiLeds[mlIndex].setFixedHue(round(0xFF * (value * 1.0f / 0x7F))); initNotes(mlIndex); break;
+        case CC_ATTACK_TIME: midiLeds[mlIndex].setAttackTime(round(TIME_RANGE * (value * 1.0f / 0x7F))); break;
+        case CC_DECAY_TIME: midiLeds[mlIndex].setDecayTime(round(TIME_RANGE * (value * 1.0f / 0x7F))); break;
+        case CC_SUSTAIN_LEVEL: midiLeds[mlIndex].setSustainLevel(1.0f * (value * 1.0f / 0x7F)); break;
+        case CC_RELEASE_TIME: midiLeds[mlIndex].setReleaseTime(round(TIME_RANGE * (value * 1.0f / 0x7F))); break;
+        case CC_IGNORE_VELOCITY: midiLeds[mlIndex].setIgnoreVelocity(value < 0x40 ? false : true); break;
+        case CC_BASE_BRIGHTNESS: midiLeds[mlIndex].setBaseBrightness(value); initNotes(mlIndex); break;
+        case CC_ALL_NOTES_OFF: midiLeds[mlIndex].allNotesOff(); break;
+        case CC_RESET_ALL_CONTROLLERS: midiLeds[mlIndex].resetAllControllers(); break;
+        case CC_DAMPER_PEDAL:
             if (value < 0x40) damperPedal.release(channel - 1);
             else damperPedal.press(channel - 1);
             break;
-        case 0x42: // Sostenuto Pedal
+        case CC_SOFT_PEDAL:
+            if (value < 0x40) softPedal.release(channel - 1);
+            else softPedal.press(channel - 1);
+            break;
+        case CC_SOSTENUTO_PEDAL:
             if (value < 0x40) sostenutoPedal.release(channel - 1);
             else sostenutoPedal.press(channel - 1);
             break;
-        case 0x43: // Soft Pedal
-            if (value < 0x40) softPedal.release(channel - 1);
-            else softPedal.press(channel - 1);
-            break;
     }
     digitalWrite(STATUS_LED_PIN, LOW);
 }
@@ -186,9 +197,9 @@
 //***********************************************************************
 // Quickly send Note On and Note Off messages to re-init Leds

-void initNotes(uint8_t mlNum) {
+void initNotes(size_t mlIndex) {
     for (uint8_t i=NOTE_MIN; i<=NOTE_MAX; i++) {
-        midiLeds[mlNum].noteOn(i, 0x00);
-        midiLeds[mlNum].noteOff(i);
+        midiLeds[mlIndex].noteOn(i, 0x00);
+        midiLeds[mlIndex].noteOff(i);
     }
 }


add trigger to arm

add trigger to arm

--- testing/arduino/arm_with_trigger.ino
+++ testing/arduino/arm_with_trigger.ino
@@ -7,6 +7,9 @@

 Servo ServoMotor_Top;
 const uint8_t servoPin_Top = 6;
+
+Servo ServoMotor_Trigger;
+const uint8_t servoPin_Trigger = 9;

 const uint8_t servoRotationAngel = 10;

@@ -19,21 +22,17 @@

 void setup()
 {
-    Serial.begin(9600); // Starts the serial communication
-
     pinMode(buttonSW, INPUT_PULLUP);
-
-    pinMode(A5, OUTPUT);
-    pinMode(buttonSW, OUTPUT);

     ServoMotor_Bottom.attach(servoPin_Bottom, 400, 2600);
     ServoMotor_Bottom.write(90);

     ServoMotor_Top.attach(servoPin_Top, 400, 2600);
     ServoMotor_Top.write(90);
+
+    ServoMotor_Trigger.attach(servoPin_Trigger, 400, 2600);
+    ServoMotor_Trigger.write(100);
 }
-
-

 void loop()
 {
@@ -41,10 +40,11 @@

     ServoMotor_Top.write(map(analogRead(buttonY), 0, 1024, 0, 180));

-
-    analogWrite(A5, 255);
     if(digitalRead(buttonSW) == 0)
     {
-        analogWrite(A5, 0);
+        ServoMotor_Trigger.write(135);
+        delay(100);
+        ServoMotor_Trigger.write(100);
     }
+
 }


前回のコードは動作しなかったため，実装方法を変更した その結果gotoが使えなくなったため，gotoによる処理を消した

--- sensorbox.ino
+++ sensorbox.ino
@@ -19,12 +19,12 @@
     Serial.println();
     if (!SPIFFS.begin()) {
         Serial.println("Failed to mount file system");
-        goto restart;
+        return;
     }
     wifi_config config;
     if (loadConfig(config) != 0) {
         Serial.println("Failed to load config file");
-        goto restart;
+        return;
     }
     initializeSensors();

@@ -44,35 +44,55 @@
     server->on("/sensor/help", handleHelp);
     server->onNotFound(handleNotFound);

-    server->on("/sensor/button",     []{ respondValue(getButton); });
-    server->on("/sensor/brightness", []{ respondValue(getBrightness); });
-    server->on("/sensor/pressure",   []{ respondValue(getPressure); });
-    server->on("/sensor/mpltemp",    []{ respondValue(getMPLTemp); });
-    server->on("/sensor/temprature", []{ respondValue(getTemprature); });
-    server->on("/sensor/humidity",   []{ respondValue(getHumidity); });
-    server->on("/sensor/currentpir", []{ respondValue(getCurrentPIR); });
-    server->on("/sensor/pircount",   []{ respondValue(getPIRCount); });
+    server->on("/sensor/all", respondAllValues);
+
+    // 各センサ値用のハンドラを追加する．
+    // ラムダ式内からコード内の通常関数は呼び出せず(not in scope)，
+    // 更にaddHandlerに関しても 'class ESP8266WebServer' has no member
+    // だったため，このような実装にするしかなかった
+    std::function<void(std::function<void(JsonObject&)>)> handler = [](std::function<void(JsonObject&)> getter){
+        String result;
+
+        StaticJsonBuffer<200> jsonBuffer;
+        JsonObject& root = jsonBuffer.createObject();
+        getter(root);
+        root.printTo(result);
+
+        Serial.println(result);
+        server->send(200, "application/json", result);
+    };
+    server->on("/sensor/button",     [handler]{ handler(getButton); });
+    server->on("/sensor/brightness", [handler]{ handler(getBrightness); });
+    server->on("/sensor/pressure",   [handler]{ handler(getPressure); });
+    server->on("/sensor/mpltemp",    [handler]{ handler(getMPLTemp); });
+    server->on("/sensor/temprature", [handler]{ handler(getTemprature); });
+    server->on("/sensor/humidity",   [handler]{ handler(getHumidity); });
+    server->on("/sensor/currentpir", [handler]{ handler(getCurrentPIR); });
+    server->on("/sensor/pircount",   [handler]{ handler(getPIRCount); });

     server->begin();
     Serial.println("HTTP server started !!");
-    return;
-
-restart:
-    Serial.println("setup failed. It's to restart after 5 secs.");
-    delay(5 * 1000);
-    ESP.restart();
 }

 void loop() {
     if (server != NULL) server->handleClient();
 }

-void respondValue(std:function<void(JsonObject&)> getter) {
+void respondAllValues() {
     String result;

-    StaticJsonBuffer<200> jsonBuffer;
-    JsonObject& root = jsonBuffer.createObject();
-    getter(root);
+    StaticJsonBuffer<1000> jsonBuffer;
+    JsonArray& root = jsonBuffer.createArray();
+
+    getButton    (root.createNestedObject());
+    getBrightness(root.createNestedObject());
+    getPressure  (root.createNestedObject());
+    getMPLTemp   (root.createNestedObject());
+    getTemprature(root.createNestedObject());
+    getHumidity  (root.createNestedObject());
+    getCurrentPIR(root.createNestedObject());
+    getPIRCount  (root.createNestedObject());
+
     root.printTo(result);

     Serial.println(result);