RCK Ruđera Boškovića - mobilna / uslužna robotika

Biblioteke - Senzori

Zadatak

Napraviti biblioteku za čitanje senzora.
p

Priprema

Za vježbu je potrebno:

Datoteke

U ovom prikazu ćemo se koristiti datotekama "mrm-board.h" i "mrm-board.cpp", koje se nalaze u biblioteci "mrm-board", te "mrm-lid-can-b.h" i "mrm-lid-can-b.cpp" u biblioteci "mrm-lid-can-b".

Deklaracija reading() u baznoj klasi

class SensorBoard : public Board {
	...

	/** All readings
	@param subsensorNumberInSensor - like a single IR transistor in mrm-ref-can
	@param deviceNumber - Device's ordinal number. Each call of function add() assigns a increasing number to the device, starting with 0.
	@return - analog value
	*/
	virtual uint16_t reading(uint8_t subsensorNumberInSensor, uint8_t deviceNumber = 0){ return 0;}

	...
};
U baznoj klasi za sve senzore, "SenzorBoard", postoji virtualna funkcija "reading()".

Ona je zajednička za sve senzore jer svi imaju neko očitanje vrijednosti.

Funkcija će vraćati 0, ako ova vrijednost neće biti promijenjena u izvedenim klasama.

Deklaracija reading() u izvedenoj klasi

class Mrm_lid_can_b : public SensorBoard
{
	...
	/** Analog readings
	@param receiverNumberInSensor - always 0
	@param deviceNumber - Device's ordinal number. Each call of function add() assigns a increasing number to the device, starting with 0.
	@return - analog value
	*/
	uint16_t reading(uint8_t receiverNumberInSensor, uint8_t deviceNumber = 0);
	...

};
Deklaracija je ponovljena u izvedenoj klasi za lidare, koja se nalazi u "mrm-lid-can-b.h".

Naravno da je cilj pregaziti nulu, koju vraća bazna klasa.

Definicija reading() u izvedenoj klasi

/** Analog readings
@param receiverNumberInSensor - always 0
@param deviceNumber - Device's ordinal number. Each call of function add() assigns a increasing number to the device, starting with 0.
@return - analog value
*/
uint16_t Mrm_lid_can_b::reading(uint8_t receiverNumberInSensor, uint8_t deviceNumber){
	return distance(deviceNumber);
}
Prebacimo se sad u datoteku "mrm-lid-can-b.cpp", gdje ćemo naći definiciju funkcije.

Definicija prebacuje sav rad u funkciju "distance()".

Ne prenosi se parametar koji određuje redni broj podsenzora u senzoru jer ova vrsta lidara mjeri samo jednu vrijednost.

Koristimo ime "distance()" jer je "udaljenost" naziv primjeren našem senzoru.

Kad bismo mjerili temperaturu, koristili bismo drugo ime.

"reading()" je zajedničko za sve senzore, ali imamo pojedinačna imena za svaki, kako bi korisnik lakše našao funkciju.

Osim toga, "reading()" ima u sebi broj podsenzora, što nema smisla za naš senzor. Podsenzor je npr. redni broj fototranzistora u senzoru za liniju, koji ih ima više.

distance() - 1

/** Distance in mm. Warning - the function will take considerable amount of time to execute if sampleCount > 0!
@param deviceNumber - Device's ordinal number. Each call of function add() assigns a increasing number to the device, starting with 0.
@param sampleCount - Number or readings. 40% of the readings, with extreme values, will be discarded and the
				rest will be averaged. Keeps returning 0 till all the sample is read.
				If sampleCount is 0, it will not wait but will just return the last value.
@param sigmaCount - Values outiside sigmaCount sigmas will be filtered out. 1 sigma will leave 68% of the values, 2 sigma 95%, 3 sigma 99.7%.
				Therefore, lower sigma number will remove more errornous readings.
@return - distance in mm
*/
uint16_t Mrm_lid_can_b::distance(uint8_t deviceNumber, uint8_t sampleCount, uint8_t sigmaCount){
	const uint16_t TIMEOUT = 3000;
	if (deviceNumber > nextFree) {
		strcpy(errorMessage, "mrm-lid-can-b doesn't exist");
		return 0;
	}
	...
}
Nađimo, u istoj datoteci, definiciju funkcije "distance()".

Kao i ranije, provjerimo postoji li traženi broj jedinice (adresa).

Ako ne postoji, upišemo poruku greške u odgovarajuću varijablu.

distance() - 2

/** Distance in mm. Warning - the function will take considerable amount of time to execute if sampleCount > 0!
@param deviceNumber - Device's ordinal number. Each call of function add() assigns a increasing number to the device, starting with 0.
@param sampleCount - Number or readings. 40% of the readings, with extreme values, will be discarded and the
				rest will be averaged. Keeps returning 0 till all the sample is read.
				If sampleCount is 0, it will not wait but will just return the last value.
@param sigmaCount - Values outiside sigmaCount sigmas will be filtered out. 1 sigma will leave 68% of the values, 2 sigma 95%, 3 sigma 99.7%.
				Therefore, lower sigma number will remove more errornous readings.
@return - distance in mm
*/
uint16_t Mrm_lid_can_b::distance(uint8_t deviceNumber, uint8_t sampleCount, uint8_t sigmaCount){
	const uint16_t TIMEOUT = 3000;
	if (deviceNumber > nextFree) {
		strcpy(errorMessage, "mrm-lid-can-b doesn't exist");
		return 0;
	}

	if (started(deviceNumber)){
		...
	}
	else
		return 0;
}
Funkcija "started()" javlja je li pokrenuto čitanje na izabranom senzoru.

Čitanje je mjerenje vrijednosti, u našem slučaju udaljenosti.

Sistem je napravljen tako da senzori ništa ne mjere, dok ne dobiju naredbu, kako bi se štedjela energija i smanjio tok podataka po CAN Bus sabirnici.

"started()" će, ako već nije, pokrenuti proces čitanja (slanjem CAN Bus poruke) i nakon toga će senzor kontinuirano mjeriti udaljenost.

Kako mu treba neko vrijeme da se pokrene, nećemo čekati, nego u tom slučaju vraćamo nulu.

Ako prođe dovoljno vremena, svako će sljedeće čitanje vraćati udaljenost.

distance() - 3

/** Distance in mm. Warning - the function will take considerable amount of time to execute if sampleCount > 0!
@param deviceNumber - Device's ordinal number. Each call of function add() assigns a increasing number to the device, starting with 0.
@param sampleCount - Number or readings. 40% of the readings, with extreme values, will be discarded and the
				rest will be averaged. Keeps returning 0 till all the sample is read.
				If sampleCount is 0, it will not wait but will just return the last value.
@param sigmaCount - Values outiside sigmaCount sigmas will be filtered out. 1 sigma will leave 68% of the values, 2 sigma 95%, 3 sigma 99.7%.
				Therefore, lower sigma number will remove more errornous readings.
@return - distance in mm
*/
uint16_t Mrm_lid_can_b::distance(uint8_t deviceNumber, uint8_t sampleCount, uint8_t sigmaCount){
	const uint16_t TIMEOUT = 3000;
	if (deviceNumber > nextFree) {
		strcpy(errorMessage, "mrm-lid-can-b doesn't exist");
		return 0;
	}

	if (started(deviceNumber)){
		if (sampleCount == 0)
			return (*readings)[deviceNumber];
		else{
			...
		}
	}
	else
		return 0;
}
U suprotnom, ako je senzor pokrenut, pogledat ćemo dalje želimo li čitati jednu vrijednost ili uzeti točniju vrijednost iz više čitanja.

Ako je jedna ("sampleCount" je 0), vraćamo posljednje čitanje, iz polja "readings".

"readings" se osvježava u pozadini, kako stižu CAN Bus poruke iz senzora.

Ne bi bilo razumno čekati na senzor jer se ne bi dobila bitnije točnija vrijednost, a rad glavne petlje robota bi se radikalno usporio.

Znači, svi senzori se keširaju i čita se keš.

distance() - 4

/** Distance in mm. Warning - the function will take considerable amount of time to execute if sampleCount > 0!
@param deviceNumber - Device's ordinal number. Each call of function add() assigns a increasing number to the device, starting with 0.
@param sampleCount - Number or readings. 40% of the readings, with extreme values, will be discarded and the
				rest will be averaged. Keeps returning 0 till all the sample is read.
				If sampleCount is 0, it will not wait but will just return the last value.
@param sigmaCount - Values outiside sigmaCount sigmas will be filtered out. 1 sigma will leave 68% of the values, 2 sigma 95%, 3 sigma 99.7%.
				Therefore, lower sigma number will remove more errornous readings.
@return - distance in mm
*/
uint16_t Mrm_lid_can_b::distance(uint8_t deviceNumber, uint8_t sampleCount, uint8_t sigmaCount){
	const uint16_t TIMEOUT = 3000;
	if (deviceNumber > nextFree) {
		strcpy(errorMessage, "mrm-lid-can-b doesn't exist");
		return 0;
	}

	if (started(deviceNumber)){
		if (sampleCount == 0)
			return (*readings)[deviceNumber];
		else{
			uint16_t rds[sampleCount];
			for (uint8_t i = 0; i < sampleCount; i++){
				if (i != 0) // For 2. reading, etc. - force new readout
					(*readings)[deviceNumber] = 0;
				uint32_t ms = millis();
				while ((*readings)[deviceNumber] == 0){
					robotContainer->noLoopWithoutThis();
					if (millis() - ms > TIMEOUT){
						errorCode = 73;
						break;
					}
				}
				rds[i] = (*readings)[deviceNumber];
			}

			...
		}
	}
	else
		return 0;
}
Ako želimo točnu vrijednost, trebat ćemo (puno) više vremena.

Želimo li prisiliti program da čeka sljedeću izmjerenu vrijednost, spremamo 0 u "readings" i čekamo da se nula promijeni u novu vrijednost.

Istu spremamo u "rds", polje koje drži sve vrijednosti, broj kojih je veličina uzorka.

Uočite da ovo može biti jako sporo.

Recimo da senzor čita 30 puta u sekundi (realna pretpostavka) i da želimo čitati svaki put 5 uzastopnih vrijednosti.

Znači da ćemo gubiti svaki put 0.165 seknudi, tj. čitanje jednog senzora će smanjiti frekvenciju glavne petlje na 6 Hz.

Jasno, više senzora će još više pokvariti stvar.

6 Hz je daleko premalo za iole bržeg robota.

distance() - 5

/** Distance in mm. Warning - the function will take considerable amount of time to execute if sampleCount > 0!
@param deviceNumber - Device's ordinal number. Each call of function add() assigns a increasing number to the device, starting with 0.
@param sampleCount - Number or readings. 40% of the readings, with extreme values, will be discarded and the
				rest will be averaged. Keeps returning 0 till all the sample is read.
				If sampleCount is 0, it will not wait but will just return the last value.
@param sigmaCount - Values outiside sigmaCount sigmas will be filtered out. 1 sigma will leave 68% of the values, 2 sigma 95%, 3 sigma 99.7%.
				Therefore, lower sigma number will remove more errornous readings.
@return - distance in mm
*/
uint16_t Mrm_lid_can_b::distance(uint8_t deviceNumber, uint8_t sampleCount, uint8_t sigmaCount){
	const uint16_t TIMEOUT = 3000;
	if (deviceNumber > nextFree) {
		strcpy(errorMessage, "mrm-lid-can-b doesn't exist");
		return 0;
	}

	if (started(deviceNumber)){
		if (sampleCount == 0)
			return (*readings)[deviceNumber];
		else{
			uint16_t rds[sampleCount];
			for (uint8_t i = 0; i < sampleCount; i++){
				if (i != 0) // For 2. reading, etc. - force new readout
					(*readings)[deviceNumber] = 0;
				uint32_t ms = millis();
				while ((*readings)[deviceNumber] == 0){
					robotContainer->noLoopWithoutThis();
					if (millis() - ms > TIMEOUT){
						errorCode = 73;
						break;
					}
				}
				rds[i] = (*readings)[deviceNumber];
			}

			// Average and standard deviation
			float sum = 0.0;
			for(uint8_t i = 0; i < sampleCount; i++)
				sum += rds[i];
			float mean = sum / sampleCount;
			float standardDeviation = 0.0;
			for(int i = 0; i < sampleCount; i++) 
				standardDeviation += pow(rds[i] - mean, 2);
			standardDeviation = sqrt(standardDeviation / sampleCount);

			...
		}
	}
	else
		return 0;
}
Izračunamo srednju vrijednost ("mean") i standardnu devijaciju ("standardDeviation").

distance() - 6

/** Distance in mm. Warning - the function will take considerable amount of time to execute if sampleCount > 0!
@param deviceNumber - Device's ordinal number. Each call of function add() assigns a increasing number to the device, starting with 0.
@param sampleCount - Number or readings. 40% of the readings, with extreme values, will be discarded and the
				rest will be averaged. Keeps returning 0 till all the sample is read.
				If sampleCount is 0, it will not wait but will just return the last value.
@param sigmaCount - Values outiside sigmaCount sigmas will be filtered out. 1 sigma will leave 68% of the values, 2 sigma 95%, 3 sigma 99.7%.
				Therefore, lower sigma number will remove more errornous readings.
@return - distance in mm
*/
uint16_t Mrm_lid_can_b::distance(uint8_t deviceNumber, uint8_t sampleCount, uint8_t sigmaCount){
	const uint16_t TIMEOUT = 3000;
	if (deviceNumber > nextFree) {
		strcpy(errorMessage, "mrm-lid-can-b doesn't exist");
		return 0;
	}

	if (started(deviceNumber)){
		if (sampleCount == 0)
			return (*readings)[deviceNumber];
		else{
			uint16_t rds[sampleCount];
			for (uint8_t i = 0; i < sampleCount; i++){
				if (i != 0) // For 2. reading, etc. - force new readout
					(*readings)[deviceNumber] = 0;
				uint32_t ms = millis();
				while ((*readings)[deviceNumber] == 0){
					robotContainer->noLoopWithoutThis();
					if (millis() - ms > TIMEOUT){
						errorCode = 73;
						break;
					}
				}
				rds[i] = (*readings)[deviceNumber];
			}

			// Average and standard deviation
			float sum = 0.0;
			for(uint8_t i = 0; i < sampleCount; i++)
				sum += rds[i];
			float mean = sum / sampleCount;
			float standardDeviation = 0.0;
			for(int i = 0; i < sampleCount; i++) 
				standardDeviation += pow(rds[i] - mean, 2);
			standardDeviation = sqrt(standardDeviation / sampleCount);

			// Filter out all the values outside n-sigma boundaries and return average value of the rest
			sum = 0;
			uint8_t cnt = 0;
			for (uint8_t i = 0; i < sampleCount; i++)
				if (mean - sigmaCount * standardDeviation < rds[i] && rds[i] < mean + sigmaCount * standardDeviation){
					sum += rds[i];
					cnt++;
				}

			...
		}
	}
	else
		return 0;
}
Zbrajamo sve vrijednosti koje su unutar željenog broja standardnih devijacija oko srednje vrijednosti.

distance() - 7

/** Distance in mm. Warning - the function will take considerable amount of time to execute if sampleCount > 0!
@param deviceNumber - Device's ordinal number. Each call of function add() assigns a increasing number to the device, starting with 0.
@param sampleCount - Number or readings. 40% of the readings, with extreme values, will be discarded and the
				rest will be averaged. Keeps returning 0 till all the sample is read.
				If sampleCount is 0, it will not wait but will just return the last value.
@param sigmaCount - Values outiside sigmaCount sigmas will be filtered out. 1 sigma will leave 68% of the values, 2 sigma 95%, 3 sigma 99.7%.
				Therefore, lower sigma number will remove more errornous readings.
@return - distance in mm
*/
uint16_t Mrm_lid_can_b::distance(uint8_t deviceNumber, uint8_t sampleCount, uint8_t sigmaCount){
	const uint16_t TIMEOUT = 3000;
	if (deviceNumber > nextFree) {
		strcpy(errorMessage, "mrm-lid-can-b doesn't exist");
		return 0;
	}

	if (started(deviceNumber)){
		if (sampleCount == 0)
			return (*readings)[deviceNumber];
		else{
			uint16_t rds[sampleCount];
			for (uint8_t i = 0; i < sampleCount; i++){
				if (i != 0) // For 2. reading, etc. - force new readout
					(*readings)[deviceNumber] = 0;
				uint32_t ms = millis();
				while ((*readings)[deviceNumber] == 0){
					robotContainer->noLoopWithoutThis();
					if (millis() - ms > TIMEOUT){
						errorCode = 73;
						break;
					}
				}
				rds[i] = (*readings)[deviceNumber];
			}

			// Average and standard deviation
			float sum = 0.0;
			for(uint8_t i = 0; i < sampleCount; i++)
				sum += rds[i];
			float mean = sum / sampleCount;
			float standardDeviation = 0.0;
			for(int i = 0; i < sampleCount; i++) 
				standardDeviation += pow(rds[i] - mean, 2);
			standardDeviation = sqrt(standardDeviation / sampleCount);

			// Filter out all the values outside n-sigma boundaries and return average value of the rest
			sum = 0;
			uint8_t cnt = 0;
			for (uint8_t i = 0; i < sampleCount; i++)
				if (mean - sigmaCount * standardDeviation < rds[i] && rds[i] < mean + sigmaCount * standardDeviation){
					sum += rds[i];
					cnt++;
				}

			return (uint16_t)(sum / cnt);
		}
	}
	else
		return 0;
}
Na kraju podijelimo sumu s preostalim uzorkom i vratimo vrijednost kao udaljenost.

Ostale funkcije, bazna klasa

class SensorBoard : public Board {
	...

protected:

	/** Standard deviation
	@param sampleCount - count.
	@param sample - values.
	@param averageValue - output parameter.
	@return - standard deviation.*/
	float stardardDeviation(uint8_t sampleCount, uint16_t sample[], float * averageValue);

	/** Filter out data outliers and return average of the rest
	@param sampleCount - count.
	@param sample - values.
	@param averageValue - average value.
	@param sigmaCount - number of sigmas to keep.
	@param standardDeviation - standard deviation.
	@return average value of the filtered set*/
	float outlierlessAverage(uint8_t sampleCount, uint16_t sample[], float averageValue, uint8_t sigmaCount,
		float standardDeviation);

public:
	...

	/** Starts periodical CANBus messages that will be refreshing values that mirror sensor's calculated values
	@param deviceNumber - Device's ordinal number. Each call of function add() assigns a increasing number to the device, starting with 0.
	*/
	void continuousReadingCalculatedDataStart(uint8_t deviceNumber = 0xFF);

	/** Read CAN Bus message into local variables
	@param canId - CAN Bus id
	@param data - 8 bytes from CAN Bus message.
	@param length - number of data bytes
	@return - true if canId for this class
	*/
	virtual bool messageDecode(uint32_t canId, uint8_t data[8], uint8_t length){return false;}

	/** All readings
	@param subsensorNumberInSensor - like a single IR transistor in mrm-ref-can
	@param deviceNumber - Device's ordinal number. Each call of function add() assigns a increasing number to the device, starting with 0.
	@return - analog value
	*/
	virtual uint16_t reading(uint8_t subsensorNumberInSensor, uint8_t deviceNumber = 0){ return 0;}

	uint8_t readingsCount(){return _readingsCount;}
};
Pronađimo sve funkcije koje su u klasama za ovaj senzor. Potraga će ići u 2 smjera:
  • bazna klasa u "mrm-board.h" i
  • izvedena klasa u "mrm-lid-can-b.h".
Lijevo je bazna klasa.

Ostale funkcije, izvedena klasa

class Mrm_lid_can_b : public SensorBoard
{
	...

	/** If sensor not started, start it and wait for 1. message
	@param deviceNumber - Device's ordinal number. Each call of function add() assigns a increasing number to the device, starting with 0.
	@return - started or not
	*/
	bool started(uint8_t deviceNumber);
	
public:
	
	...

	/** Add a mrm-ref-can sensor
	@param deviceName - device's name
	*/
	void add(char * deviceName = (char*)"");
	
	/** Calibration, only once after production
	@param deviceNumber - Device's ordinal number. Each call of function add() assigns a increasing number to the device, starting with 0.
	*/
	void calibration(uint8_t deviceNumber = 0);

	/** Distance in mm. Warning - the function will take considerable amount of time to execute if sampleCount > 0!
	@param deviceNumber - Device's ordinal number. Each call of function add() assigns a increasing number to the device, starting with 0.
	@param sampleCount - Number or readings. 40% of the c, with extreme values, will be discarded and the
					rest will be averaged. Keeps returning 0 till all the sample is read.
					If sampleCount is 0, it will not wait but will just return the last value.
	@param sigmaCount - Values outiside sigmaCount sigmas will be filtered out. 1 sigma will leave 68% of the values, 2 sigma 95%, 3 sigma 99.7%.
					Therefore, lower sigma number will remove more errornous readings.
	@return - distance in mm
	*/
	uint16_t distance(uint8_t deviceNumber = 0, uint8_t sampleCount = 0, uint8_t sigmaCount = 1);

	/** Read CAN Bus message into local variables
	@param canId - CAN Bus id
	@param data - 8 bytes from CAN Bus message.
	@param length - number of data bytes
	*/
	bool messageDecode(uint32_t canId, uint8_t data[8], uint8_t length);

	/** Enable plug and play
	@param enable - enable or disable
	@param deviceNumber - Device's ordinal number. Each call of function add() assigns a increasing number to the device, starting with 0.
	*/
	void pnpSet(bool enable = true, uint8_t deviceNumber = 0);

	/** Ranging type
	@param deviceNumber - Device's ordinal number. Each call of function add() assigns a increasing number to the device, starting with 0.
	@param value - long range 0, high speed 1, high accuracy 2
	*/
	void rangingType(uint8_t deviceNumber, uint8_t value = 0);

	/** Analog readings
	@param receiverNumberInSensor - always 0
	@param deviceNumber - Device's ordinal number. Each call of function add() assigns a increasing number to the device, starting with 0.
	@return - analog value
	*/
	uint16_t reading(uint8_t receiverNumberInSensor, uint8_t deviceNumber = 0);

	/** Print all readings in a line
	*/
	void readingsPrint();

	/**Test
	@param deviceNumber - Device's ordinal number. Each call of function add() assigns a increasing number to the device, starting with 0. 0xFF - all devices.
	@param betweenTestsMs - time in ms between 2 tests. 0 - default.
	*/
	void test(uint8_t deviceNumber = 0xFF, uint16_t betweenTestsMs = 0);

};
Ovdje je popis funkcija iz izvedene klase.

Korisnik može mijenjati i dodavati funkcije iz postojeće bazne klase "SensorBoard", ali mora paziti da je iz te klase izvedeno niz klasa za senzore pa nisu moguće proizvoljne promjene.

Mijenjanje izvedene klase je znatno lakše.

Također može napraviti i potpuno novu izvedenu klasu. Najlakši je način kopirati postojeći kod za "Mrm_lid_can_b", promijeniti ime klase i sve drugo što je potrebno.

Primjedbe