New Logic w/ 45 Ult

New Logic w/ 45 Ult

--- firefighter/firefighter.ino
+++ firefighter/firefighter.ino
@@ -1,650 +1,662 @@

-#include "Ultrasonic.h"
-#include "ColorSensor.h"
-#include "Motor.h"
-#include "FreqCount.h"
-#include <Servo.h>
-
-#include "I2Cdev.h"
-#include "MPU6050_6Axis_MotionApps20.h"
-//#include "MPU6050.h" // not necessary if using MotionApps include file
-
-#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
-    #include "Wire.h"
-#endif
-
-#define SOUND_LED 45
-#define FLAME_LED 53
-
-#define WALL_DISTANCE 7
-
-#define flame_sensor 30
-#define servo_pin 31
-#define interval 10
-
-#define LOW_START 3230
-#define HIGH_START 4370
-
-#define LEFT_MOTOR_1A 44
-#define LEFT_MOTOR_1B 3
-
-#define INTERRUPT_PIN 2
-
-// PID
-#define setpoint 0
-#define kP 0.05
-
-Servo extingusher_servo;
-
-Motor leftMotor1(LEFT_MOTOR_1A, LEFT_MOTOR_1B);
-Motor leftMotor2(4, 5);
-Motor rightMotor1(6, 7);
-Motor rightMotor2(8, 9);
-Motor strafeMotor(10, 11);
-Motor fan_motor(12, 13);
-
-ColorSensor colour_sensor(42, 46, 48, 50, 52);
-
-Ultrasonic frontUltrasonic(28, 29, true);
-Ultrasonic leftUltrasonic(26, 27, true);
-Ultrasonic rightUltrasonic(24, 25, true);
-Ultrasonic backUltrasonic(22, 23, true);
+#include "Ultrasonic.h"
+#include "ColorSensor.h"
+#include "Motor.h"
+#include "FreqCount.h"
+#include <Servo.h>
+
+#include "I2Cdev.h"
+#include "MPU6050_6Axis_MotionApps20.h"
+//#include "MPU6050.h" // not necessary if using MotionApps include file
+
+#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
+    #include "Wire.h"
+#endif
+
+#define SOUND_LED 45
+#define FLAME_LED 53
+
+#define WALL_DISTANCE 7
+
+#define flame_sensor 30
+#define servo_pin 31
+#define interval 10
+
+#define LOW_START 3230
+#define HIGH_START 4370
+
+#define LEFT_MOTOR_1A 44
+#define LEFT_MOTOR_1B 3
+
+#define INTERRUPT_PIN 2
+
+// PID
+#define setpoint 0
+#define kP 0.05
+
+Servo extingusher_servo;
+
+Motor leftMotor1(LEFT_MOTOR_1A, LEFT_MOTOR_1B);
+Motor leftMotor2(4, 5);
+Motor rightMotor1(6, 7);
+Motor rightMotor2(8, 9);
+Motor strafeMotor(10, 11);
+Motor fan_motor(12, 13);
+
+ColorSensor colour_sensor(42, 46, 48, 50, 52);
+
+Ultrasonic frontUltrasonic(28, 29, true);
+Ultrasonic leftUltrasonic(26, 27, true);
+Ultrasonic rightUltrasonic(24, 25, true);
+Ultrasonic backUltrasonic(22, 23, true);

 //top ultrasonic array
 Ultrasonic leftArrayInner(34, 35, true);//blue
 Ultrasonic leftArrayOutter(32, 33, true);//orange

 Ultrasonic rightArrayInner(36, 37, true);//orange
-Ultrasonic fightArrayOutter(38, 39, true);//black
-
-MPU6050 mpu; // Default address 0x68
-
-// Drivetrain commands
-void driveToWhite();
-void driveThroughRoom();
-void hDrive(double move, double rotate, double strafe);
-void tankDrive(double leftSpeed, double rightSpeed, double strafeSpeed);
-void definedStartSearch();
-double getPercentError(double actual, double target);
-void turnToAngle(double targetAngle, double speed);
-void naviguessMaze(double swagSpeed);
-void driveToDistance(double targetDistance, double speed);
-
-// Encoder commands
-void countPulseRight();
-int pulseRight;
-const byte encoderARight = 18;
-const byte encoderBRight = 16;
-byte encoderALastRight;
-bool directionRight;
-
-int pulseLeft;
-void countPulseLeft();
-const byte encoderALeft = 19;
-const byte encoderBLeft = 17;
-byte encoderALastLeft;
-bool directionLeft;
-
-double getAngle();
-double getDistanceLeft();
-double getDistanceRight();
-void resetEncoders();
-
-// Sound Sensor
-bool robotOn = false;
-unsigned long count;
-
-// Extinguisher
-void extinguish();
-
-int startSwitch = 14;
-
-// Gyro
-bool dmpReady = false;  // set true if DMP init was successful
-uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
-uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
-uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
-uint16_t fifoCount;     // count of all bytes currently in FIFO
-uint8_t fifoBuffer[64]; // FIFO storage buffer
-
-// orientation/motion vars
-Quaternion q;           // [w, x, y, z]         quaternion container
-float euler[3];         // [psi, theta, phi]    Euler angle container
-
-double gyroResetValX;
-double gyroResetValY;
-double gyroResetValZ;
-void resetGyro(char axis);
-double getAngle(char axis);
-
-int ri;
-
-bool pidEnabled = false;
-
-volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
-void dmpDataReady() {
-    mpuInterrupt = true;
-}
-
-void setup() {
-  // join I2C bus (I2Cdev library doesn't do this automatically)
-  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
-      Wire.begin();
-      Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
-  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
-      Fastwire::setup(400, true);
-  #endif
-
-  Serial.begin(9600);
-
-  // Start Switch
-  pinMode(startSwitch, INPUT);
-
-  // Status Lights
-  pinMode(SOUND_LED, OUTPUT);
-  pinMode(FLAME_LED, OUTPUT);
-
-  // Right Encoder
-  directionRight = true;//default -> Forward
-  pinMode(encoderBRight,INPUT);
-  attachInterrupt(5, countPulseRight, CHANGE);
-
-  // Left Encoder
-  directionLeft = true;//default -> Forward
-  pinMode(encoderBLeft,INPUT);
-  attachInterrupt(4, countPulseLeft, CHANGE);
-
-  // Extinguisher
-  pinMode(flame_sensor, INPUT);
-
-  ri = -1;
-
-  extingusher_servo.attach(servo_pin);
-  extingusher_servo.write(90);
-
-  mpu.initialize();
-  pinMode(INTERRUPT_PIN, INPUT);
-
-  devStatus = mpu.dmpInitialize();
-
-  // accel offsets
-  mpu.setXAccelOffset(-4265);
-  mpu.setYAccelOffset(-283);
-  mpu.setZAccelOffset(1675); // 1688 factory default for my test chip
-  // supply your own gyro offsets here, scaled for min sensitivity
-  mpu.setXGyroOffset(-51);
-  mpu.setYGyroOffset(40);
-  mpu.setZGyroOffset(27);
-
-  // make sure it worked (returns 0 if so)
-  if (devStatus == 0) {
-      // turn on the DMP, now that it's ready
-      mpu.setDMPEnabled(true);
-
-      // enable Arduino interrupt detection
-      attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
-      mpuIntStatus = mpu.getIntStatus();
-
-      // set our DMP Ready flag so the main loop() function knows it's okay to use it
-      Serial.println(F("DMP ready! Waiting for first interrupt..."));
-      dmpReady = true;
-
-      // get expected DMP packet size for later comparison
-      packetSize = mpu.dmpGetFIFOPacketSize();
+Ultrasonic rightArrayOutter(38, 39, true);//black
+
+MPU6050 mpu; // Default address 0x68
+
+// Drivetrain commands
+void driveToWhite();
+void driveThroughRoom();
+void hDrive(double move, double rotate, double strafe);
+void tankDrive(double leftSpeed, double rightSpeed, double strafeSpeed);
+void definedStartSearch();
+double getPercentError(double actual, double target);
+void turnToAngle(double targetAngle, double speed);
+void naviguessMaze(double swagSpeed);
+void driveToDistance(double targetDistance, double speed);
+
+// Encoder commands
+void countPulseRight();
+int pulseRight;
+const byte encoderARight = 18;
+const byte encoderBRight = 16;
+byte encoderALastRight;
+bool directionRight;
+
+int pulseLeft;
+void countPulseLeft();
+const byte encoderALeft = 19;
+const byte encoderBLeft = 17;
+byte encoderALastLeft;
+bool directionLeft;
+
+double getAngle();
+double getDistanceLeft();
+double getDistanceRight();
+void resetEncoders();
+
+// Sound Sensor
+bool robotOn = false;
+unsigned long count;
+
+// Extinguisher
+void extinguish();
+
+int startSwitch = 14;
+
+// Gyro
+bool dmpReady = false;  // set true if DMP init was successful
+uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
+uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
+uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
+uint16_t fifoCount;     // count of all bytes currently in FIFO
+uint8_t fifoBuffer[64]; // FIFO storage buffer
+
+// orientation/motion vars
+Quaternion q;           // [w, x, y, z]         quaternion container
+float euler[3];         // [psi, theta, phi]    Euler angle container
+
+double gyroResetValX;
+double gyroResetValY;
+double gyroResetValZ;
+void resetGyro(char axis);
+double getAngle(char axis);
+
+int ri;
+
+bool pidEnabled = false;
+
+volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
+void dmpDataReady() {
+    mpuInterrupt = true;
+}
+
+void setup() {
+  // join I2C bus (I2Cdev library doesn't do this automatically)
+  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
+      Wire.begin();
+      Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
+  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
+      Fastwire::setup(400, true);
+  #endif
+
+  Serial.begin(9600);
+
+  // Start Switch
+  pinMode(startSwitch, INPUT);
+
+  // Status Lights
+  pinMode(SOUND_LED, OUTPUT);
+  pinMode(FLAME_LED, OUTPUT);
+
+  // Right Encoder
+  directionRight = true;//default -> Forward
+  pinMode(encoderBRight,INPUT);
+  attachInterrupt(5, countPulseRight, CHANGE);
+
+  // Left Encoder
+  directionLeft = true;//default -> Forward
+  pinMode(encoderBLeft,INPUT);
+  attachInterrupt(4, countPulseLeft, CHANGE);
+
+  // Extinguisher
+  pinMode(flame_sensor, INPUT);
+
+  ri = -1;
+
+  extingusher_servo.attach(servo_pin);
+  extingusher_servo.write(90);
+
+  mpu.initialize();
+  pinMode(INTERRUPT_PIN, INPUT);
+
+  devStatus = mpu.dmpInitialize();
+
+  // accel offsets
+  mpu.setXAccelOffset(-4265);
+  mpu.setYAccelOffset(-283);
+  mpu.setZAccelOffset(1675); // 1688 factory default for my test chip
+  // supply your own gyro offsets here, scaled for min sensitivity
+  mpu.setXGyroOffset(-51);
+  mpu.setYGyroOffset(40);
+  mpu.setZGyroOffset(27);
+
+  // make sure it worked (returns 0 if so)
+  if (devStatus == 0) {
+      // turn on the DMP, now that it's ready
+      mpu.setDMPEnabled(true);
+
+      // enable Arduino interrupt detection
+      attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
+      mpuIntStatus = mpu.getIntStatus();
+
+      // set our DMP Ready flag so the main loop() function knows it's okay to use it
+      Serial.println(F("DMP ready! Waiting for first interrupt..."));
+      dmpReady = true;
+
+      // get expected DMP packet size for later comparison
+      packetSize = mpu.dmpGetFIFOPacketSize();
+  }
+
+  resetGyro('z');
+
+  FreqCount.begin(1000);
+}
+
+bool firstStart = false;
+bool foundFlame = false;
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  if (FreqCount.available()) {
+    count = FreqCount.read();
+	  //Serial.println(count);
+  } else {
+    count = 0;
+  }
+//  Serial.println(getAngle('z'));
+  if ((count > 3230 && count < 4370) || digitalRead(startSwitch)) {
+    digitalWrite(SOUND_LED, HIGH);
+    firstStart = true;
+    robotOn = !robotOn;
+    resetGyro('z');
+  }
+
+//  Serial.println(getAngle('z'));
+  if(robotOn){
+    if (firstStart) {
+      FreqCount.end();
+      ri = colour_sensor.getColor('r');
+    }
+
+  	while(!foundFlame && digitalRead(flame_sensor)) {
+  		// naviguessMaze(0.3);
+  		// followWall(1.0);
+       ultrasonicCheck();
+      // drivetrainCheck();
+
+//      if (!pidEnabled) {
+//        pidEnabled = true;
+//        resetGyro('z');
+//      }
+//      hDrive(0.5, getPidOutput(), 0.0);   // Correctional driving
+//      Serial.println(getPidOutput());
+
+  	}
+
+  	foundFlame = true;
+  	digitalWrite(FLAME_LED, foundFlame);
+
+  	fan_motor.set(1.0);
+
+  	hDrive(0.0, 0.0, 0.0);
+  	delay(100);
+
+  	while (frontUltrasonic.getDistance() > 0.5) {
+  		hDrive(0.3, 0.0, 0.0);
+  		extingusher_servo.write(110);
+  		delay(600);
+  		extingusher_servo.write(70);
+  		delay(600);
+  		hDrive(0.0, 0.0, 0.0);
+  		delay(200);
+  		digitalWrite(FLAME_LED, !digitalRead(flame_sensor));
+  	}
+
+	  hDrive(0.0, 0.0, 0.0);
+  }
+  else{
+    digitalWrite(SOUND_LED, LOW);
+    firstStart = false;
+    hDrive(0.0, 0.0, 0.0);
+  }
+
+//  delay(100);
+}
+
+void hDrive(double move, double rotate, double strafe) {
+  double leftSpeed, rightSpeed;
+
+  move = constrain(move, -1.0, 1.0);
+  rotate = constrain(rotate, -1.0, 1.0);
+  strafe = constrain(strafe, -1.0, 1.0);
+
+  if (move > 0.0) {
+    if (rotate > 0.0) {
+      leftSpeed =  move - rotate;
+      rightSpeed = max(move, rotate);
+    } else {
+      leftSpeed = max(move, -rotate);
+      rightSpeed = move + rotate;
+    }
+  } else {
+    if (rotate > 0.0) {
+      leftSpeed = -max(-move, rotate);
+      rightSpeed = move + rotate;
+    } else {
+      leftSpeed = move - rotate;
+      rightSpeed = -max(-move, -rotate);
+    }
+  }
+
+  leftMotor1.set(constrain(leftSpeed, -1.0, 1.0));
+  leftMotor2.set(constrain(leftSpeed, -1.0, 1.0));
+  rightMotor1.set(-constrain(rightSpeed, -1.0, 1.0));
+  rightMotor2.set(-constrain(rightSpeed, -1.0, 1.0));
+  strafeMotor.set(strafe);
+}
+
+void drivetrainCheck() {
+  hDrive(0.3, 0.0, 0.0); delay(300); hDrive(-0.3, 0.0, 0.0); delay(300);
+  hDrive(0.0, 0.3, 0.0); delay(300); hDrive(0.0, -0.3, 0.0); delay(300);
+  hDrive(0.0, 0.0, 0.3); delay(300); hDrive(0.0, 0.0, -0.3); delay(300);
+}
+
+void ultrasonicCheck() {
+	Serial.println("====================================");
+	Serial.print("F: ");
+	Serial.println(frontUltrasonic.getDistance());
+	Serial.print("L: ");
+	Serial.println(leftUltrasonic.getDistance());
+	Serial.print("R: ");
+	Serial.println(rightUltrasonic.getDistance());
+	Serial.print("B: ");
+	Serial.println(backUltrasonic.getDistance());
+	delay(300);
+}
+
+void driveToWhite() {
+  int ri = colour_sensor.getColor('r');
+  int r = ri;
+  hDrive(.3,0,0);
+
+  while(r > ri - 70) {
+    r = colour_sensor.getColor('r');
+  }
+
+  hDrive(0.0, 0.0, 0.0);
+}
+
+void driveThroughRoom() {
+  if (frontUltrasonic.getDistance() < WALL_DISTANCE) {
+    if (leftUltrasonic.getDistance() < WALL_DISTANCE) {
+      hDrive(0.0, 0.0, 1.0);
+    } else {
+      hDrive(0.0, 0.0, -1.0);
+    }
+  } else {
+    if (leftUltrasonic.getDistance() < WALL_DISTANCE) {
+      hDrive(0.0, 0.0, 1.0);
+    } else {
+      hDrive(0.0, 0.0, -1.0);
+    }
+  }
+}
+
+void definedStartSearch() {
+  // Start A
+  if (rightUltrasonic.getDistance() < WALL_DISTANCE) {
+    hDrive(0.0, 0.0, -1.0);
+    delay(500);
+    while (frontUltrasonic.getDistance() < WALL_DISTANCE) { }
+    hDrive(0.0, 0.0, 0.0);
+  // Start B
+  } else {
+    hDrive(1.0, 0.0, 0.0);
+    while (frontUltrasonic.getDistance() > WALL_DISTANCE) { }
+    hDrive(0.0, 0.0, 0.0);
+  }
+}
+
+void countPulseRight() {
+  int Lstate = digitalRead(encoderARight);
+  if((encoderALastRight == LOW) && Lstate==HIGH)
+  {
+    int val = digitalRead(encoderBRight);
+    if(val == LOW && directionRight)
+    {
+      directionRight = false; //Reverse
+    } else if(val == HIGH && !directionRight) {
+      directionRight = true;
+    }
+  }
+  encoderALastRight = Lstate;
+
+  if(!directionRight)  pulseRight--;
+  else  pulseRight++;
+}
+
+void countPulseLeft() {
+  int Lstate = digitalRead(encoderALeft);
+  if((encoderALastLeft == LOW) && Lstate==HIGH)
+  {
+    int val = digitalRead(encoderBLeft);
+    if(val == LOW && directionLeft)
+    {
+      directionLeft = false; //Reverse
+    } else if(val == HIGH && !directionLeft) {
+      directionLeft = true;
+    }
+  }
+  encoderALastLeft = Lstate;
+
+  if(!directionLeft)  pulseLeft++;
+  else  pulseLeft--;
+}
+
+double getAngle() {
+  return (180 / PI) * (getDistanceLeft() - getDistanceRight()) / 11.5;
+}
+
+double getDistanceLeft() {
+	return (pulseLeft * 12.5) / 1356;
+}
+
+double getDistanceRight() {
+	return (pulseRight * 12.5) / 1356;
+}
+
+void resetEncoders() {
+	pulseLeft = pulseRight = 0;
+}
+
+double getPercentError(double actual, double target) {
+  return abs(actual - target) / target;
+}
+
+void turnToAngle(double targetAngle, double speed) {
+  resetGyro('z');
+
+  hDrive(0.0, speed, 0.0);
+
+  while (abs(getAngle('z')) < abs(targetAngle)) {
+    // Serial.println(getAngle('z'));
+  }
+
+  hDrive(0.0, 0.0, 0.0);
+}
+
+void driveToDistance(double targetDistance, double speed) {
+  resetEncoders();
+
+  hDrive(speed, 0.0, 0.0);
+
+  while (abs(getDistanceLeft()) < abs(targetDistance) && abs(getDistanceRight()) < abs(targetDistance) ) {
+    // Serial.println(getDistanceLeft());
+  }
+
+  hDrive(0.0, 0.0, 0.0);
+}
+
+double getPidOutput() {
+  return (setpoint - getAngle('x')) * kP;
+}
+
+void extinguish() {
+  for(int i = 40; i <= 160; i += interval){
+    extingusher_servo.write(i);
+    delay(15*interval);//give time for servo to move
+
+    if(!digitalRead(flame_sensor))
+      digitalWrite(FLAME_LED, HIGH);
+
+    //turn on fan
+    fan_motor.set(1);
+    while(!digitalRead(flame_sensor)){
+      //while there is a fire
+      delay(1000);
+    }
+    //turn off fan
+    fan_motor.set(0);
+    digitalWrite(FLAME_LED, LOW);
+  }
+
+  for(int i = 140; i >=20; i -= interval){
+    extingusher_servo.write(i);
+    delay(15*interval);//give time for servo to move
+
+    if(!digitalRead(flame_sensor))
+      digitalWrite(FLAME_LED, HIGH);
+
+    //turn on fan
+    fan_motor.set(1);
+    while(!digitalRead(flame_sensor)){
+      //while there is a fire
+      delay(1000);
+    }
+    //turn off fan
+    fan_motor.set(0);
+    digitalWrite(FLAME_LED, LOW);
+  }
+}
+
+bool direction = true;
+int leftTurns = 0;
+int rightTurns = 0;
+
+void naviguessMaze(double swagSpeed) {
+	bool frontTriggered = frontUltrasonic.getDistance() < 2;
+	bool leftTriggered = leftUltrasonic.getDistance() < 2;
+	bool rightTriggered = rightUltrasonic.getDistance() < 2;
+	bool backTriggered = backUltrasonic.getDistance() < 2;
+  bool rightFavor = rightUltrasonic.getDistance() <7 && rightUltrasonic.getDistance() > 2 && frontUltrasonic.getDistance() < 20;
+  bool rightInner = rightArrayInner.getDistance() < 5;
+  bool rightOuter = rightArrayOutter.getDistance() < 5;
+  bool leftInner = leftArrayInner.getDistance() < 5;
+  bool leftOuter = leftArrayOutter.getDistance() < 5;
+
+	if (leftTriggered && frontTriggered) {
+		hDrive(-swagSpeed, 0.0, 0.0);
+		delay(100);
+		// hDrive(0.0, swagSpeed , 0.0);
+
+		if (rightTurns == 4) {
+			turnToAngle(-90, swagSpeed);
+			rightTurns = 0;
+		} else {
+			turnToAngle(90, -swagSpeed);
+			rightTurns++;
+		}
+	} else if (rightTriggered && frontTriggered) {
+		hDrive(-swagSpeed, 0.0, 0.0);
+		delay(100);
+		// hDrive(0.0, -swagSpeed , 0.0);
+
+		if (leftTurns == 4) {
+			turnToAngle(-90, -swagSpeed);
+			leftTurns = 0;
+		} else {
+			turnToAngle(-90, swagSpeed);
+			leftTurns++;
+		}
+	} else if (leftTriggered && !frontTriggered) {
+		// hDrive(swagSpeed - 0.2, -swagSpeed, 0.0);
+		turnToAngle(2, -swagSpeed);
+	} else if (rightTriggered && !frontTriggered) {
+		//hDrive(swagSpeed - 0.2, swagSpeed , 0.0);
+		turnToAngle(-2, swagSpeed);
+	} else if (frontTriggered && !rightTriggered & !leftTriggered){
+		hDrive(-swagSpeed, 0.0, 0.0);
+		delay(50);
+		if (rightTurns == 4) {
+			turnToAngle(-90, swagSpeed);
+			rightTurns = 0;
+		} else {
+			turnToAngle(90, -swagSpeed);
+			rightTurns++;
+		}
+		// turnToAngle(90, -swagSpeed);
+		// rightTurns++;
+	} else if (backTriggered) {
+		hDrive(swagSpeed, swagSpeed, 0.0);
+	} else if(rightFavor) {
+    turnToAngle(-2, swagSpeed);
+  } else if(rightInner || rightOuter && ! frontTriggered) {
+    hDrive(-swagSpeed, 0.0, 0.0);
+    delay(100);
+    turnToAngle(-15, swagSpeed);
+  } else if(leftInner || leftOuter && !frontTriggered) {
+    hDrive(-swagSpeed, 0.0, 0.0);
+    delay(100);
+    turnToAngle(15, -swagSpeed);
+  } else {
+		hDrive(swagSpeed, 0.0, 0.0);
+	}
+
+	if (direction) {
+		strafeMotor.set(0.3);
+	} else {
+		strafeMotor.set(-0.3);
+	}
+
+	direction = !direction;
+
+	delay(50);
+}
+
+void tankDrive(double leftSpeed, double rightSpeed, double strafeSpeed) {
+	leftMotor1.set(constrain(leftSpeed, -1.0, 1.0));
+	leftMotor2.set(constrain(leftSpeed, -1.0, 1.0));
+	rightMotor1.set(-constrain(rightSpeed, -1.0, 1.0));
+	rightMotor2.set(-constrain(rightSpeed, -1.0, 1.0));
+	strafeMotor.set(strafeSpeed);
+}
+
+void followWall(double targetDistance) {
+	if (rightUltrasonic.getDistance() < targetDistance) {
+		tankDrive(0.0, 0.3, 0.0);
+	} else if (rightUltrasonic.getDistance() > targetDistance) {
+		tankDrive(0.3, 0.0, 0.0);
+	} else {
+		tankDrive(0.3, 0.4, 0.0);
+	}
+}
+
+double getAngle(char axis) {
+  // if programming failed, don't try to do anything
+    if (!dmpReady) return;
+
+    // wait for MPU interrupt or extra packet(s) available
+    while (!mpuInterrupt && fifoCount < packetSize) {
+        // other program behavior stuff here
+        // .
+        // .
+        // .
+        // if you are really paranoid you can frequently test in between other
+        // stuff to see if mpuInterrupt is true, and if so, "break;" from the
+        // while() loop to immediately process the MPU data
+        // .
+        // .
+        // .
+    }
+
+    // reset interrupt flag and get INT_STATUS byte
+    mpuInterrupt = false;
+    mpuIntStatus = mpu.getIntStatus();
+
+    // get current FIFO count
+    fifoCount = mpu.getFIFOCount();
+
+    // check for overflow (this should never happen unless our code is too inefficient)
+    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
+        // reset so we can continue cleanly
+        mpu.resetFIFO();
+        Serial.println(F("FIFO overflow!"));
+
+    // otherwise, check for DMP data ready interrupt (this should happen frequently)
+    } else if (mpuIntStatus & 0x02) {
+        // wait for correct available data length, should be a VERY short wait
+        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
+
+        // read a packet from FIFO
+        mpu.getFIFOBytes(fifoBuffer, packetSize);
+
+        // track FIFO count here in case there is > 1 packet available
+        // (this lets us immediately read more without waiting for an interrupt)
+        fifoCount -= packetSize;
+
+        // display Euler angles in degrees
+        mpu.dmpGetQuaternion(&q, fifoBuffer);
+        mpu.dmpGetEuler(euler, &q);
+
+        switch (axis) {
+          case 'x':
+            return (euler[2] * 180/M_PI) - gyroResetValX;
+            break;
+          case 'y':
+            return (euler[1] * 180/M_PI) - gyroResetValY;
+            break;
+          case 'z':
+            return (euler[0] * 180/M_PI) - gyroResetValZ;
+            break;
+        }
+    }
+}
+
+void resetGyro(char axis) {
+  switch (axis) {
+    case 'x':
+      gyroResetValX += getAngle('x');
+      break;
+    case 'y':
+      gyroResetValY += getAngle('y');
+      break;
+    case 'z':
+      gyroResetValZ += getAngle('z');
+      break;
   }
-
-  resetGyro('z');
-
-  FreqCount.begin(1000);
-}
-
-bool firstStart = false;
-bool foundFlame = false;
-
-void loop() {
-  // put your main code here, to run repeatedly:
-  if (FreqCount.available()) {
-    count = FreqCount.read();
-	  //Serial.println(count);
-  } else {
-    count = 0;
-  }
-//  Serial.println(getAngle('z'));
-  if ((count > 3230 && count < 4370) || digitalRead(startSwitch)) {
-    digitalWrite(SOUND_LED, HIGH);
-    firstStart = true;
-    robotOn = !robotOn;
-    resetGyro('z');
-  }
-
-//  Serial.println(getAngle('z'));
-  if(robotOn){
-    if (firstStart) {
-      FreqCount.end();
-      ri = colour_sensor.getColor('r');
-    }
-
-  	while(!foundFlame && digitalRead(flame_sensor)) {
-  		// naviguessMaze(0.3);
-  		// followWall(1.0);
-       ultrasonicCheck();
-      // drivetrainCheck();
-
-//      if (!pidEnabled) {
-//        pidEnabled = true;
-//        resetGyro('z');
-//      }
-//      hDrive(0.5, getPidOutput(), 0.0);   // Correctional driving
-//      Serial.println(getPidOutput());
-
-  	}
-
-  	foundFlame = true;
-  	digitalWrite(FLAME_LED, foundFlame);
-
-  	fan_motor.set(1.0);
-
-  	hDrive(0.0, 0.0, 0.0);
-  	delay(100);
-
-  	while (frontUltrasonic.getDistance() > 0.5) {
-  		hDrive(0.3, 0.0, 0.0);
-  		extingusher_servo.write(110);
-  		delay(600);
-  		extingusher_servo.write(70);
-  		delay(600);
-  		hDrive(0.0, 0.0, 0.0);
-  		delay(200);
-  		digitalWrite(FLAME_LED, !digitalRead(flame_sensor));
-  	}
-
-	  hDrive(0.0, 0.0, 0.0);
-  }
-  else{
-    digitalWrite(SOUND_LED, LOW);
-    firstStart = false;
-    hDrive(0.0, 0.0, 0.0);
-  }
-
-//  delay(100);
-}
-
-void hDrive(double move, double rotate, double strafe) {
-  double leftSpeed, rightSpeed;
-
-  move = constrain(move, -1.0, 1.0);
-  rotate = constrain(rotate, -1.0, 1.0);
-  strafe = constrain(strafe, -1.0, 1.0);
-
-  if (move > 0.0) {
-    if (rotate > 0.0) {
-      leftSpeed =  move - rotate;
-      rightSpeed = max(move, rotate);
-    } else {
-      leftSpeed = max(move, -rotate);
-      rightSpeed = move + rotate;
-    }
-  } else {
-    if (rotate > 0.0) {
-      leftSpeed = -max(-move, rotate);
-      rightSpeed = move + rotate;
-    } else {
-      leftSpeed = move - rotate;
-      rightSpeed = -max(-move, -rotate);
-    }
-  }
-
-  leftMotor1.set(constrain(leftSpeed, -1.0, 1.0));
-  leftMotor2.set(constrain(leftSpeed, -1.0, 1.0));
-  rightMotor1.set(-constrain(rightSpeed, -1.0, 1.0));
-  rightMotor2.set(-constrain(rightSpeed, -1.0, 1.0));
-  strafeMotor.set(strafe);
-}
-
-void drivetrainCheck() {
-  hDrive(0.3, 0.0, 0.0); delay(300); hDrive(-0.3, 0.0, 0.0); delay(300);
-  hDrive(0.0, 0.3, 0.0); delay(300); hDrive(0.0, -0.3, 0.0); delay(300);
-  hDrive(0.0, 0.0, 0.3); delay(300); hDrive(0.0, 0.0, -0.3); delay(300);
-}
-
-void ultrasonicCheck() {
-	Serial.println("====================================");
-	Serial.print("F: ");
-	Serial.println(frontUltrasonic.getDistance());
-	Serial.print("L: ");
-	Serial.println(leftUltrasonic.getDistance());
-	Serial.print("R: ");
-	Serial.println(rightUltrasonic.getDistance());
-	Serial.print("B: ");
-	Serial.println(backUltrasonic.getDistance());
-	delay(300);
-}
-
-void driveToWhite() {
-  int ri = colour_sensor.getColor('r');
-  int r = ri;
-  hDrive(.3,0,0);
-
-  while(r > ri - 70) {
-    r = colour_sensor.getColor('r');
-  }
-
-  hDrive(0.0, 0.0, 0.0);
-}
-
-void driveThroughRoom() {
-  if (frontUltrasonic.getDistance() < WALL_DISTANCE) {
-    if (leftUltrasonic.getDistance() < WALL_DISTANCE) {
-      hDrive(0.0, 0.0, 1.0);
-    } else {
-      hDrive(0.0, 0.0, -1.0);
-    }
-  } else {
-    if (leftUltrasonic.getDistance() < WALL_DISTANCE) {
-      hDrive(0.0, 0.0, 1.0);
-    } else {
-      hDrive(0.0, 0.0, -1.0);
-    }
-  }
-}
-
-void definedStartSearch() {
-  // Start A
-  if (rightUltrasonic.getDistance() < WALL_DISTANCE) {
-    hDrive(0.0, 0.0, -1.0);
-    delay(500);
-    while (frontUltrasonic.getDistance() < WALL_DISTANCE) { }
-    hDrive(0.0, 0.0, 0.0);
-  // Start B
-  } else {
-    hDrive(1.0, 0.0, 0.0);
-    while (frontUltrasonic.getDistance() > WALL_DISTANCE) { }
-    hDrive(0.0, 0.0, 0.0);
-  }
-}
-
-void countPulseRight() {
-  int Lstate = digitalRead(encoderARight);
-  if((encoderALastRight == LOW) && Lstate==HIGH)
-  {
-    int val = digitalRead(encoderBRight);
-    if(val == LOW && directionRight)
-    {
-      directionRight = false; //Reverse
-    } else if(val == HIGH && !directionRight) {
-      directionRight = true;
-    }
-  }
-  encoderALastRight = Lstate;
-
-  if(!directionRight)  pulseRight--;
-  else  pulseRight++;
-}
-
-void countPulseLeft() {
-  int Lstate = digitalRead(encoderALeft);
-  if((encoderALastLeft == LOW) && Lstate==HIGH)
-  {
-    int val = digitalRead(encoderBLeft);
-    if(val == LOW && directionLeft)
-    {
-      directionLeft = false; //Reverse
-    } else if(val == HIGH && !directionLeft) {
-      directionLeft = true;
-    }
-  }
-  encoderALastLeft = Lstate;
-
-  if(!directionLeft)  pulseLeft++;
-  else  pulseLeft--;
-}
-
-double getAngle() {
-  return (180 / PI) * (getDistanceLeft() - getDistanceRight()) / 11.5;
-}
-
-double getDistanceLeft() {
-	return (pulseLeft * 12.5) / 1356;
-}
-
-double getDistanceRight() {
-	return (pulseRight * 12.5) / 1356;
-}
-
-void resetEncoders() {
-	pulseLeft = pulseRight = 0;
-}
-
-double getPercentError(double actual, double target) {
-  return abs(actual - target) / target;
-}
-
-void turnToAngle(double targetAngle, double speed) {
-  resetGyro('z');
-
-  hDrive(0.0, speed, 0.0);
-
-  while (abs(getAngle('z')) < abs(targetAngle)) {
-    // Serial.println(getAngle('z'));
-  }
-
-  hDrive(0.0, 0.0, 0.0);
-}
-
-void driveToDistance(double targetDistance, double speed) {
-  resetEncoders();
-
-  hDrive(speed, 0.0, 0.0);
-
-  while (abs(getDistanceLeft()) < abs(targetDistance) && abs(getDistanceRight()) < abs(targetDistance) ) {
-    // Serial.println(getDistanceLeft());
-  }
-
-  hDrive(0.0, 0.0, 0.0);
-}
-
-double getPidOutput() {
-  return (setpoint - getAngle('x')) * kP;
-}
-
-void extinguish() {
-  for(int i = 40; i <= 160; i += interval){
-    extingusher_servo.write(i);
-    delay(15*interval);//give time for servo to move
-
-    if(!digitalRead(flame_sensor))
-      digitalWrite(FLAME_LED, HIGH);
-
-    //turn on fan
-    fan_motor.set(1);
-    while(!digitalRead(flame_sensor)){
-      //while there is a fire
-      delay(1000);
-    }
-    //turn off fan
-    fan_motor.set(0);
-    digitalWrite(FLAME_LED, LOW);
-  }
-
-  for(int i = 140; i >=20; i -= interval){
-    extingusher_servo.write(i);
-    delay(15*interval);//give time for servo to move
-
-    if(!digitalRead(flame_sensor))
-      digitalWrite(FLAME_LED, HIGH);
-
-    //turn on fan
-    fan_motor.set(1);
-    while(!digitalRead(flame_sensor)){
-      //while there is a fire
-      delay(1000);
-    }
-    //turn off fan
-    fan_motor.set(0);
-    digitalWrite(FLAME_LED, LOW);
-  }
-}
-
-bool direction = true;
-int leftTurns = 0;
-int rightTurns = 0;
-
-void naviguessMaze(double swagSpeed) {
-	bool frontTriggered = frontUltrasonic.getDistance() < 2;
-	bool leftTriggered = leftUltrasonic.getDistance() < 2;
-	bool rightTriggered = rightUltrasonic.getDistance() < 2;
-	bool backTriggered = backUltrasonic.getDistance() < 2;
-  bool rightFavor = rightUltrasonic.getDistance() <7 && rightUltrasonic.getDistance() > 2 && frontUltrasonic.getDistance() < 20;
-
-	if (leftTriggered && frontTriggered) {
-		hDrive(-swagSpeed, 0.0, 0.0);
-		delay(100);
-		// hDrive(0.0, swagSpeed , 0.0);
-
-		if (rightTurns == 4) {
-			turnToAngle(-90, swagSpeed);
-			rightTurns = 0;
-		} else {
-			turnToAngle(90, -swagSpeed);
-			rightTurns++;
-		}
-	} else if (rightTriggered && frontTriggered) {
-		hDrive(-swagSpeed, 0.0, 0.0);
-		delay(100);
-		// hDrive(0.0, -swagSpeed , 0.0);
-
-		if (leftTurns == 4) {
-			turnToAngle(-90, -swagSpeed);
-			leftTurns = 0;
-		} else {
-			turnToAngle(-90, swagSpeed);
-			leftTurns++;
-		}
-	} else if (leftTriggered && !frontTriggered) {
-		// hDrive(swagSpeed - 0.2, -swagSpeed, 0.0);
-		turnToAngle(2, -swagSpeed);
-	} else if (rightTriggered && !frontTriggered) {
-		//hDrive(swagSpeed - 0.2, swagSpeed , 0.0);
-		turnToAngle(-2, swagSpeed);
-	} else if (frontTriggered && !rightTriggered & !leftTriggered){
-		hDrive(-swagSpeed, 0.0, 0.0);
-		delay(50);
-		if (rightTurns == 4) {
-			turnToAngle(-90, swagSpeed);
-			rightTurns = 0;
-		} else {
-			turnToAngle(90, -swagSpeed);
-			rightTurns++;
-		}
-		// turnToAngle(90, -swagSpeed);
-		// rightTurns++;
-	} else if (backTriggered) {
-		hDrive(swagSpeed, swagSpeed, 0.0);
-	} else if(rightFavor) {
-    turnToAngle(-2, swagSpeed);
-	} else {
-		hDrive(swagSpeed, 0.0, 0.0);
-	}
-
-	if (direction) {
-		strafeMotor.set(0.3);
-	} else {
-		strafeMotor.set(-0.3);
-	}
-
-	direction = !direction;
-
-	delay(50);
-}
-
-void tankDrive(double leftSpeed, double rightSpeed, double strafeSpeed) {
-	leftMotor1.set(constrain(leftSpeed, -1.0, 1.0));
-	leftMotor2.set(constrain(leftSpeed, -1.0, 1.0));
-	rightMotor1.set(-constrain(rightSpeed, -1.0, 1.0));
-	rightMotor2.set(-constrain(rightSpeed, -1.0, 1.0));
-	strafeMotor.set(strafeSpeed);
-}
-
-void followWall(double targetDistance) {
-	if (rightUltrasonic.getDistance() < targetDistance) {
-		tankDrive(0.0, 0.3, 0.0);
-	} else if (rightUltrasonic.getDistance() > targetDistance) {
-		tankDrive(0.3, 0.0, 0.0);
-	} else {
-		tankDrive(0.3, 0.4, 0.0);
-	}
-}
-
-double getAngle(char axis) {
-  // if programming failed, don't try to do anything
-    if (!dmpReady) return;
-
-    // wait for MPU interrupt or extra packet(s) available
-    while (!mpuInterrupt && fifoCount < packetSize) {
-        // other program behavior stuff here
-        // .
-        // .
-        // .
-        // if you are really paranoid you can frequently test in between other
-        // stuff to see if mpuInterrupt is true, and if so, "break;" from the
-        // while() loop to immediately process the MPU data
-        // .
-        // .
-        // .
-    }
-
-    // reset interrupt flag and get INT_STATUS byte
-    mpuInterrupt = false;
-    mpuIntStatus = mpu.getIntStatus();
-
-    // get current FIFO count
-    fifoCount = mpu.getFIFOCount();
-
-    // check for overflow (this should never happen unless our code is too inefficient)
-    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
-        // reset so we can continue cleanly
-        mpu.resetFIFO();
-        Serial.println(F("FIFO overflow!"));
-
-    // otherwise, check for DMP data ready interrupt (this should happen frequently)
-    } else if (mpuIntStatus & 0x02) {
-        // wait for correct available data length, should be a VERY short wait
-        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
-
-        // read a packet from FIFO
-        mpu.getFIFOBytes(fifoBuffer, packetSize);
-
-        // track FIFO count here in case there is > 1 packet available
-        // (this lets us immediately read more without waiting for an interrupt)
-        fifoCount -= packetSize;
-
-        // display Euler angles in degrees
-        mpu.dmpGetQuaternion(&q, fifoBuffer);
-        mpu.dmpGetEuler(euler, &q);
-
-        switch (axis) {
-          case 'x':
-            return (euler[2] * 180/M_PI) - gyroResetValX;
-            break;
-          case 'y':
-            return (euler[1] * 180/M_PI) - gyroResetValY;
-            break;
-          case 'z':
-            return (euler[0] * 180/M_PI) - gyroResetValZ;
-            break;
-        }
-    }
-}
-
-void resetGyro(char axis) {
-  switch (axis) {
-    case 'x':
-      gyroResetValX += getAngle('x');
-      break;
-    case 'y':
-      gyroResetValY += getAngle('y');
-      break;
-    case 'z':
-      gyroResetValZ += getAngle('z');
-      break;
-  }


Got basic node working!

Got basic node working!

--- firmware/node/node.ino
+++ firmware/node/node.ino
@@ -1,14 +1,9 @@
-#include <SPI85.h>
-#include <Mirf.h>
-#include <nRF24L01.h>
-#include <MirfHardwareSpi85Driver.h>
+#include <util/crc16.h>
 #include <Adafruit_NeoPixel.h>

 const uint8_t NUM_PIXELS = 8;
-const uint8_t OUT_PIN = 1;
-const uint64_t ADDRESS = 0x45;
-byte addr[mirf_ADDR_LEN] = { ADDRESS, 0, 0, 0, 0 };
-
+const uint8_t OUT_PIN = 13;
+const uint8_t MAX_PACKET_LEN = 32;

 Adafruit_NeoPixel pixels = Adafruit_NeoPixel(NUM_PIXELS, OUT_PIN, NEO_GRB + NEO_KHZ800);

@@ -52,43 +47,81 @@
     show_color(0, 0, 0);
 }

-
 void setup()
-{
+{
     pixels.begin();
     startup_animation();

-    //Serial.begin(9600);
-    //Serial.println("node starting");
+    Serial.begin(38400);
+}

-    Mirf.spi = &MirfHardwareSpi85;
-    Mirf.cePin = 7;
-    Mirf.csnPin = 3;
-    Mirf.init();
-    Mirf.setRADDR((byte *)"aaaaa");
-    Mirf.setTADDR((byte *)"aaaab");
-    Mirf.payload = 3;
-    Mirf.channel = 111;
-    Mirf.configRegister(RF_SETUP, 0b00100110); //0x05);
-    //Mirf.configRegister(EN_AA, 0);      // turn off auto ack for all channels.
-    Mirf.baseConfig = _BV(EN_CRC) | _BV(CRCO);
-    Mirf.config();
-    delay(100);
-    //Serial.println("startup complete:" + String(Mirf.getStatus()));
+void handle_packet(uint8_t len, uint8_t *data)
+{
+    uint8_t            j;
+
+    for(int j=0; j < NUM_PIXELS; j++)
+        pixels.setPixelColor(j, pixels.Color(data[0], data[1], data[2]));
+
+    pixels.show();
 }

 void loop()
 {
-    byte data[Mirf.payload];
+    uint8_t            i, ch, data[3];
+    static uint8_t     recd = 0, found_header = 0, len = 0;
+    static uint16_t    crc = 0;
+    static uint8_t     packet[MAX_PACKET_LEN];

-    if (Mirf.dataReady())
-    {
-        Mirf.getData(data);
-        //Serial.println("data: " + String(data[0]) + " " + String(data[1]) + " " + String(data[2]));
-        for(int i=0; i < NUM_PIXELS; i++)
+    if (Serial.available() > 0)
+    {
+        ch = Serial.read();
+        if (!found_header)
         {
-            pixels.setPixelColor(i, pixels.Color(data[0], data[1], data[2]));
+            if (ch == 0xFF)
+            {
+                //show_color(255, 0, 255);
+                found_header = 1;
+            }
         }
-        pixels.show();
+        else
+        {
+            if (!len)
+            {
+                // We just found the header, now a realistic packet size must follow
+                if (ch > 0 && ch <= MAX_PACKET_LEN)
+                {
+                    len = ch;
+                    recd = 0;
+                }
+                else
+                    // Nope, that was no header, better keep looking.
+                    found_header = 0;
+            }
+            else
+            {
+                packet[recd++] = ch;
+
+                if (recd <= len - 2)
+                    crc = _crc16_update(crc, ch);
+
+                if (recd == len || recd == MAX_PACKET_LEN)
+                {
+                    uint16_t *pcrc;
+
+                    // if we received the right length, check the crc. If that matches, we have a packet!
+                    if (recd == len)
+                    {
+                        pcrc = (uint16_t *)(packet + len - 2);
+                        if (crc == *pcrc)
+                            handle_packet(len - 2, packet);
+                    }
+
+                    len = 0;
+                    crc = 0;
+                    found_header = 0;
+                    recd = 0;
+                }
+            }
+        }
     }
 }


Add another FIR lowpass for temperature signal

--- firmware/firmware.ino
+++ firmware/firmware.ino
@@ -12,6 +12,8 @@
  *  * "PID" by Brett Beauregard
  *  * "Plotter" by Devin Conley
  *  * "ArduinoThread" by Ivan Seidel
+ *  * "FIR Filter" by LeemanGeophysicalLLC - not very efficient but
+ *                                           get's the job done.
  *
  */

@@ -19,6 +21,7 @@
 #include <Plotter.h>
 #include <Thread.h>
 #include <ThreadController.h>
+#include <FIR.h>

 Plotter plotter; // for plotting over serial

@@ -50,6 +53,14 @@

 // Analog input for temperature measurement:
 const int TEMP_INP = A7;
+
+/* The temperature signal is lowpass filtered using a
+ * moving average filter during the acquisition.
+ * The cutoff of that filter is still pretty high for
+ * a temperature signal, we need another FIR
+ * filter with the cutoff in the .5Hz range.
+ */
+FIR<double, 15> filt_2nd_stage;

 void plot_callback(void) {
   // plotting thread
@@ -121,7 +132,6 @@
   uint16_t count = 0;
   unsigned long start_time = millis();
   while(millis()-start_time < 10) {
-  //while(count < 5) {
     sum += analogRead(TEMP_INP);
     count++;
   }
@@ -141,7 +151,7 @@
   const double PTB = -5.775e-7;
   double temperature =
     -(PTA/PTB/2)-sqrt((PTA/PTB/2)*(PTA/PTB/2)-(1-R_PT100/100)/PTB);
-  return(temperature);
+  return(filt_2nd_stage.processReading(temperature));
 }

 void setup() {
@@ -172,16 +182,23 @@

   // Set up serial plotter
   plotter.Begin(); // start plotter
-  plotter.AddTimeGraph( "Reflow oven variables", 18000, // about 3 minutes
+  plotter.AddTimeGraph( "Reflow oven variables", 6000,//18000, // about 3 minutes
                           "PID Setpoint (°C)", PID_setpoint,
                           "PID Input (°C)", PID_in,
-                          "PID Output (%)", PID_out_percent,
+                          //"PID Output (%)", PID_out_percent,
                           "Oven state (on/off)", oven_is_on);

   // Configure PID controller
   control_PID.SetOutputLimits(0, pwm_window);
   // turn PID controller on
   control_PID.SetMode(AUTOMATIC);
+
+  // Calculated using scipy.signal. Have a look at the "design" folder
+  double fir_coeffs[] = {
+        0.0367883 ,  0.05045512,  0.06366198,  0.07568267,  0.08583937,
+        0.09354893,  0.09836316,  0.1       ,  0.09836316,  0.09354893,
+        0.08583937,  0.07568267,  0.06366198,  0.05045512,  0.0367883 };
+  filt_2nd_stage.setFilterCoeffs(fir_coeffs);
 }

 void loop() {


use Servo lib for steering

use Servo lib for steering

--- firmware/firmware.ino
+++ firmware/firmware.ino
@@ -1,3 +1,4 @@
+#include <Servo.h>

 #include "SharpIR.h"
 #include "Communication.h"
@@ -34,6 +35,8 @@
 uint8_t m_tx_len = 0;
 HDLC hdlc(m_rx_buffer, 40);

+Servo steeringservo;
+
 void send_telemetry(uint8_t left, uint8_t right, uint8_t front_left, uint8_t front_right) {
     bc_telemetry_packet_t bc_telemetry;
     bc_telemetry.header = BC_TELEMETRY;
@@ -52,6 +55,8 @@
     pinMode(IR_RIGHT, INPUT);
     pinMode(IR_FRONT_LEFT, INPUT);
     pinMode(IR_FRONT_RIGHT, INPUT);
+
+    steeringservo.attach(STEERING_PWM_PIN);
 }

 void loop() {
@@ -64,7 +69,8 @@

             if (CB_MOTOR_COMMAND == header) {
                 cb_motor_command_packet_t* motor = (cb_motor_command_packet_t*)m_rx_buffer;
-                analogWrite(STEERING_PWM_PIN, motor->steering_pwm);
+                //analogWrite(STEERING_PWM_PIN, motor->steering_pwm);
+                steeringservo.write(motor->steering_pwm);
                 analogWrite(DRIVE_PWM_PIN, motor->drive_pwm);
             }
         }


no message

no message

--- firmware/firmware.ino
+++ firmware/firmware.ino
@@ -179,57 +179,6 @@
           servoR.write(mass[4],mass[5]);
           servoHandRot.write(mass[6],mass[7]);
         break;
-=======
-  uarm.init();          // initialize the uArm position
-  uarm.setServoSpeed(SERVO_R, 50);  // 0=full speed, 1-255 slower to faster
-  uarm.setServoSpeed(SERVO_L, 50);  // 0=full speed, 1-255 slower to faster
-  uarm.setServoSpeed(SERVO_ROT, 50); // 0=full speed, 1-255 slower to faster
-}
-
-void loop(void) {
-  uarm.calibration();   // if corrected, you could remove it, no harm though
-  if (Serial.available()) {
-    rxBuf = Serial.read();
-    Serial.print("rxBuf = " + String(rxBuf) + "\n");
-
-    if (stateMachine == 0) {
-      stateMachine = rxBuf == 0xFF ? 1 : 0;
-    } else if (stateMachine == 1) {
-      if (rxBuf == 0xEE) {
-        stateMachine = 2;
-      } else if (rxBuf == 0xDD) {
-        stateMachine = 3;
-      } else {
-        stateMachine = 0;
-      }
-    } else if (stateMachine == 2) {
-      dataBuf[counter++] = rxBuf;
-      if(counter > 7) {
-        stateMachine = 0;
-        counter=0;
-
-        *((char *)(&stretchTemp )  )  = dataBuf[1];
-        *((char *)(&stretchTemp )+1)  = dataBuf[0];
-        *((char *)(&heightTemp  )  )  = dataBuf[3];
-        *((char *)(&heightTemp  )+1)  = dataBuf[2];
-        *((char *)(&rotationTemp)  )  = dataBuf[5];
-        *((char *)(&rotationTemp)+1)  = dataBuf[4];
-        *((char *)(&handRotTemp )  )  = dataBuf[7];
-        *((char *)(&handRotTemp )+1)  = dataBuf[6];
-
-        Serial.print("stretch = " + String(stretchTemp) + "; height = " + String(heightTemp) + "; rotation = " + String(rotationTemp) + "; handRot = " + String(handRotTemp) + ";\n");
-
-        uarm.setPosition(stretchTemp, heightTemp, rotationTemp, handRotTemp);
-      }
-    } else if (stateMachine == 3) {
-      stateMachine = 0;
-      if (rxBuf) {
-        uarm.gripperCatch();
-        Serial.print("gripperCatch\n");
-      } else {
-        uarm.gripperRelease();
-        Serial.print("gripperRelease\n");
->>>>>>> bea2b19c183c0fccc0dbb2e7aeadd60bfa027926
       }
       msg = "";
     }


Updated teensy serial test

Updated teensy serial test

--- proj/hypercube/src/teensy_serial_in_test/teensy_serial_in_test.ino
+++ proj/hypercube/src/teensy_serial_in_test/teensy_serial_in_test.ino
@@ -1,7 +1,7 @@
 #include <FastLED.h>

 #define NUM_LED 30
-#define LED_PIN 1
+#define LED_PIN 5

 CRGB leds[NUM_LED];

@@ -10,9 +10,8 @@

 void setup() {
   // Setup Serial
-  Serial2.begin(9600);
-
-  pinMode(0, OUTPUT);
+  Serial1.begin(9600);
+  Serial.begin(9600);

   // Setup FastLED
   FastLED.addLeds<WS2812B, LED_PIN, GRB>(leds, NUM_LED);
@@ -23,20 +22,35 @@

 void loop() {

-  fill_solid(leds, NUM_LED, CHSV(gHue, 255, 100));
-  FastLED.show();
-  FastLED.delay(10); // idk why I'm doing this..
-    delay(1000);
+  if ( Serial1.available() > 0) {
+    gHue = Serial1.read();
+    fill_solid(leds, NUM_LED, CHSV(gHue, 255, 100));
+    FastLED.show();
+    FastLED.delay(10); // idk why I'm doing this..
+  }

+
+  //    if (got_event) {
+  //      fill_solid(leds, NUM_LED, CHSV(gHue, 255, 100));
+  //      FastLED.show();
+  //      FastLED.delay(10); // idk why I'm doing this..
+  //      got_event = false;
+  //    }
+
+  //  fill_solid(leds, NUM_LED, CHSV(gHue, 255, 100));
+  //  FastLED.show();
+  //  FastLED.delay(10); // idk why I'm doing this..
+  //  gHue++;

 }

 void serialEvent() {
-  while (Serial2.available()) {
-    gHue = Serial2.read();
-    if ((char)Serial2.read() == '\n') {
-      got_event = true;
-    }
+  while (Serial1.available()) {
+    gHue = Serial1.read();
+    //    if ((char)Serial.read() == '\n') {
+    //      got_event = true;
+    //    }
+    got_event = true;
   }
 }


Use ResponsiveAnalogRead to filter out noise. Removed debug spam to make manual control work once again.

--- maze_robot.ino
+++ maze_robot.ino
@@ -1,6 +1,7 @@
 #include <Servo.h>
 #include <Metro.h>
 //#define Serial Serial1
+#include <ResponsiveAnalogRead.h>

 #include <stdarg.h>
 void p(char *fmt, ... ){
@@ -32,10 +33,13 @@
 // #define UP_BUTTON 2

 #define DIST_IN_F 20
+ResponsiveAnalogRead dist_f_reader(DIST_IN_F, true);
 int dist_f = 0;
 #define DIST_IN_L 19
+ResponsiveAnalogRead dist_l_reader(DIST_IN_L, true);
 int dist_l = 0;
 #define DIST_IN_R 18
+ResponsiveAnalogRead dist_r_reader(DIST_IN_R, true);
 int dist_r = 0;

 #define MOTOR_OUT_L 16
@@ -63,9 +67,10 @@
 int dir_sign = 0;

 bool manual_control = true;
+int debug_light_on = 1;
 byte byteRead;

-Metro debug_out = Metro(1000);
+Metro debug_out = Metro(10000);

 void setup() {
   pinMode(13, OUTPUT);
@@ -75,7 +80,8 @@
   wheel_right.attach(MOTOR_OUT_R);

   pinMode(DEBUGGER_ATTACHED_PIN, INPUT_PULLUP);
-  manual_control = digitalRead(DEBUGGER_ATTACHED_PIN) == LOW;
+  delay(100);
+  manual_control = !digitalRead(DEBUGGER_ATTACHED_PIN);

   // pinMode(STOP_BUTTON, INPUT_PULLUP);
   // pinMode(DOWN_BUTTON, INPUT_PULLUP);
@@ -88,6 +94,12 @@
 }

 void loop() {
+  debug_light_on = 1 - debug_light_on;
+  digitalWrite(13, debug_light_on);
+
+  dist_f_reader.update();
+  dist_l_reader.update();
+  dist_r_reader.update();
   dist_f = analogRead(DIST_IN_F);
   dist_l = analogRead(DIST_IN_L);
   dist_r = analogRead(DIST_IN_R);
@@ -138,7 +150,6 @@
       map(speed, 0, 255, right_zero, right_rw_max),
       right_zero,
       map(speed, 0, 255, right_zero, right_fwd_max));
-
   }
   else
   {
@@ -189,4 +200,4 @@
 {
   p(F("MANUAL: %d, L %03d, F %03d, R %03d, dir %03d, speed %03d, left_mapped %03d, right_mapped %03d"),
     manual_control, dist_l, dist_f, dist_r, dir, speed, left_mapped, right_mapped);
-}+}


delete

delete

--- Platforms/Arduino/temphumi.ino
+++ Platforms/Arduino/temphumi.ino
@@ -1,35 +0,0 @@
-#include <DHT11.h>
-int pin=D3;
-DHT11 dht11(pin);
-void setup()
-{
-   Serial.begin(9600);
-  while (!Serial) {
-      ; // wait for serial port to connect. Needed for Leonardo only
-    }
-}
-
-void loop()
-{
-  int err;
-  float temp, humi;
-  if((err=dht11.read(humi, temp))==0)
-  {
-    Serial.print("temperature:");
-    Serial.print(temp);
-    Serial.print(" humidity:");
-    Serial.print(humi);
-    Serial.println();
-  }
-  else
-  {
-    Serial.println();
-    Serial.print("Error No :");
-    Serial.print(err);
-    Serial.println();
-  }
-  delay(DHT11_RETRY_DELAY); //delay for reread
-}
-
-
-


report interval 1 min to 1 sec

report interval 1 min to 1 sec

--- _25-attiny85-nrf24-ds18b20/_25-attiny85-nrf24-ds18b20.ino
+++ _25-attiny85-nrf24-ds18b20/_25-attiny85-nrf24-ds18b20.ino
@@ -129,10 +129,14 @@
 }

 void sleep() {
+  /* 60 seconds
   for (int i = 0; i < 7; i++) {
     LowPower.powerDown(SLEEP_8S, ADC_OFF, BOD_OFF);
   }
   LowPower.powerDown(SLEEP_4S, ADC_OFF, BOD_OFF);
+  */
+  /* 1 sec */
+  LowPower.powerDown(SLEEP_1S, ADC_OFF, BOD_OFF);
 }

 int readVcc() {


Code changes as per hw rev.1 and later

Code changes as per hw rev.1 and later

--- control_board/firmware/vasttrapi.ino
+++ control_board/firmware/vasttrapi.ino
@@ -26,7 +26,7 @@

 const unsigned short touchSensorPin = PB2;
 const unsigned short rpiPwrPin = PB3;
-const boolean powerOnState = LOW; // The rpiPwrPin state that turns the power on to the RPi
+const boolean powerOnState = HIGH; // The rpiPwrPin state that turns the power on to the RPi

 volatile unsigned long timeToSleep = 0;
 const unsigned long awakePeriod = 600000; // Time in milliseconds to remain awake on every new touch


mode button works (needs debounce)

mode button works (needs debounce)

--- main/main.ino
+++ main/main.ino
@@ -156,6 +156,9 @@
 bool octoModeIndex = 0;
 bool inError = false;

+bool currentModeButton = 1; //default to high, 0 when pressed
+bool previousModeButton = 1;
+
 MCP_CAN CAN(SPI_CS_PIN);

 void setup() {
@@ -252,7 +255,14 @@
     }
   }

-  bool modeButton = digitalRead(MODE_PIN);
+  previousModeButton = currentModeButton;
+  currentModeButton = digitalRead(MODE_PIN);
+  if(currentModeButton != previousModeButton) {
+    if(currentModeButton == 0) {
+      next_mode();
+    }
+  }
+
   //Serial.println(modeButton);

 }


variables definition and code cleaning

variables definition and code cleaning

issue: preparing for merge encoder reading function

--- send/send.ino
+++ send/send.ino
@@ -7,14 +7,57 @@
 #include "mcp_can.h"
 #include <mcp_can_dfs.h>

+// Define States
+#define Startup             1
+#define Pre_Operational     2
+#define Operational         3
+
+// Statemachine State variable and initial value
+byte State = Startup;

 // the cs pin of the version after v1.1 is default to D9
 // v0.9b and v1.0 is default D10
 const int SPI_CS_PIN = 9;
+// Set CS pin
+MCP_CAN CAN(SPI_CS_PIN);
+
+// Sensor variables
 uint32_t encoder_data = 0;
 uint32_t loadcell_data = 0;

-MCP_CAN CAN(SPI_CS_PIN);                                    // Set CS pin
+// ***************************
+// EPOS2 CANOpen Communication
+// ***************************
+
+// Statemachine
+unsigned char set_pre_operational[2] = {0x80, 0};
+unsigned char set_operational[2] = {0x1, 0};
+unsigned char enable_epos[8] = {0x2B, 0x40, 0x60, 0, 0x0F, 0, 0, 0};
+unsigned char disable_epos[8] = {0x2B, 0x40, 0x60, 0, 0x06, 0, 0, 0};
+
+// PDO Configuration
+unsigned char pdo_actual_position_1[8] = {0x22, 0x02, 0x18, 0x01, 0x81, 0x03, 0, 0};
+unsigned char pdo_actual_position_2[8] = {0x22, 0x02, 0x18, 0x02, 0x01, 0, 0, 0};
+
+unsigned char pdo_actual_velocity_1[8] = {0x22, 0x02, 0x18, 0x01, 0x81, 0x03, 0, 0};
+unsigned char pdo_actual_velocity_2[8] = {0x22, 0x02, 0x18, 0x02, 0x01, 0, 0, 0};
+
+unsigned char pdo_actual_current_1[8] = {0x22, 0x00, 0x1A, 0x00, 0x00, 0, 0, 0};
+unsigned char pdo_actual_current_2[8] = {0x22, 0x00, 0x1A, 0x01, 0x10, 0, 0x78, 0x60};
+unsigned char pdo_actual_current_3[8] = {0x22, 0x00, 0x1A, 0x02, 0x10, 0, 0x27, 0x20};
+unsigned char pdo_actual_current_4[8] = {0x22, 0x00, 0x18, 0x02, 0x01, 0, 0, 0};
+unsigned char pdo_actual_current_5[8] = {0x22, 0x00, 0x1A, 0x00, 0x01, 0, 0, 0};
+
+// Objects Writing
+unsigned char set_max_following_error[8] = {0x22, 0x65, 0x60, 0, 0xD0, 7, 0, 0};
+unsigned char set_max_acceleration[8] = {0x22, 0xC5, 0x60, 0, 0x88, 0x13, 0, 0};
+unsigned char set_max_profile_velocity[8] = {0x22, 0x7F, 0x60, 0, 0xD0, 7, 0, 0};
+unsigned char setpoint_position[8] = {0x22, 0x62, 0x20, 0, 0, 0, 0, 0};
+
+// Objects Reading
+unsigned char get_actual_position[8] = {0x40, 0x64, 0x60, 0, 0, 0, 0, 0};
+unsigned char get_actual_velocity[8] = {0x40, 0x6C, 0x60, 0, 0, 0, 0, 0};
+unsigned char get_actual_current[8] = {0x40, 0x78, 0x60, 0, 0, 0, 0, 0};

 void setup()
 {
@@ -68,11 +111,6 @@

 }

-
-
 /*********************************************************************************************************
   END FILE
 *********************************************************************************************************/
-
-
-


Move getTemperature() debug to its own #define

--- lcd_temperature_display.ino
+++ lcd_temperature_display.ino
@@ -95,7 +95,7 @@
   *averageval *= (float)TEMPERATURE_MULTIPLIER;
   *averageval += (float)TEMPERATURE_ADDEND;

-  #ifdef DEBUG
+  #ifdef DEBUG_GETTEMPERATURE
     Serial.print("getTemperature: readval=");
     Serial.print(readval);
     Serial.print(" currentVoltageBin=");


Swap back to motor stim

Swap back to motor stim

--- ant_trigger/ant_trigger/ant_trigger.ino
+++ ant_trigger/ant_trigger/ant_trigger.ino
@@ -12,7 +12,7 @@
 // Timing variables
 const int PREDELAY  = 10;      // in ms, before stim goes on
 const int ANTDELAY = 250;      // in ms, between whap start and shutter
-const int SHUTTERDELAY = 1;    // in ms, between shutter and camera
+const int SHUTTERDELAY = 50;   // in ms, between shutter and camera
 const int EXPOSUREDUR = 1;     // in ms, camera image duration  TODO: SEND THIS ONE OVER SERIAL TOO
 const int POSTEXPOSURE = 1000; // in ms, before returning to length

@@ -102,10 +102,7 @@
 void run(int predelay, int antdelay, int shutterdelay, int exposuredur, int postexposure)
 {
 //    delay(predelay);      // just to let things settle
-//    turn_ant(true);       // whap the ant
-    while(!digitalRead(STIM_IN)){
-      delayMicroseconds(2);
-    }
+    turn_ant(true);       // whap the ant
     delay(antdelay);      // wait for motor movement
     turn_shutter(true);   // tell the shutter to open
     delay(shutterdelay);  // let the shutter physically respond


tweaks

tweaks

--- PinewoodDerbyPracticeTrackTimer/PinewoodDerbyPracticeTrackTimer.ino
+++ PinewoodDerbyPracticeTrackTimer/PinewoodDerbyPracticeTrackTimer.ino
@@ -1,10 +1,10 @@
 #include <LiquidCrystal.h>

-#define END_SENSOR A1
-#define START_SENSOR A2
+#define END_SENSOR A1 // sensor at start gate
+#define START_SENSOR A2 // sensor at finish line
 #define SENSOR_LOW 0
-#define SENSOR_HIGH 150 //250
-// sensor % is used
+
+// sensor pct is used
 // as a multiplier against the ambient
 // light
 #define SENSOR_PCT 0.8
@@ -21,6 +21,7 @@
 void setup() {
   lcd.begin(16, 2);
   lcd.clear();
+
   pinMode(START_SENSOR, INPUT);
   pinMode(END_SENSOR, INPUT);
   Serial.begin(9600);      // open the serial port at 9600 bps:
@@ -100,6 +101,7 @@
     elapsed_time = endtime - starttime;

     // debounce finished sensor
+    // probably not strictly necessary
     while (finished) {
       finished = sensor_tripped(END_SENSOR, SENSOR_LOW, sensor_high_end);
     }


Add "ra" column to the input character list.

--- mozc-piropiro/arduino/piropiro/piropiro.ino
+++ mozc-piropiro/arduino/piropiro/piropiro.ino
@@ -26,7 +26,7 @@
 #define COMMIT_TIME (600 / INTERVAL_MSEC)

 #define ROWS 5
-#define COLS 9
+#define COLS 10

 const char* characters[ROWS * COLS] = {
   "a", "i", "u", "e", "o",
@@ -37,6 +37,7 @@
   "ha", "hi", "hu", "he", "ho",
   "ma", "mi", "mu", "me", "mo",
   "ya", "ya", "yu", "yu", "yo",
+  "ra", "ri", "ru", "re", "ro",
   "wa", "wo", "nn", ",", "."
 };


cleaned up code a bit

cleaned up code a bit

--- NTPclock/NTPclock.ino
+++ NTPclock/NTPclock.ino
@@ -36,11 +36,12 @@
   connectWifi(); //Then connect to wifi
   beginNTP();    //Start up NTP Client & time keeping

+  //Called upon NTP update.
   NTP.onNTPSyncEvent([](NTPSyncEvent_t ntpEvent) {
     if (ntpEvent == 0) {
       Serial.print("Got NTP time: ");
       Serial.println(NTP.getTimeDateString(NTP.getLastNTPSync()));
-      RTC.set(now() );
+      RTC.set(NTP.getLastNTPSync());
     }
   });

@@ -54,18 +55,7 @@
   updateMisc();   //
   delay(1000);    //

-  Serial.print( month() );
-  Serial.print(" ");
-  Serial.print( day() );
-  Serial.print(" ");
-  Serial.print( year() );
-  Serial.print(" ");
-  Serial.print( hour() );
-  Serial.print(" ");
-  Serial.print( minute() );
-  Serial.print(" ");
-  Serial.print( second() );
-  Serial.println();
+  NTP.getTimeDateString(now() );
 }

 //


Fixed negative anticlockwise spin and position

--- src/miscr.ino
+++ src/miscr.ino
@@ -285,8 +285,8 @@
     in = trim(in);
     int yindex = getIndex('Y', in);

-    if(xindex > -1) sX.testSpin(xindex * 80);
-    if(yindex > -1) sY.testSpin(yindex * 80);
+    if(xindex != INT_MIN) sX.testSpin(xindex * 80);
+    if(yindex != INT_MIN) sY.testSpin(yindex * 80);

     if(xindex == INT_MIN && yindex == INT_MIN) {
       out.msg(1, "No parameters/Invalid values for G0/G1");


tweak

tweak

--- examples/Ports/dht_demo/dht_demo.ino
+++ examples/Ports/dht_demo/dht_demo.ino
@@ -12,11 +12,12 @@

 void loop () {
   int t, h;
-  dht.reading(t, h);
-  Serial.print("temperature = ");
-  Serial.println(t);
-  Serial.print("humidity = ");
-  Serial.println(h);
-  Serial.println();
+  if (dht.reading(t, h)) {
+    Serial.print("temperature = ");
+    Serial.println(t);
+    Serial.print("humidity = ");
+    Serial.println(h);
+    Serial.println();
+  }
   delay(3000);
 }


Fix warning.

Fix warning.

--- examples/SimpleClock/SimpleClock.ino
+++ examples/SimpleClock/SimpleClock.ino
@@ -41,7 +41,7 @@
   Serial.println(time);
   Serial.println("Press Enter to adjust the time");
   Serial.setTimeout(3000);
-  if (Serial.find("\n")) {
+  if (Serial.find((char*)"\n")) {
     Serial.setTimeout(99999);
     Serial.println("Type the new time in the format above and press Enter");
     Serial.println("Press only Enter if you want to keep the same time");


Fixed: make CI work well.

Fixed: make CI work well.

--- examples/multiDevice/multiDevice.ino
+++ examples/multiDevice/multiDevice.ino
@@ -12,7 +12,7 @@
 #include "MLX90615.h"

 // #define BY_PINS // Uncomment this to use pins for every MLX
-// #define BY_ADDR // Uncomment this to use single bus
+#define BY_ADDR // Uncomment this to use single bus

 #ifdef BY_PINS // USING ADDITIONAL PINS
     byte sda_1 = 3;


Update SerialDisplay.ino

Update SerialDisplay.ino

--- Arduino/SerialDisplay/SerialDisplay.ino
+++ Arduino/SerialDisplay/SerialDisplay.ino
@@ -1,59 +1,60 @@
-#include <Wire.h>  // Comes with Arduino IDE
+#include <LiquidCrystal_I2C.h> // From https://bitbucket.org/fmalpartida/new-liquidcrystal
+#include <Wire.h>              // Comes with Arduino IDE
 #include <string.h>
-#include <LiquidCrystal_I2C.h>
-
-LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);

 #define LCD_ROWS 20
 #define LCD_COLS 4

-void setup(){
+LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);
+
+void print();
+void blink();
+void disable();
+void enable();
+
+void setup() {
   Serial.begin(9600);
-  lcd.begin(LCD_ROWS,LCD_COLS);
-  lcd.backlight();
-  lcd.setCursor(7,1);
+  lcd.begin(LCD_ROWS, LCD_COLS);
+  lcd.setCursor(7, 1);
   lcd.print("Ready!");
 }

-void loop(){
-  if(Serial.available()){
+void loop() {
+  if (Serial.available()) {
     String received = Serial.readStringUntil('\n');
-    if(received == "_DISABLE"){
+    if (received == "_DISABLE") {
       disable();
-    }else if(received == "_ENABLE"){
+    } else if (received == "_ENABLE") {
       enable();
-    }
-    }else{
+    } else {
       print(received);
     }
   }
 }

-void print(String s){
+void print(const String &s) {
   lcd.clear();
   int currentLine = 0;
   int increment = LCD_ROWS;
-  for (int start = 0; start < s.length() ; start += increment){
-    if(currentLine < 4){
-      while(s.substring(start).startsWith(" ")){
-        start++;
-      }
-      String line;
-      if(start + LCD_ROWS > s.length()){
-        line = s.substring(start);
-      }else{
-        line = s.substring(start, start + LCD_ROWS);
-      }
-      lcd.setCursor(0,currentLine++);
-      lcd.print(line);
+  for (int start = 0; start < s.length() && currentLine < 4;
+       start += increment) {
+    while (s.substring(start).startsWith(" ")) {
+      start++;
     }
+    String line;
+    if (start + LCD_ROWS > s.length()) {
+      line = s.substring(start);
+    } else {
+      line = s.substring(start, start + LCD_ROWS);
+    }
+    lcd.setCursor(0, currentLine++);
+    lcd.print(line);
   }
   delay(100);
 }

-void blink(int n){
-  for(int i = 0; i<n; i++)
-  {
+void blink(int n) {
+  for (int i = 0; i < n; i++) {
     lcd.backlight();
     delay(250);
     lcd.noBacklight();
@@ -61,12 +62,12 @@
   }
 }

-void disable(){
+void disable() {
   lcd.noDisplay();
   lcd.noBacklight();
 }

-void enable(){
+void enable() {
   lcd.display();
   lcd.backlight();
-}+}


Temp sensor example: add code to sample from DHT22 sensor

--- examples/temp_sensor/temp_sensor.ino
+++ examples/temp_sensor/temp_sensor.ino
@@ -1,5 +1,7 @@
 // TimerOne library: https://code.google.com/p/arduino-timerone/
-#include <TimerOne.h>
+#include <TimerOne.h>
+// DHT library: https://github.com/adafruit/DHT-sensor-library
+#include "DHT.h"
 #include <SPI.h>
 #include <BLEPeripheral.h>

@@ -8,14 +10,25 @@
 #define BLE_RDY   2
 #define BLE_RST   9

+#define DHTTYPE DHT22
+#define DHTPIN 3
+
+DHT dht(DHTPIN, DHTTYPE);
+
 BLEPeripheral blePeripheral = BLEPeripheral(BLE_REQ, BLE_RDY, BLE_RST);

 BLEService tempService = BLEService("CCC0");
+BLECharacteristic tempCharacteristic = BLECharacteristic("CCC1", BLERead | BLENotify, 7);
+BLEDescriptor tempDescriptor = BLEDescriptor("2901", "Temp Celsius");

-BLEIntCharacteristic tempCharacteristic = BLEIntCharacteristic("CCC1", BLERead | BLENotify);
-BLEDescriptor tempDescriptor = BLEDescriptor("2901", "Celsius");
+BLEService humidityService = BLEService("DDD0");
+BLECharacteristic humidityCharacteristic = BLECharacteristic("DDD1", BLERead | BLENotify, 6);
+BLEDescriptor humidityDescriptor = BLEDescriptor("2901", "Humidity Percent");

-volatile bool readTemperature = false;
+volatile bool readFromSensor = false;
+
+float lastTempReading;
+float lastHumidityReading;

 void setup() {
   Serial.begin(115200);
@@ -27,43 +40,74 @@
 #endif

   blePeripheral.setLocalName("Temperature");
+
   blePeripheral.setAdvertisedServiceUuid(tempService.uuid());
   blePeripheral.addAttribute(tempService);
   blePeripheral.addAttribute(tempCharacteristic);
   blePeripheral.addAttribute(tempDescriptor);
+
+  blePeripheral.setAdvertisedServiceUuid(humidityService.uuid());
+  blePeripheral.addAttribute(humidityService);
+  blePeripheral.addAttribute(humidityCharacteristic);
+  blePeripheral.addAttribute(humidityDescriptor);

   blePeripheral.setEventHandler(BLEConnected, blePeripheralConnectHandler);
   blePeripheral.setEventHandler(BLEDisconnected, blePeripheralDisconnectHandler);

   blePeripheral.begin();

-  Timer1.initialize(1 * 1000000); // in milliseconds
+  Timer1.initialize(2 * 1000000); // in milliseconds
   Timer1.attachInterrupt(timerHandler);
+
+  Serial.println(F("BLE Temperature Sensor Peripheral"));
 }

 void loop() {
   blePeripheral.poll();

-  if (readTemperature) {
+  if (readFromSensor) {
     setTempCharacteristicValue();
-    readTemperature = false;
+    setHumidityCharacteristicValue();
+    readFromSensor = false;
   }
 }

 void timerHandler() {
-  readTemperature = true;
+  readFromSensor = true;
 }

 void setTempCharacteristicValue() {
-  int temp = readTempC();
-  tempCharacteristic.setValue(temp);
-  Serial.println(temp);
+  float reading = dht.readTemperature();
+//  float reading = random(100);
+
+  if (!isnan(reading) && significantChange(lastTempReading, reading, 0.5)) {
+    char formatted[7];
+    dtostrf(reading, 3, 1, formatted);
+    tempCharacteristic.setValue(formatted);
+
+    Serial.print(F("Temperature: ")); Serial.print(reading); Serial.println(F("C"));
+
+    lastTempReading = reading;
+  }
 }

-int readTempC() {
-  // Stubbing out for demo with random value generator
-  // Replace with actual sensor reading code
-  return random(100);
+void setHumidityCharacteristicValue() {
+  float reading = dht.readHumidity();
+//  float reading = random(100);
+
+  if (!isnan(reading) && significantChange(lastHumidityReading, reading, 1.0)) {
+    char formatted[6];
+    dtostrf(reading, 3, 1, formatted);
+    humidityCharacteristic.setValue(formatted);
+
+    Serial.print(F("Humidity: ")); Serial.print(reading); Serial.println(F("%"));
+
+    lastHumidityReading = reading;
+  }
+}
+
+boolean significantChange(float val1, float val2, float threshold) {
+  return (abs(val1 - val2) >= threshold);
 }

 void blePeripheralConnectHandler(BLECentral& central) {
@@ -75,3 +119,4 @@
   Serial.print(F("Disconnected event, central: "));
   Serial.println(central.address());
 }
+


Update WaterQualityMonitor.ino

Update WaterQualityMonitor.ino


COH-23: WIP

COH-23: WIP

--- test/test.ino
+++ test/test.ino
@@ -182,9 +182,31 @@
   }

 // Some tests for all ISRRXQueue implementations.
-// Assumes being passed a freshly-created instance.
+// Assumes being passed an empty freshly-created instance.
+// Leaves the queue instance empty when done.
 static void allISRRXQueue(OTRadioLink::ISRRXQueue &q)
   {
+  uint8_t queueRXMsgsMin;
+  uint8_t maxRXMsgLen;
+  q.getRXCapacity(queueRXMsgsMin,maxRXMsgLen);
+  AssertIsTrueWithErr((queueRXMsgsMin >= 1), queueRXMsgsMin);
+  AssertIsTrueWithErr((maxRXMsgLen >= 64), maxRXMsgLen);
+  AssertIsEqual(0, q.getRXMsgsQueued());
+  uint8_t buf1[1];
+  AssertIsEqual(0, q.getRXMsg(buf1, 1));
+  // Pretend to be an ISR and try to load up a message.
+  volatile uint8_t *ib1 = q._getRXBufForInbound();
+  AssertIsTrue(NULL != ib1);
+  const uint8_t r1 = OTV0P2BASE::randRNG8();
+  *ib1 = r1;
+  q._loadedBuf(1);
+  // Try to retrieve the queued message.
+  AssertIsEqual(1, q.getRXMsgsQueued());
+  AssertIsEqual(1, q.getRXMsg(buf1, 1));
+  AssertIsEqual(r1, buf1[0]);
+  // Check that the queue is empty again.
+  AssertIsEqual(0, q.getRXMsgsQueued());
+  AssertIsEqual(0, q.getRXMsg(buf1, 1));
   }

 // Do some basic exercise of the RFM23B class, eg that it compiles.
@@ -193,8 +215,7 @@
   Serial.println("ISRRXQueue1Deep");
   OTRadioLink::ISRRXQueue1Deep<> q0;
   allISRRXQueue(q0);
-
-
+  // TODO: specific tests for this buffer.
   }


Update code.ino

Update code.ino

--- code/code.ino
+++ code/code.ino
@@ -1,16 +1,9 @@
 float angulo;
-
 void setup() {
-
-	// Executa o setup do angulo.ino
 	MPU_setup();
-	PID_setup();
-
+	PID_setup();
+}
+void loop() {
+	MPU_loop();
 }

-void loop() {
-
-	MPU_loop();
-
-}
-


style: Update code style w/cstyle

style: Update code style w/cstyle

--- cold-brew.ino
+++ cold-brew.ino
@@ -76,14 +76,14 @@
 uint32_t drainLEDColor = ledStat.Color(255, 0, 0);      // Red

 void setup() {
-	Serial.begin(9600);
-	pinMode(DRAIN_MTR, OUTPUT);
-	pinMode(VALV_ATM, OUTPUT);
-	pinMode(VACU_PUMP, OUTPUT);
-	pinMode(PRS_SEN, INPUT_PULLUP);
-	pinMode(CAP_TCH, INPUT);
-	pinMode(MECH_SWT, INPUT_PULLUP);
-	ledStat.begin();
+  Serial.begin(9600);
+  pinMode(DRAIN_MTR, OUTPUT);
+  pinMode(VALV_ATM, OUTPUT);
+  pinMode(VACU_PUMP, OUTPUT);
+  pinMode(PRS_SEN, INPUT_PULLUP);
+  pinMode(CAP_TCH, INPUT);
+  pinMode(MECH_SWT, INPUT_PULLUP);
+  ledStat.begin();
 }

 //	Keeps track of button presses to direct button states
@@ -92,224 +92,224 @@

 void loop() {

-    // read the state of the switch into a local variable:
-    int reading = digitalRead(CAP_TCH);
-
-    // If the switch changed, due to noise or pressing:
-    if (reading != lastButtonState) {
-        // reset the debouncing timer
-        lastDebounceTime = millis();
+  // read the state of the switch into a local variable:
+  int reading = digitalRead(CAP_TCH);
+
+  // If the switch changed, due to noise or pressing:
+  if (reading != lastButtonState) {
+    // reset the debouncing timer
+    lastDebounceTime = millis();
+  }
+
+  if ((millis() - lastDebounceTime) > debounceDelay) {
+    // if the button state has changed:
+    if (reading != buttonState) {
+      buttonState = reading;
+
+      // only toggle the LED if the new button state is HIGH
+      if (buttonState == LOW) {
+        //    Buttons used to manipulate states in SM
+
+
+        //  Increment buttonPresses and constrain it to [0, NUM_OF_STATES - 1]
+        buttonPresses = ++buttonPresses % NUM_OF_STATES;
+
+        Serial.print("This is what: ");
+        Serial.println(buttonPresses);
+        switch (buttonPresses){
+          case IDLED:
+            Serial.println("IDLE");
+            closeArmIntoClosedLidState();
+            stateMachine.transitionTo(idle_state);
+            break;
+
+          case VACU:
+            Serial.println("VACU");
+
+            closeArmIntoClosedLidState();
+
+            //  Begin timer for total time between Vacuum and Dwell states
+            startTime = millis();
+            stateStartTime = millis();
+
+            //	Ensure next button press will lead to DRAIN state.
+            buttonPresses = 2;
+
+            stateMachine.transitionTo(vacu_state);
+            break;
+
+          case DWLL:
+            Serial.println("DWLL");
+            stateMachine.transitionTo(drin_state);
+            break;
+
+          case DRIN:
+            Serial.println("DRIN");
+            moveArmIntoDrainOpenState();
+            stateStartTime = millis();
+            stateMachine.transitionTo(drin_state);
+            break;
+        }
+      }
     }
-
-    if ((millis() - lastDebounceTime) > debounceDelay) {
-        // if the button state has changed:
-        if (reading != buttonState) {
-          buttonState = reading;
-
-          // only toggle the LED if the new button state is HIGH
-          if (buttonState == LOW) {
-            //    Buttons used to manipulate states in SM
-
-
-            //  Increment buttonPresses and constrain it to [0, NUM_OF_STATES - 1]
-            buttonPresses = ++buttonPresses % NUM_OF_STATES;
-
-            Serial.print("This is what: ");
-            Serial.println(buttonPresses);
-            switch (buttonPresses){
-                case IDLED:
-                	Serial.println("IDLE");
-					closeArmIntoClosedLidState();
-               		stateMachine.transitionTo(idle_state);
-                	break;
-
-                case VACU:
-                	Serial.println("VACU");
-
-					closeArmIntoClosedLidState();
-
-                   	//  Begin timer for total time between Vacuum and Dwell states
-                   	startTime = millis();
-					stateStartTime = millis();
-
-					//	Ensure next button press will lead to DRAIN state.
-					buttonPresses = 2;
-
-                    stateMachine.transitionTo(vacu_state);
-                    break;
-
-                case DWLL:
-                    Serial.println("DWLL");
-                    stateMachine.transitionTo(drin_state);
-                    break;
-
-                case DRIN:
-                	Serial.println("DRIN");
-					moveArmIntoDrainOpenState();
-					stateStartTime = millis();
-                	stateMachine.transitionTo(drin_state);
-                	break;
-            }
-          }
-        }
+  }
+
+#ifdef DEBUG
+  //    if ((millis() % 1000) == 0) {
+  //        Serial.print("This is pressure switch");
+  //        Serial.println(digitalRead(PRS_SEN));
+  //    }
+#endif
+
+  lastButtonState = reading;
+
+  //	Updates the SM for every loop -- APPLICATION CRITICAL
+  stateMachine.update();
+}
+
+void moveArmIntoDrainOpenState() {
+  digitalWrite(DRAIN_MTR, HIGH);
+  Serial.println("PREPARING TO DRAIN -- ARM MOVING");
+  delay(TIME_FOR_MTR_TO_OPEN_LID);
+  digitalWrite(DRAIN_MTR, LOW);
+}
+
+void closeArmIntoClosedLidState() {
+  //	At switch state ON (When CAM is pushing against
+  //		push button) CAM is in closed state.
+  //		Default value is OFF, or logic high; switch
+  //		uses input pullup.
+  //static unsigned long currentTime = millis();
+  int mechSwitchResult = digitalRead(MECH_SWT);
+  Serial.println("POST DRAIN -- ARM CLOSING");
+  Serial.print("This is switch value: ");
+  Serial.println(mechSwitchResult);
+  while (mechSwitchResult == HIGH) {
+    digitalWrite(DRAIN_MTR, HIGH);
+    delay(PULSE_TIME);
+    digitalWrite(DRAIN_MTR, LOW);
+    delay(DISSIPATE_TIME);
+    mechSwitchResult = digitalRead(MECH_SWT);
+    if (mechSwitchResult == LOW) {
+      Serial.println("THIS IS EVAL TO TRUE");
     }
-
-#ifdef DEBUG
-//    if ((millis() % 1000) == 0) {
-//        Serial.print("This is pressure switch");
-//        Serial.println(digitalRead(PRS_SEN));
-//    }
-#endif
-
-    lastButtonState = reading;
-
-	//	Updates the SM for every loop -- APPLICATION CRITICAL
-    stateMachine.update();
-}
-
-void moveArmIntoDrainOpenState() {
-	digitalWrite(DRAIN_MTR, HIGH);
-    Serial.println("PREPARING TO DRAIN -- ARM MOVING");
-	delay(TIME_FOR_MTR_TO_OPEN_LID);
-	digitalWrite(DRAIN_MTR, LOW);
-}
-
-void closeArmIntoClosedLidState() {
-	//	At switch state ON (When CAM is pushing against
-	//		push button) CAM is in closed state.
-	//		Default value is OFF, or logic high; switch
-	//		uses input pullup.
-	//static unsigned long currentTime = millis();
-	int mechSwitchResult = digitalRead(MECH_SWT);
-    Serial.println("POST DRAIN -- ARM CLOSING");
-    Serial.print("This is switch value: ");
-    Serial.println(mechSwitchResult);
-	while (mechSwitchResult == HIGH) {
-		digitalWrite(DRAIN_MTR, HIGH);
-		delay(PULSE_TIME);
-		digitalWrite(DRAIN_MTR, LOW);
-		delay(DISSIPATE_TIME);
-		mechSwitchResult = digitalRead(MECH_SWT);
-		if (mechSwitchResult == LOW) {
-			Serial.println("THIS IS EVAL TO TRUE");
-		}
-	}
-    Serial.print("This is switch value 2: ");
-    Serial.println(digitalRead(MECH_SWT));
-	digitalWrite(DRAIN_MTR, LOW);
+  }
+  Serial.print("This is switch value 2: ");
+  Serial.println(digitalRead(MECH_SWT));
+  digitalWrite(DRAIN_MTR, LOW);
 }

 void setPixelRingColor(uint32_t val) {
-	for (int i = 0; i < NUM_OF_PIX; i++) {
-		ledStat.setPixelColor(i, val);
-	}
-	ledStat.show();
+  for (int i = 0; i < NUM_OF_PIX; i++) {
+    ledStat.setPixelColor(i, val);
+  }
+  ledStat.show();
 }

 //	Pump is turned off first, pump valve closes, and finally the atm valve
 //		closes. LED status is cleared.
 void idled() {
-	digitalWrite(VACU_PUMP, LOW);
-	digitalWrite(DRAIN_MTR, LOW);
-	digitalWrite(VALV_ATM, LOW);
-
-	setPixelRingColor(idleLEDColor);
+  digitalWrite(VACU_PUMP, LOW);
+  digitalWrite(DRAIN_MTR, LOW);
+  digitalWrite(VALV_ATM, LOW);
+
+  setPixelRingColor(idleLEDColor);
 }

 //	The pump valve opens, then the pump turns on. Atm valve is ensured
 //		to be closed due to necessary safety redundancy. Displays
 //		Yellow on LED status.
 void vacuum() {
-	digitalWrite(DRAIN_MTR, LOW);
-	digitalWrite(VACU_PUMP, HIGH);
-	digitalWrite(VALV_ATM, LOW);
-
-	setPixelRingColor(vacuumLEDColor);
+  digitalWrite(DRAIN_MTR, LOW);
+  digitalWrite(VACU_PUMP, HIGH);
+  digitalWrite(VALV_ATM, LOW);
+
+  setPixelRingColor(vacuumLEDColor);
 }

 //	Keeps track of pressure and transitions to Dwell state if
 //		pressure threshold is met. If time set by setTimeLimit
 //		is met, transition immediately to the Drain state.
 void vacuumUpdate() {
-	//	Keep track of time. Kick into Drain state when time is up.
-	//		Ensures timer has priority over pressure.
-	if ((millis() - startTime) > setTimeLimit) {
-		stateStartTime = millis();
-
-		//	Ensure that next button press in DRAIN state will lead to IDLE
-		buttonPresses = 3;
-
-        moveArmIntoDrainOpenState();
-       	stateMachine.immediateTransitionTo(drin_state);
-   	} else if ((digitalRead(PRS_SEN) == LOW) && ((millis() - stateStartTime) > vacuDebounceLimit)) {
-    	//	If the pressure is at or above the calibrated sensor threshold
-    	//		the pin will read LOW, setting immediate transition to
-    	//		the Dwell state. PRS_SEN is NO.
-        Serial.print("Vacuum Update:  \t");
-        Serial.println((millis() - stateStartTime));
-
-		//  Resets state start time for next state.
-        stateStartTime = millis();
-
-		//	Ensure button kicks into DRAIN state if button is pressed
-		buttonPresses = 2;
-
-    	stateMachine.immediateTransitionTo(dwll_state);
-	}
+  //	Keep track of time. Kick into Drain state when time is up.
+  //		Ensures timer has priority over pressure.
+  if ((millis() - startTime) > setTimeLimit) {
+    stateStartTime = millis();
+
+    //	Ensure that next button press in DRAIN state will lead to IDLE
+    buttonPresses = 3;
+
+    moveArmIntoDrainOpenState();
+    stateMachine.immediateTransitionTo(drin_state);
+  } else if ((digitalRead(PRS_SEN) == LOW) && ((millis() - stateStartTime) > vacuDebounceLimit)) {
+    //	If the pressure is at or above the calibrated sensor threshold
+    //		the pin will read LOW, setting immediate transition to
+    //		the Dwell state. PRS_SEN is NO.
+    Serial.print("Vacuum Update:  \t");
+    Serial.println((millis() - stateStartTime));
+
+    //  Resets state start time for next state.
+    stateStartTime = millis();
+
+    //	Ensure button kicks into DRAIN state if button is pressed
+    buttonPresses = 2;
+
+    stateMachine.immediateTransitionTo(dwll_state);
+  }
 }

 //	Pump is first turned off, then the valve pump is turned off.
 //		Atm valve is ensured to be closed. Displays Red on LED status.
 void dwell() {
-	digitalWrite(VACU_PUMP, LOW);
-	digitalWrite(DRAIN_MTR, LOW);
-   	digitalWrite(VALV_ATM, LOW);
-
-	setPixelRingColor(dwellLEDColor);
+  digitalWrite(VACU_PUMP, LOW);
+  digitalWrite(DRAIN_MTR, LOW);
+  digitalWrite(VALV_ATM, LOW);
+
+  setPixelRingColor(dwellLEDColor);
 }

 //	If pressure fall threshold, SM transitions immediately to Vacuum state.
 //		Similar timer exists as in vacuumUpdate.
 void dwellUpdate() {
-	//	Keep track of time. Kick into Drain state when time is up.
-	//		Ensures timer has priority over pressure.
-	if ((millis() - startTime) > setTimeLimit) {
-		stateStartTime = millis();
-
-		//	Ensure that next button press in DRAIN state will lead to IDLE
-		buttonPresses = 3;
-
-        moveArmIntoDrainOpenState();
-		stateMachine.immediateTransitionTo(drin_state);
-	} else if ((digitalRead(PRS_SEN) == HIGH) && ((millis() - stateStartTime) > stateDebounceLimit)) {
-    	//	If pressure is below set pressure threshold, return to
-    	//		Vacuum state to repressurize.
-        Serial.print("Dwell Update: \t");
-        Serial.println((millis() - stateStartTime));
-
-        stateStartTime = millis();
-		stateMachine.immediateTransitionTo(vacu_state);
-	}
+  //	Keep track of time. Kick into Drain state when time is up.
+  //		Ensures timer has priority over pressure.
+  if ((millis() - startTime) > setTimeLimit) {
+    stateStartTime = millis();
+
+    //	Ensure that next button press in DRAIN state will lead to IDLE
+    buttonPresses = 3;
+
+    moveArmIntoDrainOpenState();
+    stateMachine.immediateTransitionTo(drin_state);
+  } else if ((digitalRead(PRS_SEN) == HIGH) && ((millis() - stateStartTime) > stateDebounceLimit)) {
+    //	If pressure is below set pressure threshold, return to
+    //		Vacuum state to repressurize.
+    Serial.print("Dwell Update: \t");
+    Serial.println((millis() - stateStartTime));
+
+    stateStartTime = millis();
+    stateMachine.immediateTransitionTo(vacu_state);
+  }
 }

 //	Pump is turned off first, then the pump valve closes. The atm valve
 //		opens, and displays Green as the LED status.
 void drain() {
-	// Close pump valve, turn off pump, and open the atmosphere valve
-	//	to equalize with external pressure.
-	digitalWrite(VACU_PUMP, LOW);
-	//digitalWrite(DRAIN_MTR, LOW);
-	digitalWrite(VALV_ATM, HIGH);
-
-	setPixelRingColor(drainLEDColor);
+  // Close pump valve, turn off pump, and open the atmosphere valve
+  //	to equalize with external pressure.
+  digitalWrite(VACU_PUMP, LOW);
+  //digitalWrite(DRAIN_MTR, LOW);
+  digitalWrite(VALV_ATM, HIGH);
+
+  setPixelRingColor(drainLEDColor);
 }

 void drainUpdate() {
-	if((millis() - stateStartTime) >  drainTimeout) {
-		closeArmIntoClosedLidState();
-		stateMachine.transitionTo(idle_state);
-	}
-}
-
+  if((millis() - stateStartTime) >  drainTimeout) {
+    closeArmIntoClosedLidState();
+    stateMachine.transitionTo(idle_state);
+  }
+}
+


lastest version update

lastest version update


Update example for v2

Update example for v2

--- Photon/Plugins/Fan/examples/fan/fan.ino
+++ Photon/Plugins/Fan/examples/fan/fan.ino
@@ -16,7 +16,7 @@
 All text above must be included in any redistribution.

 Changelog:
-2017-10-25: Update for IoT v1.2.0. Add off behavior.
+2017-10-25: Update for IoT v2. Add off behavior.
 2017-05-19: Create fan plugin library
 ********************************************************/
 #include <IoT.h>
@@ -26,7 +26,6 @@

 void setup() {
     iot = IoT::getInstance();
-    iot->setControllerName("myPhoton");
     iot->begin();

     // Create device


v4.3 Add day of week

v4.3 Add day of week

--- WeatherDisplay.ino
+++ WeatherDisplay.ino
@@ -243,7 +243,8 @@
                  setI,
                  idString.toInt(),
                  dateS,
-                 timeUTC);
+                 timeUTC,
+                 dayI);
   }
   else {
     // Display the main weather
@@ -267,7 +268,8 @@
                   int set,
                   int weatherID,
                   String dateS,
-                  String timeUTC)
+                  String timeUTC,
+                  int dayI)
 {
   static long lastRandomNumber;
   long randomColor;
@@ -291,10 +293,10 @@
   }
   lastRandomNumber = randomColor;
   if (randomColor == 0) {
-    anotherRandomColor = 1;
+    anotherRandomColor = randomColor + 1;
   }
   else if (randomColor == 6) {
-    anotherRandomColor = 5;
+    anotherRandomColor = randomColor - 1;
   }
   else {
     anotherRandomColor = randomColor + 1;
@@ -335,6 +337,7 @@
   tft.print("Sunset: ");
   tft.print(set);
   tft.println(":00pm");
+  tft.println("Today is " + getDayOfWeek(dayI + 1));
   tft.println((night) ? "It is Night time" : "It is Day time");
 }

@@ -881,6 +884,25 @@
       if (*month > 12) *year += 1;
     } // if (*day > DaysAMonth[*month - 1])
   } // if (round((lon / 15.0d)) < 0.0d)
+}
+
+// Get the day of the week
+String getDayOfWeek(uint8_t day)
+{
+  String DOW;
+  if (day == 7) day = 0;
+  switch (day)
+  {
+    case 0:  DOW = "Monday"; break;
+    case 1:  DOW = "Tuesday"; break;
+    case 2:  DOW = "Wensday"; break;
+    case 3:  DOW = "Thursday"; break;
+    case 4:  DOW = "Friday"; break;
+    case 5:  DOW = "Saturday" ; break;
+    case 6:  DOW = "Sunday"; break;
+    default: DOW = "Error" ;
+  }
+  return (DOW);
 }

   /*


v4.7 Improve connection to web site

v4.7 Improve connection to web site

--- WeatherDisplay.ino
+++ WeatherDisplay.ino
@@ -146,8 +146,8 @@
   Serial.println("Waiting for data");
 #endif

-  while (client.connected() || client.available()) { //connected or data available
-    char c = client.read(); //gets byte from ethernet buffer
+  while (client.connected() && client.available()) { // While connected
+    char c = client.read(); // Gets byte from ethernet buffer
     result = result + c;
   }


change error cumulator since the direction of compass reading angle is reversed

--- arduino_motor/mains/ErrorCumulator.ino
+++ arduino_motor/mains/ErrorCumulator.ino
@@ -35,6 +35,7 @@
 **/
 void ErrorCumulator::record_turning_error_compass(double adjusted_target_angle) {
   double actual_angle = this->compass->getHeading() - this->compass_reading_0;
+  actual_angle = -actual_angle; // the direction of compass reading angle is reversed
   this->turning_error += (actual_angle - adjusted_target_angle);
 }


Changed the default temperature (fixes calibration issues related to the placement of the temp sensor) and updated the refresh timer to 60 seconds to avoid the on/off 10 second interval annoyance.

--- Arduino-SDK-and-IBM-IoTF/node_temp_w_bluemix/node_temp_w_bluemix.ino
+++ Arduino-SDK-and-IBM-IoTF/node_temp_w_bluemix/node_temp_w_bluemix.ino
@@ -22,7 +22,7 @@

 /* Values will want (temp till we can push from IoTF) */
 boolean HEATER_ON = true;
-float DESIRED_TEMP = 21.6;
+float DESIRED_TEMP = 21.65;
 int ON_POSITION = 153;
 int OFF_POSITION = 122;
 int SERVO_PIN = 5; // D1 (NodeMCU) / GPIO 5 (ESP8266)
@@ -120,7 +120,7 @@
   }

   /* Delay before repeating the process */
-  delay(10000); // 1000 == one second
+  delay(60000); // 1000 == one second
 }

 boolean toggleHeater(float tempDesired, float tempActual, boolean heat) {


Only allow power on if current speed is 0

--- code/MotorController/MotorController.ino
+++ code/MotorController/MotorController.ino
@@ -110,7 +110,7 @@
 void loop()
 {
   onOffSwitch.update();
-  if (onOffSwitch.fallingEdge())
+  if (onOffSwitch.fallingEdge() && CanChangePowerState())
   {
     powerEnabled = !powerEnabled;
   }
@@ -165,6 +165,13 @@
   delay(delayBetweenAdjustments);
 }

+boolean CanChangePowerState()
+{
+    // We can always turn off immediately, but we don't want to turn on unless
+    // the current speed is zero, i.e. we always complete a stop command.
+    return powerEnabled || (!powerEnabled && currentSpeed == 0);
+}
+
 int ReadPotentiometer(int pin)
 {
   int value = analogRead(pin);


Convert periodic tasks to use ArduinoThread library

--- code/MotorController/MotorController.ino
+++ code/MotorController/MotorController.ino
@@ -1,3 +1,7 @@
+// Installed via library manager, https://github.com/ivanseidel/ArduinoThread
+#include <Thread.h>
+#include <ThreadController.h>
+
 // Installed via library manager, https://github.com/thomasfredericks/Bounce2
 #include <Bounce2.h>

@@ -48,16 +52,19 @@
 int targetSpeed = 0;
 int currentSpeed = 0;
 int delayBetweenMotorUpdates = 15;
-unsigned long lastMotorUpdateTime = 0;
 boolean powerEnabled = false;
 int targetDirection = CLOCKWISE;
 int currentDirection = CLOCKWISE;
 volatile int motorTicks = 0;
-unsigned long lastClockTickTime = 0;
-unsigned long lastDisplayTime = 0;
 unsigned long secondsOfOperation = 0;
 uint32_t* nextClockBufferLocation = 0;
 boolean showDebugDisplay = false;
+unsigned long lastDisplayTime = 0;
+
+Thread motorUpdateTask = Thread();
+Thread displayUpdateTask = Thread();
+Thread clockUpdateTask = Thread();
+ThreadController taskController = ThreadController();

 void setup()
 {
@@ -114,16 +121,27 @@
   lcd.print(versionString);
   delay(2000);
   lcd.clear();
+
+  motorUpdateTask.setInterval(delayBetweenMotorUpdates);
+  motorUpdateTask.onRun(SetMotorState);
+  motorUpdateTask.enabled = true;
+  taskController.add(&motorUpdateTask);
+
+  displayUpdateTask.setInterval(1000);
+  displayUpdateTask.onRun(UpdateDisplay);
+  displayUpdateTask.enabled = true;
+  taskController.add(&displayUpdateTask);
+
+  clockUpdateTask.setInterval(1000);
+  clockUpdateTask.onRun(UpdateClock);
+  clockUpdateTask.enabled = true;
+  taskController.add(&clockUpdateTask);
 }

 void loop()
 {
   HandleInputs();
-
-  SetMotorState();
-
-  UpdateClock();
-  UpdateDisplay();
+  taskController.run();
 }

 void HandleInputs()
@@ -168,8 +186,8 @@
     targetSpeed = 0;
   }

-  delayBetweenMotorUpdates = ReadPotentiometer(rateOfSpeedChangeInPin);
-  delayBetweenMotorUpdates = map(delayBetweenMotorUpdates, 0, potentiometerCeiling, minimumDelay, maximumDelay);
+  int newDelayBetweenMotorUpdates = ReadPotentiometer(rateOfSpeedChangeInPin);
+  newDelayBetweenMotorUpdates = map(newDelayBetweenMotorUpdates, 0, potentiometerCeiling, minimumDelay, maximumDelay);
   int absoluteCurrentSpeed = abs(currentSpeed);
   if (absoluteCurrentSpeed > abs(targetSpeed) && absoluteCurrentSpeed < startOfStopDamping)
   {
@@ -177,34 +195,32 @@
     // doesn't work as well at low speed and we don't the bobbin to run ahead of
     // the flyer. So at the low end we start raising the delay toward max regardless
     // of what it's actually set to.
-    delayBetweenMotorUpdates = map(absoluteCurrentSpeed, minimumSpeed, startOfStopDamping, maximumDelay, delayBetweenMotorUpdates);
+    newDelayBetweenMotorUpdates = map(absoluteCurrentSpeed, minimumSpeed, startOfStopDamping, maximumDelay, newDelayBetweenMotorUpdates);
+  }
+
+  if (newDelayBetweenMotorUpdates != delayBetweenMotorUpdates)
+  {
+    delayBetweenMotorUpdates = newDelayBetweenMotorUpdates;
+    motorUpdateTask.setInterval(delayBetweenMotorUpdates);
   }
 }

 void SetMotorState()
 {
-  unsigned long now = millis();
-  unsigned int timeSinceLastMotorUpdate = now - lastMotorUpdateTime;
-
-  if (timeSinceLastMotorUpdate > delayBetweenMotorUpdates)
-  {
-    lastMotorUpdateTime = now;
-
-    if (currentSpeed == 0 && currentDirection != targetDirection)
-    {
-      // We're at zero speed and want to switch directions.
-      digitalWrite(directionOutPin, targetDirection);
-      currentDirection = targetDirection;
-      UpdateDirectionDisplay();
-
-      // We don't want to actually start moving the other direction,
-      // wait for the user to hit the power button again
-      powerEnabled = false;
-    }
+  if (currentSpeed == 0 && currentDirection != targetDirection)
+  {
+    // We're at zero speed and want to switch directions.
+    digitalWrite(directionOutPin, targetDirection);
+    currentDirection = targetDirection;
+    UpdateDirectionDisplay();

-    currentSpeed += SlewToward(currentSpeed, targetSpeed);
-    analogWrite(speedOutPin, abs(currentSpeed));
-  }
+    // We don't want to actually start moving the other direction,
+    // wait for the user to hit the power button again
+    powerEnabled = false;
+  }
+
+  currentSpeed += SlewToward(currentSpeed, targetSpeed);
+  analogWrite(speedOutPin, abs(currentSpeed));
 }

 boolean CanChangePowerState()
@@ -253,57 +269,43 @@

 void UpdateClock()
 {
-  unsigned long now = millis();
-  unsigned int timeSinceLastClockTick = now - lastClockTickTime;
-
-  if (timeSinceLastClockTick > 1000)
-  {
-    lastClockTickTime = now;
+  if (powerEnabled)
+  {
+    secondsOfOperation++;

-    if (powerEnabled)
-    {
-      secondsOfOperation++;
-
-      if (secondsOfOperation % 60 == 0)
-      {
-        WriteClock();
-      }
+    if (secondsOfOperation % 60 == 0)
+    {
+      WriteClock();
     }
   }
 }

 void UpdateDisplay()
 {
-  unsigned long now = millis();
-  unsigned int timeSinceLastDisplay = now - lastDisplayTime;
-
   // TODO: To avoid mistakes, maybe we should have a buffer that
   // contains boilerplate text for the whole display, then write
   // data to it in the appropriate places, and write the whole
   // buffer to the display.

-  if (timeSinceLastDisplay > 1000)
-  {
-    lastDisplayTime = now;
-
-    if (showDebugDisplay)
-    {
-      UpdateDebugDisplay();
-    }
-    else
-    {
-      UpdateRpmDisplay(now, timeSinceLastDisplay);
-      UpdateDirectionDisplay();
-      UpdateHoursDisplay();
-    }
-  }
-}
-
-void UpdateRpmDisplay(unsigned long now, unsigned int timeSinceLastDisplay)
-{
+  if (showDebugDisplay)
+  {
+    UpdateDebugDisplay();
+  }
+  else
+  {
+    UpdateRpmDisplay();
+    UpdateDirectionDisplay();
+    UpdateHoursDisplay();
+  }
+}
+
+void UpdateRpmDisplay()
+{
+  unsigned long now = millis();
   int totalMotorTicks = motorTicks;
   motorTicks = 0;
-
+  unsigned int timeSinceLastDisplay = now - lastDisplayTime;
+
   // We get one pulse every 90 deg. of rotation
   float rpm = totalMotorTicks * 60 / 4.0 * (1000.0 / timeSinceLastDisplay);
   rpm /= gearRatio;
@@ -312,6 +314,8 @@
   lcd.home();
   lcd.print(F("RPM: "));
   lcd.print(rpmToString((int)rpm, buffer));
+
+  lastDisplayTime = now;
 }

 void UpdateDirectionDisplay()


Update bb-8-Head-stabiliser-controller.ino

--- Head/Stabiliser/Arduino-firmware/bb-8-Head-stabiliser-controller.ino
+++ Head/Stabiliser/Arduino-firmware/bb-8-Head-stabiliser-controller.ino
@@ -26,6 +26,8 @@

   Big thanks for http://r2builders.fr/ !
+  Initial project : http://r2builders.fr/forum/viewtopic.php?f=26&t=3928&hilit=BB8+par+MOUS
+
 */


Delete C_CDETPO-Arduino.ino

Delete C_CDETPO-Arduino.ino

--- BMS_Francais/Examples/C_CDETPO-Arduino/C_CDETPO-Arduino.ino
+++ BMS_Francais/Examples/C_CDETPO-Arduino/C_CDETPO-Arduino.ino
@@ -1,70 +0,0 @@
-/*-------------------------------------------------------------------------------------------------------------------------------------------*
- * SE2R : C_CDETPO - Fonction de temporisation de commande à l'action et à l'arrêt                                                03/06/2015 *
- *-------------------------------------------------------------------------------------------------------------------------------------------*
- *        C_CDETPO(boolean CDE, boolean DFT, float CPT[], int RES[], int CFG)                                                     03/06/2015 *
- *                CDE : Demande calculée ou donnée par l'utilisateur (peut être le résultat d'une fonction appelée plus haut)                *
- *                DFT : Défaut de l'équipement commandé. True signale un équipement en état normal. Un false signale un défaut               *
- *                CPT : [00] Compteur de temporisation                                                                                *
- *                      [01] Tempo entre le front montant de la demande CDE avant l'action sur la commande                                   *
- *                      [02] Tempo entre le front descendant de la demande CDE avant la désactivation de la commande                         *
- *                      [03] Temps de cycle automate calculé par la fonction                                                                 *
- *                      [04] Mémorisation de la variable système Millis pour le calcul du temps de cycle                                     *
- *                RES : [00] Commande de l'équipement - forcément un tableau de taille au moins 1                                            *
- *                CFG : Commande de forçage.                                                                                *
- *                      <0 : RES maintenu à false                                                                                *
- *                      0  : RES figée dans son dernier état connu                                                                           *
- *                      1  : RES soumise à CDE (à TMA et TAR près) et à DFT                                                                  *
- *                      2  : RES soumise seulement à DFT (comme si CDE était true en permanence)                                             *
- *                      3  : RES  maintenu à true indépendamment de DFT et CDE                                                               *
- *                      >3 : RES maintenu à false                                                                                *
- *-------------------------------------------------------------------------------------------------------------------------------------------*
- * Copyright SE2R under the APACHE Licence                                                                                *
- *-------------------------------------------------------------------------------------------------------------------------------------------*
- * Plate-formes testées : Arduino Leonardo, Arduino Uno                                                                                *
- *-------------------------------------------------------------------------------------------------------------------------------------------*
- * 27/05/2015 : DD, SE2R : Testée et validée sur banc                                                                                *
- * 27/05/2015 : DD, SE2R : Cette fonction nécessite l'utilisation de S_SCANTS pour accéder à la valeur du cycle automate                     *
- * 03/06/2015 : DD, SE2R : Intégration du calcul de temps de cycle, passage de la commande en boolean et simplification du code              *
- * 03/06/2015 : DD, SE2R : Passage en mode void et ajout des commentaires d'appartenance à C_CDETPO                                          *
- *-------------------------------------------------------------------------------------------------------------------------------------------*/
-
-// Déclaration des bibliothèques nécessaires à l'exécution du programme ---------------------------------------------------------------------*
-#include <BMS.h>                         // Biblio   - SE2R - Fonctions BMS
-BMS BMS;
-
-// Déclaration des variables nécessaires à l'exécution du programme -------------------------------------------------------------------------*
-boolean FCO_XXXX;                        // C_CDETPO - Demande de marche de l'équipement
-boolean ED2_DGL_XXXX;                    // C_CDETPO - Etat de la voie de défaut
-boolean FCC_XXXX[1];                     // C_CDETPO - Etat de la commande à transmettre à la sortie
-float VTT_XXXX[5];                       // C_CDETPO - Table de mesure des temporisations
-int VCO_XXXX;                            // C_CDETPO - Commande de forçage, position auto
-void setup() {                           // Initialisation ----------------------------------------------------------------------------------*
-  pinMode(2, INPUT);                     // C_CDETPO - Affectation de la voie 02 en tant qu'entrée de défaut
-  pinMode(3, INPUT);                     // C_CDETPO - Affectation de la voie 03 en tant qu'entrée de commande
-  pinMode(7, OUTPUT);                    // C_CDETPO - Affectation de la voie 07 en tant que sortie
-  VTT_XXXX[1] = 2;                       // C_CDETPO - Paramétrage des temporisations à l'enclenchement
-  VTT_XXXX[2] = 5;                       // C_CDETPO - Paramétrage des temporisations au déclenchement
-  VCO_XXXX = 1;                          // C_CDETPO - Passage en automatique
-  Serial.begin(9600);                    // Port COM - Paramétrage du port série
-}
-void loop() {                            // Exécution ---------------------------------------------------------------------------------------*
-  if (digitalRead(2) == LOW) { ED2_DGL_XXXX = true; } else { ED2_DGL_XXXX = false; } // Entrée de défaut NF (shunt = normal)
-  if (digitalRead(3) == LOW) { FCO_XXXX = true; } else { FCO_XXXX = false; }         // Entrée de demande de marche NO (shunt = demande)
-  BMS.C_CDETPO(FCO_XXXX, ED2_DGL_XXXX, VTT_XXXX, FCC_XXXX, VCO_XXXX);
-  if (FCC_XXXX[0]) { digitalWrite(7, HIGH); } else { digitalWrite(7, LOW); }
-  Serial.print("Demande : ");
-  Serial.print("\t");
-  Serial.print(FCO_XXXX);
-  Serial.print("\t");
-  Serial.print("Defaut : ");
-  Serial.print("\t");
-  Serial.print(ED2_DGL_XXXX);
-  Serial.print("\t");
-  Serial.print("Compteur T : ");
-  Serial.print("\t");
-  Serial.print(VTT_XXXX[0]);
-  Serial.print("\t");
-  Serial.print("Commande : ");
-  Serial.print("\t");
-  Serial.println(FCC_XXXX[0]);
-}


Delete C_TFONCT-Arduino.ino

Delete C_TFONCT-Arduino.ino


It lives!

It lives!

Signed-off-by: Gustavo Olivares <1cdd110637281f42872eea185e948b8fee0e7823@gmail.com>

--- PACMAN/arduino_firmware/ArduinoProMini/dusty_acorn/dusty_acorn.ino
+++ PACMAN/arduino_firmware/ArduinoProMini/dusty_acorn/dusty_acorn.ino
@@ -91,11 +91,12 @@
 	Serial.print(N1000);
 	Serial.print("\t");
 	//Distance from Range Finder
-	Distance=sonar.ping_cm();
+	Distance=(sonar.ping_median(5)/US_ROUNDTRIP_CM);
 	Serial.print(Distance);
 	Serial.print("\t");
 	//Serial.println("Distance Done");
 	//Temperature and RH from DHT22
+        int chk = DHT.read22(DHT22_PIN);
 	Serial.print(DHT.temperature);
 	Serial.print("\t");
 	Serial.print(DHT.humidity);
@@ -121,10 +122,10 @@
 }
 void loop(){
 	timer1 = millis();
-	if  ((timer1 - timer0) > 1000) {
-		timer0 = timer1;
+//	if  ((timer1 - timer0) > 1000) {
+//		timer0 = timer1;
 		SendData();
 		if (dustport.available())
 			Serial.readBytes((char *)readbuffer,64);
-	}
+//	}
 }