SensorCog.cpp

#include "SensorCog.h"

void sensorCog(){
	Sensor* sensorLeft = k9->sensorLeft;
	Sensor* sensorRight = k9->sensorRight;
	Sensor* sensorFront = k9->sensorFront;

	while(!shutdown){
		// Update sensors with new measurement, only if sensor is not locked
		if(!sensorLeft->getThreadSafeLock())
			sensorLeft->update(ping_cm(PIN_PING_SENSOR_LEFT));

		if(!sensorRight->getThreadSafeLock())
			sensorRight->update(ping_cm(PIN_PING_SENSOR_RIGHT));

		if(!sensorFront->getThreadSafeLock())
			sensorFront->update(ping_cm(PIN_PING_SENSOR_FRONT));

		// Check if the last measurement of the left and right sensor is deviating
		// If so, wait till the main core has taken care of it
		if(sensorLeft->isLastMeasurementDeviating())
			sensorLeft->setThreadSafeLock(true);
		if(sensorRight->isLastMeasurementDeviating())
			sensorRight->setThreadSafeLock(true);


		// Sensor helper stuff
	}
}