Progress

CapacitiveTouch.cpp

@@ -0,0 +1,74 @@
+#include "CapacitiveTouch.h"
+
+CapacitiveTouch::CapacitiveTouch():
+HasOut
+	(this){
+	registerInput(place);
+	registerInput(min);
+	registerInput(max);
+
+	place = NO_LOCATION;
+	min = 0;
+	max = 1;
+
+	filter.alpha = 0.96;
+	reference.alpha = 0.98;
+	reference.set(0);
+
+	measuring = false;
+}
+CapacitiveTouch::~CapacitiveTouch(){}
+void CapacitiveTouch::onInternalInputChange(BaseInput &internalInput){
+	if(&internalInput == &place) {
+		int location = place.get();
+
+		frontPin = Bot::locationToFrontPin(location);
+
+		if(frontPin == NO_LOCATION) return;
+
+		// Internal pull up
+		pinMode(PULL_UP_PIN, OUTPUT);
+		digitalWrite(PULL_UP_PIN, HIGH);
+	}
+};
+
+void CapacitiveTouch::update(){
+	if(frontPin == NO_LOCATION) return;
+	if(measuring){
+		if(bool(digitalRead(frontPin)) || Bot::micros > deadlineTime) {
+			int reading = Bot::micros - startTime;
+
+			// Filter the signal
+			upper0.push(reading);
+			upper1.push(upper0.get());
+			upper2.push(upper1.get());
+			upper3.push(upper2.get());
+
+			// Apply 2 low passes and compute the differente
+			filter.push(upper3.get());
+			reference.push(upper3.get());
+
+			//float diff = filter.get() - reference.get();
+			//if(diff < 0) diff = -diff;
+
+			// Map to 0 - 1
+			//float value = Bot::map(diff, reference.get(), QB_CAPACITIVE_SENSOR_MAX_TIME, 0, 1);
+			float value = Bot::map(filter.get(), 0, QB_CAPACITIVE_SENSOR_MAX_TIME, 0, 1);
+
+			out.set(value);
+
+			// Discharge pin
+			pinMode(frontPin, OUTPUT);
+			digitalWrite(frontPin, LOW);
+			deadlineTime += QB_CAPACITIVE_SENSOR_DISCHARGE_TIME;
+			measuring = false;
+		}
+	}
+
+	if(Bot::micros > deadlineTime) {
+		pinMode(frontPin, INPUT);
+		startTime = Bot::micros;
+		deadlineTime = startTime + QB_CAPACITIVE_SENSOR_MAX_TIME;
+		measuring = true;
+	}
+}

CapacitiveTouch.h

@@ -0,0 +1,42 @@
+#ifndef CapacitiveTouch_h_
+#define CapacitiveTouch_h_
+
+#include "CommonNodeIncludes.h"
+
+#define QB_CAPACITIVE_SENSOR_MAX_TIME 1500
+#define QB_CAPACITIVE_SENSOR_DISCHARGE_TIME 1000
+
+class CapacitiveTouch :
+public Node,
+public Updatable,
+public HasOut
+{
+	public:
+
+	CapacitiveTouch();
+	~CapacitiveTouch();
+
+	Input place;
+	Input min;
+	Input max;
+
+	void update();
+
+	protected:
+
+	void onInternalInputChange(BaseInput &internalInput);
+
+	LowPassFilter filter;
+
+	UpperFilter upper0;
+	UpperFilter upper1;
+	UpperFilter upper2;
+	UpperFilter upper3;
+
+	LowPassFilter reference;
+	unsigned int frontPin;
+	unsigned long startTime;
+	unsigned long deadlineTime;
+	bool measuring;
+};
+#endif

Filters.h

@@ -3,6 +3,7 @@
 
 #include "MedianFilter.h"
 #include "LowPassFilter.h"
+#include "LowerFilter.h"
 #include "UpperFilter.h"
 
-#endif
+#endif

LowerFilter.cpp

@@ -0,0 +1,19 @@
+#include "LowerFilter.h"
+#include "Arduino.h"
+
+LowerFilter::LowerFilter(){
+	a = 0;
+	b = 0;
+	c = 0;
+	d = 0;
+	value = 0;
+}
+LowerFilter::~LowerFilter(){}
+void LowerFilter::push(float number){
+	a = b;
+	b = c;
+	c = d;
+	d = number;
+
+	value = min( min( min( a, b ), c ), d );
+}

LowerFilter.h

@@ -0,0 +1,23 @@
+#ifndef LowerFilter_h_
+#define LowerFilter_h_
+
+#include "BaseFilter.h"
+
+class LowerFilter :
+public BaseFilter
+{
+	public:
+
+	LowerFilter();
+	~LowerFilter();
+
+	void push(float number);
+
+	private:
+
+	float a;
+	float b;
+	float c;
+	float d;
+};
+#endif

Quirkbot.h

@@ -15,6 +15,7 @@
 #include "AnalogSensor.h"
 #include "DigitalSensor.h"
 #include "MakeyTouch.h"
+#include "CapacitiveTouch.h"
 #include "SqueezeSensor.h"
 #include "Sonar.h"