Abgabe

kawaii/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.7)
+project(kawaii-engine)
+
+add_subdirectory(kawaii-rs)
+
+set(CMAKE_CXX_STANDARD 14)
+
+set(SOURCE_FILES gpio.cpp engine.cpp measure.cpp)
+
+if (MEASURE)
+	add_definitions("-DMEASURE")
+endif (MEASURE)
+
+#add_executable(engine ${SOURCE_FILES} test.cpp ultrasound_sensor.cpp)
+#add_executable(remote ${SOURCE_FILES} remote.cpp)
+add_executable(kawaii ${SOURCE_FILES} main.cpp ultrasound_sensor.cpp rfid_reader.cpp MFRC522.cpp rfid_reader.cpp emergency_stop.cpp)
+
+#add_dependencies(engine kawaii-rs)
+#add_dependencies(remote kawaii-rs)
+add_dependencies(kawaii kawaii-rs)
+
+#target_link_libraries(engine
+#        debug "${CMAKE_SOURCE_DIR}/kawaii-rs/target/debug/libkawaii.a"
+#        optimized "${CMAKE_SOURCE_DIR}/kawaii-rs/target/release/libkawaii.a"
+#        util dl rt pthread gcc_s c m rt pthread util
+#        pthread)
+#target_link_libraries(remote
+#        debug "${CMAKE_SOURCE_DIR}/kawaii-rs/target/debug/libkawaii.a"
+#        optimized "${CMAKE_SOURCE_DIR}/kawaii-rs/target/release/libkawaii.a"
+#        util dl rt pthread gcc_s c m rt pthread util
+#        boost_system pthread)
+target_link_libraries(kawaii
+		debug "${CMAKE_SOURCE_DIR}/kawaii-rs/target/debug/libkawaii.a"
+		optimized "${CMAKE_SOURCE_DIR}/kawaii-rs/target/release/libkawaii.a"
+        util dl rt pthread gcc_s c m rt pthread util
+        pthread bcm2835)

kawaii/MFRC522.h

@@ -0,0 +1,323 @@
+/**
+ * MFRC522.h - Library to use ARDUINO RFID MODULE KIT 13.56 MHZ WITH TAGS SPI W AND R BY COOQROBOT.
+ * Based on code Dr.Leong   ( WWW.B2CQSHOP.COM )
+ * Created by Miguel Balboa (circuitito.com), Jan, 2012.
+ * Rewritten by Søren Thing Andersen (access.thing.dk), fall of 2013 (Translation to English, refactored, comments, anti collision, cascade levels.)
+ * Extended by Tom Clement with functionality to write to sector 0 of UID changeable Mifare cards.
+ * Released into the public domain.
+ * 
+
+
+-- Repurposed to fit Raspberry Pi --- 
+
+ */
+#ifndef MFRC522_h
+#define MFRC522_h
+
+#include <stdint.h>
+#include <stdio.h>
+#include <string>
+using namespace std;
+
+typedef uint8_t byte;
+typedef uint16_t word;
+
+// Firmware data for self-test
+// Reference values based on firmware version; taken from 16.1.1 in spec.
+// Version 1.0
+
+const byte MFRC522_firmware_referenceV1_0[]  = {
+	0x00, 0xC6, 0x37, 0xD5, 0x32, 0xB7, 0x57, 0x5C,
+	0xC2, 0xD8, 0x7C, 0x4D, 0xD9, 0x70, 0xC7, 0x73,
+	0x10, 0xE6, 0xD2, 0xAA, 0x5E, 0xA1, 0x3E, 0x5A,
+	0x14, 0xAF, 0x30, 0x61, 0xC9, 0x70, 0xDB, 0x2E,
+	0x64, 0x22, 0x72, 0xB5, 0xBD, 0x65, 0xF4, 0xEC,
+	0x22, 0xBC, 0xD3, 0x72, 0x35, 0xCD, 0xAA, 0x41,
+	0x1F, 0xA7, 0xF3, 0x53, 0x14, 0xDE, 0x7E, 0x02,
+	0xD9, 0x0F, 0xB5, 0x5E, 0x25, 0x1D, 0x29, 0x79
+};
+
+// Version 2.0
+const byte MFRC522_firmware_referenceV2_0[] = {
+	0x00, 0xEB, 0x66, 0xBA, 0x57, 0xBF, 0x23, 0x95,
+	0xD0, 0xE3, 0x0D, 0x3D, 0x27, 0x89, 0x5C, 0xDE,
+	0x9D, 0x3B, 0xA7, 0x00, 0x21, 0x5B, 0x89, 0x82,
+	0x51, 0x3A, 0xEB, 0x02, 0x0C, 0xA5, 0x00, 0x49,
+	0x7C, 0x84, 0x4D, 0xB3, 0xCC, 0xD2, 0x1B, 0x81,
+	0x5D, 0x48, 0x76, 0xD5, 0x71, 0x61, 0x21, 0xA9,
+	0x86, 0x96, 0x83, 0x38, 0xCF, 0x9D, 0x5B, 0x6D,
+	0xDC, 0x15, 0xBA, 0x3E, 0x7D, 0x95, 0x3B, 0x2F
+};
+
+
+
+class MFRC522 {
+public:
+	// MFRC522 registers. Described in chapter 9 of the datasheet.
+	// When using SPI all addresses are shifted one bit left in the "SPI address byte" (section 8.1.2.3)
+	enum PCD_Register {
+		// Page 0: Command and status
+		//						  0x00			// reserved for future use
+		CommandReg				= 0x01 << 1,	// starts and stops command execution
+		ComIEnReg				= 0x02 << 1,	// enable and disable interrupt request control bits
+		DivIEnReg				= 0x03 << 1,	// enable and disable interrupt request control bits
+		ComIrqReg				= 0x04 << 1,	// interrupt request bits
+		DivIrqReg				= 0x05 << 1,	// interrupt request bits
+		ErrorReg				= 0x06 << 1,	// error bits showing the error status of the last command executed 
+		Status1Reg				= 0x07 << 1,	// communication status bits
+		Status2Reg				= 0x08 << 1,	// receiver and transmitter status bits
+		FIFODataReg				= 0x09 << 1,	// input and output of 64 byte FIFO buffer
+		FIFOLevelReg			= 0x0A << 1,	// number of bytes stored in the FIFO buffer
+		WaterLevelReg			= 0x0B << 1,	// level for FIFO underflow and overflow warning
+		ControlReg				= 0x0C << 1,	// miscellaneous control registers
+		BitFramingReg			= 0x0D << 1,	// adjustments for bit-oriented frames
+		CollReg					= 0x0E << 1,	// bit position of the first bit-collision detected on the RF interface
+		//						  0x0F			// reserved for future use
+		
+		// Page 1: Command
+		// 						  0x10			// reserved for future use
+		ModeReg					= 0x11 << 1,	// defines general modes for transmitting and receiving 
+		TxModeReg				= 0x12 << 1,	// defines transmission data rate and framing
+		RxModeReg				= 0x13 << 1,	// defines reception data rate and framing
+		TxControlReg			= 0x14 << 1,	// controls the logical behavior of the antenna driver pins TX1 and TX2
+		TxASKReg				= 0x15 << 1,	// controls the setting of the transmission modulation
+		TxSelReg				= 0x16 << 1,	// selects the internal sources for the antenna driver
+		RxSelReg				= 0x17 << 1,	// selects internal receiver settings
+		RxThresholdReg			= 0x18 << 1,	// selects thresholds for the bit decoder
+		DemodReg				= 0x19 << 1,	// defines demodulator settings
+		// 						  0x1A			// reserved for future use
+		// 						  0x1B			// reserved for future use
+		MfTxReg					= 0x1C << 1,	// controls some MIFARE communication transmit parameters
+		MfRxReg					= 0x1D << 1,	// controls some MIFARE communication receive parameters
+		// 						  0x1E			// reserved for future use
+		SerialSpeedReg			= 0x1F << 1,	// selects the speed of the serial UART interface
+		
+		// Page 2: Configuration
+		// 						  0x20			// reserved for future use
+		CRCResultRegH			= 0x21 << 1,	// shows the MSB and LSB values of the CRC calculation
+		CRCResultRegL			= 0x22 << 1,
+		// 						  0x23			// reserved for future use
+		ModWidthReg				= 0x24 << 1,	// controls the ModWidth setting?
+		// 						  0x25			// reserved for future use
+		RFCfgReg				= 0x26 << 1,	// configures the receiver gain
+		GsNReg					= 0x27 << 1,	// selects the conductance of the antenna driver pins TX1 and TX2 for modulation 
+		CWGsPReg				= 0x28 << 1,	// defines the conductance of the p-driver output during periods of no modulation
+		ModGsPReg				= 0x29 << 1,	// defines the conductance of the p-driver output during periods of modulation
+		TModeReg				= 0x2A << 1,	// defines settings for the internal timer
+		TPrescalerReg			= 0x2B << 1,	// the lower 8 bits of the TPrescaler value. The 4 high bits are in TModeReg.
+		TReloadRegH				= 0x2C << 1,	// defines the 16-bit timer reload value
+		TReloadRegL				= 0x2D << 1,
+		TCounterValueRegH		= 0x2E << 1,	// shows the 16-bit timer value
+		TCounterValueRegL		= 0x2F << 1,
+		
+		// Page 3: Test Registers
+		// 						  0x30			// reserved for future use
+		TestSel1Reg				= 0x31 << 1,	// general test signal configuration
+		TestSel2Reg				= 0x32 << 1,	// general test signal configuration
+		TestPinEnReg			= 0x33 << 1,	// enables pin output driver on pins D1 to D7
+		TestPinValueReg			= 0x34 << 1,	// defines the values for D1 to D7 when it is used as an I/O bus
+		TestBusReg				= 0x35 << 1,	// shows the status of the internal test bus
+		AutoTestReg				= 0x36 << 1,	// controls the digital self test
+		VersionReg				= 0x37 << 1,	// shows the software version
+		AnalogTestReg			= 0x38 << 1,	// controls the pins AUX1 and AUX2
+		TestDAC1Reg				= 0x39 << 1,	// defines the test value for TestDAC1
+		TestDAC2Reg				= 0x3A << 1,	// defines the test value for TestDAC2
+		TestADCReg				= 0x3B << 1		// shows the value of ADC I and Q channels
+		// 						  0x3C			// reserved for production tests
+		// 						  0x3D			// reserved for production tests
+		// 						  0x3E			// reserved for production tests
+		// 						  0x3F			// reserved for production tests
+	};
+	
+	// MFRC522 commands. Described in chapter 10 of the datasheet.
+	enum PCD_Command {
+		PCD_Idle				= 0x00,		// no action, cancels current command execution
+		PCD_Mem					= 0x01,		// stores 25 bytes into the internal buffer
+		PCD_GenerateRandomID	= 0x02,		// generates a 10-byte random ID number
+		PCD_CalcCRC				= 0x03,		// activates the CRC coprocessor or performs a self test
+		PCD_Transmit			= 0x04,		// transmits data from the FIFO buffer
+		PCD_NoCmdChange			= 0x07,		// no command change, can be used to modify the CommandReg register bits without affecting the command, for example, the PowerDown bit
+		PCD_Receive				= 0x08,		// activates the receiver circuits
+		PCD_Transceive 			= 0x0C,		// transmits data from FIFO buffer to antenna and automatically activates the receiver after transmission
+		PCD_MFAuthent 			= 0x0E,		// performs the MIFARE standard authentication as a reader
+		PCD_SoftReset			= 0x0F		// resets the MFRC522
+	};
+	
+	// MFRC522 RxGain[2:0] masks, defines the receiver's signal voltage gain factor (on the PCD).
+	// Described in 9.3.3.6 / table 98 of the datasheet at http://www.nxp.com/documents/data_sheet/MFRC522.pdf
+	enum PCD_RxGain {
+		RxGain_18dB				= 0x00 << 4,	// 000b - 18 dB, minimum
+		RxGain_23dB				= 0x01 << 4,	// 001b - 23 dB
+		RxGain_18dB_2			= 0x02 << 4,	// 010b - 18 dB, it seems 010b is a duplicate for 000b
+		RxGain_23dB_2			= 0x03 << 4,	// 011b - 23 dB, it seems 011b is a duplicate for 001b
+		RxGain_33dB				= 0x04 << 4,	// 100b - 33 dB, average, and typical default
+		RxGain_38dB				= 0x05 << 4,	// 101b - 38 dB
+		RxGain_43dB				= 0x06 << 4,	// 110b - 43 dB
+		RxGain_48dB				= 0x07 << 4,	// 111b - 48 dB, maximum
+		RxGain_min				= 0x00 << 4,	// 000b - 18 dB, minimum, convenience for RxGain_18dB
+		RxGain_avg				= 0x04 << 4,	// 100b - 33 dB, average, convenience for RxGain_33dB
+		RxGain_max				= 0x07 << 4		// 111b - 48 dB, maximum, convenience for RxGain_48dB
+	};
+	
+	// Commands sent to the PICC.
+	enum PICC_Command {
+		// The commands used by the PCD to manage communication with several PICCs (ISO 14443-3, Type A, section 6.4)
+		PICC_CMD_REQA			= 0x26,		// REQuest command, Type A. Invites PICCs in state IDLE to go to READY and prepare for anticollision or selection. 7 bit frame.
+		PICC_CMD_WUPA			= 0x52,		// Wake-UP command, Type A. Invites PICCs in state IDLE and HALT to go to READY(*) and prepare for anticollision or selection. 7 bit frame.
+		PICC_CMD_CT				= 0x88,		// Cascade Tag. Not really a command, but used during anti collision.
+		PICC_CMD_SEL_CL1		= 0x93,		// Anti collision/Select, Cascade Level 1
+		PICC_CMD_SEL_CL2		= 0x95,		// Anti collision/Select, Cascade Level 2
+		PICC_CMD_SEL_CL3		= 0x97,		// Anti collision/Select, Cascade Level 3
+		PICC_CMD_HLTA			= 0x50,		// HaLT command, Type A. Instructs an ACTIVE PICC to go to state HALT.
+		// The commands used for MIFARE Classic (from http://www.nxp.com/documents/data_sheet/MF1S503x.pdf, Section 9)
+		// Use PCD_MFAuthent to authenticate access to a sector, then use these commands to read/write/modify the blocks on the sector.
+		// The read/write commands can also be used for MIFARE Ultralight.
+		PICC_CMD_MF_AUTH_KEY_A	= 0x60,		// Perform authentication with Key A
+		PICC_CMD_MF_AUTH_KEY_B	= 0x61,		// Perform authentication with Key B
+		PICC_CMD_MF_READ		= 0x30,		// Reads one 16 byte block from the authenticated sector of the PICC. Also used for MIFARE Ultralight.
+		PICC_CMD_MF_WRITE		= 0xA0,		// Writes one 16 byte block to the authenticated sector of the PICC. Called "COMPATIBILITY WRITE" for MIFARE Ultralight.
+		PICC_CMD_MF_DECREMENT	= 0xC0,		// Decrements the contents of a block and stores the result in the internal data register.
+		PICC_CMD_MF_INCREMENT	= 0xC1,		// Increments the contents of a block and stores the result in the internal data register.
+		PICC_CMD_MF_RESTORE		= 0xC2,		// Reads the contents of a block into the internal data register.
+		PICC_CMD_MF_TRANSFER	= 0xB0,		// Writes the contents of the internal data register to a block.
+		// The commands used for MIFARE Ultralight (from http://www.nxp.com/documents/data_sheet/MF0ICU1.pdf, Section 8.6)
+		// The PICC_CMD_MF_READ and PICC_CMD_MF_WRITE can also be used for MIFARE Ultralight.
+		PICC_CMD_UL_WRITE		= 0xA2		// Writes one 4 byte page to the PICC.
+	};
+	
+	// MIFARE constants that does not fit anywhere else
+	enum MIFARE_Misc {
+		MF_ACK					= 0xA,		// The MIFARE Classic uses a 4 bit ACK/NAK. Any other value than 0xA is NAK.
+		MF_KEY_SIZE				= 6			// A Mifare Crypto1 key is 6 bytes.
+	};
+	
+	// PICC types we can detect. Remember to update PICC_GetTypeName() if you add more.
+	enum PICC_Type {
+		PICC_TYPE_UNKNOWN		= 0,
+		PICC_TYPE_ISO_14443_4	= 1,	// PICC compliant with ISO/IEC 14443-4 
+		PICC_TYPE_ISO_18092		= 2, 	// PICC compliant with ISO/IEC 18092 (NFC)
+		PICC_TYPE_MIFARE_MINI	= 3,	// MIFARE Classic protocol, 320 bytes
+		PICC_TYPE_MIFARE_1K		= 4,	// MIFARE Classic protocol, 1KB
+		PICC_TYPE_MIFARE_4K		= 5,	// MIFARE Classic protocol, 4KB
+		PICC_TYPE_MIFARE_UL		= 6,	// MIFARE Ultralight or Ultralight C
+		PICC_TYPE_MIFARE_PLUS	= 7,	// MIFARE Plus
+		PICC_TYPE_TNP3XXX		= 8,	// Only mentioned in NXP AN 10833 MIFARE Type Identification Procedure
+		PICC_TYPE_NOT_COMPLETE	= 255	// SAK indicates UID is not complete.
+	};
+	
+	// Return codes from the functions in this class. Remember to update GetStatusCodeName() if you add more.
+	enum StatusCode {
+		STATUS_OK				= 1,	// Success
+		STATUS_ERROR			= 2,	// Error in communication
+		STATUS_COLLISION		= 3,	// Collission detected
+		STATUS_TIMEOUT			= 4,	// Timeout in communication.
+		STATUS_NO_ROOM			= 5,	// A buffer is not big enough.
+		STATUS_INTERNAL_ERROR	= 6,	// Internal error in the code. Should not happen ;-)
+		STATUS_INVALID			= 7,	// Invalid argument.
+		STATUS_CRC_WRONG		= 8,	// The CRC_A does not match
+		STATUS_MIFARE_NACK		= 9		// A MIFARE PICC responded with NAK.
+	};
+	
+	// A struct used for passing the UID of a PICC.
+	typedef struct {
+		byte		size;			// Number of bytes in the UID. 4, 7 or 10.
+		byte		uidByte[10];
+		byte		sak;			// The SAK (Select acknowledge) byte returned from the PICC after successful selection.
+	} Uid;
+	
+	// A struct used for passing a MIFARE Crypto1 key
+	typedef struct {
+		byte		keyByte[MF_KEY_SIZE];
+	} MIFARE_Key;
+	
+	// Member variables
+	Uid uid;								// Used by PICC_ReadCardSerial().
+	
+	// Size of the MFRC522 FIFO
+	static const byte FIFO_SIZE = 64;		// The FIFO is 64 bytes.
+	
+	/////////////////////////////////////////////////////////////////////////////////////
+	// Functions for setting up the Raspberry Pi
+	/////////////////////////////////////////////////////////////////////////////////////
+	MFRC522();
+	void setSPIConfig();
+	/////////////////////////////////////////////////////////////////////////////////////
+	// Basic interface functions for communicating with the MFRC522
+	/////////////////////////////////////////////////////////////////////////////////////
+	void PCD_WriteRegister(byte reg, byte value);
+	void PCD_WriteRegister(byte reg, byte count, byte *values);
+	byte PCD_ReadRegister(byte reg);
+	void PCD_ReadRegister(byte reg, byte count, byte *values, byte rxAlign = 0);
+	void setBitMask(unsigned char reg, unsigned char mask);
+	void PCD_SetRegisterBitMask(byte reg, byte mask);
+	void PCD_ClearRegisterBitMask(byte reg, byte mask);
+	byte PCD_CalculateCRC(byte *data, byte length, byte *result);
+	
+	/////////////////////////////////////////////////////////////////////////////////////
+	// Functions for manipulating the MFRC522
+	/////////////////////////////////////////////////////////////////////////////////////
+	void PCD_Init();
+	void PCD_Reset();
+	void PCD_AntennaOn();
+	void PCD_AntennaOff();
+	byte PCD_GetAntennaGain();
+	void PCD_SetAntennaGain(byte mask);
+	bool PCD_PerformSelfTest();
+	
+	/////////////////////////////////////////////////////////////////////////////////////
+	// Functions for communicating with PICCs
+	/////////////////////////////////////////////////////////////////////////////////////
+	byte PCD_TransceiveData(byte *sendData, byte sendLen, byte *backData, byte *backLen, byte *validBits = NULL, byte rxAlign = 0, bool checkCRC = false);
+	byte PCD_CommunicateWithPICC(byte command, byte waitIRq, byte *sendData, byte sendLen, byte *backData = NULL, byte *backLen = NULL, byte *validBits = NULL, byte rxAlign = 0, bool checkCRC = false);
+	byte PICC_RequestA(byte *bufferATQA, byte *bufferSize);
+	byte PICC_WakeupA(byte *bufferATQA, byte *bufferSize);
+	byte PICC_REQA_or_WUPA(byte command, byte *bufferATQA, byte *bufferSize);
+	byte PICC_Select(Uid *uid, byte validBits = 0);
+	byte PICC_HaltA();
+	
+	/////////////////////////////////////////////////////////////////////////////////////
+	// Functions for communicating with MIFARE PICCs
+	/////////////////////////////////////////////////////////////////////////////////////
+	byte PCD_Authenticate(byte command, byte blockAddr, MIFARE_Key *key, Uid *uid);
+	void PCD_StopCrypto1();
+	byte MIFARE_Read(byte blockAddr, byte *buffer, byte *bufferSize);
+	byte MIFARE_Write(byte blockAddr, byte *buffer, byte bufferSize);
+	byte MIFARE_Decrement(byte blockAddr, long delta);
+	byte MIFARE_Increment(byte blockAddr, long delta);
+	byte MIFARE_Restore(byte blockAddr);
+	byte MIFARE_Transfer(byte blockAddr);
+	byte MIFARE_Ultralight_Write(byte page, byte *buffer, byte bufferSize);
+	byte MIFARE_GetValue(byte blockAddr, long *value);
+	byte MIFARE_SetValue(byte blockAddr, long value);
+	
+	/////////////////////////////////////////////////////////////////////////////////////
+	// Support functions
+	/////////////////////////////////////////////////////////////////////////////////////
+	byte PCD_MIFARE_Transceive(byte *sendData, byte sendLen, bool acceptTimeout = false);
+	// old function used too much memory, now name moved to flash; if you need char, copy from flash to memory
+	//const char *GetStatusCodeName(byte code);
+	const string GetStatusCodeName(byte code);
+	byte PICC_GetType(byte sak);
+	// old function used too much memory, now name moved to flash; if you need char, copy from flash to memory
+	//const char *PICC_GetTypeName(byte type);
+	const string PICC_GetTypeName(byte type);
+	void PICC_DumpToSerial(Uid *uid);
+	void PICC_DumpMifareClassicToSerial(Uid *uid, byte piccType, MIFARE_Key *key);
+	void PICC_DumpMifareClassicSectorToSerial(Uid *uid, MIFARE_Key *key, byte sector);
+	void PICC_DumpMifareUltralightToSerial();
+	void MIFARE_SetAccessBits(byte *accessBitBuffer, byte g0, byte g1, byte g2, byte g3);
+	bool MIFARE_OpenUidBackdoor(bool logErrors);
+	bool MIFARE_SetUid(byte *newUid, byte uidSize, bool logErrors);
+	bool MIFARE_UnbrickUidSector(bool logErrors);
+	
+	/////////////////////////////////////////////////////////////////////////////////////
+	// Convenience functions - does not add extra functionality
+	/////////////////////////////////////////////////////////////////////////////////////
+	bool PICC_IsNewCardPresent();
+	bool PICC_ReadCardSerial();
+	
+private:
+	byte MIFARE_TwoStepHelper(byte command, byte blockAddr, long data);
+};
+
+#endif

kawaii/emergency_stop.cpp

@@ -0,0 +1,29 @@
+//
+// Created by Simon Wörner on 16.06.17.
+//
+
+#include "emergency_stop.h"
+
+#include <cstddef>
+
+extern "C" {
+void *emergency_stop_init(uint8_t port);
+void emergency_stop_clean(void *emergency_stop);
+bool emergency_stop_get_state(void *emergency_stop);
+}
+
+emergency_stop::emergency_stop(uint8_t port)
+{
+    rust_emergency_stop = emergency_stop_init(port);
+}
+
+emergency_stop::~emergency_stop()
+{
+    emergency_stop_clean(rust_emergency_stop);
+    rust_emergency_stop = NULL;
+}
+
+bool emergency_stop::get_state() const
+{
+    return emergency_stop_get_state(rust_emergency_stop);
+}

kawaii/emergency_stop.h

@@ -0,0 +1,24 @@
+//
+// Created by Simon Wörner on 16.06.17.
+//
+
+#ifndef KAWAII_ENGINE_EMERGENCY_STOP_H
+#define KAWAII_ENGINE_EMERGENCY_STOP_H
+
+
+#include <cstdint>
+
+class emergency_stop
+{
+private:
+	void *rust_emergency_stop;
+public:
+   	emergency_stop(uint8_t port);
+	emergency_stop(const emergency_stop &) = delete;
+	emergency_stop(const emergency_stop &&) = delete;
+	bool get_state() const;
+	~emergency_stop();
+};
+
+
+#endif //KAWAII_ENGINE_EMERGENCY_STOP_H

kawaii/engine.cpp

@@ -0,0 +1,79 @@
+#include "engine.hpp"
+
+#include <chrono>
+#include <utility>
+
+using namespace std;
+using namespace std::chrono;
+
+engine::engine(gpio&& pin_forward, gpio&& pin_reverse)
+	: speed(0),
+	  pin_forward(move(pin_forward)),
+	  pin_reverse(move(pin_reverse)),
+	  stop_thread(false),
+	  measurement(string("engine") + to_string(this->pin_forward.get_pin()))
+{
+	this->pin_forward.set_value(false);
+	this->pin_reverse.set_value(false);
+	pwm_thread = thread(&engine::pwm_loop, this);
+}
+
+engine::~engine()
+{
+	stop_thread = true;
+	pwm_thread.join();
+}
+
+void engine::set_speed(int speed)
+{
+	this->speed.store(speed, memory_order_relaxed);
+}
+
+void engine::pwm_loop()
+{
+	this_thread::sleep_for(milliseconds(100));
+	const int sleep_msec = 1;
+	int counter = 0;
+	high_resolution_clock::time_point next_sleep_target = high_resolution_clock::now();
+	while (!stop_thread)
+	{
+		measurement.start();
+		direction target_direction;
+		int speed_val = speed.load(memory_order_relaxed);
+		if (speed_val < 0)
+		{
+			target_direction = direction::REVERSE;
+			speed_val *= -1;
+		}
+		else
+		{
+			target_direction = direction::FORWARD;
+		}
+		counter += sleep_msec;
+		if (counter >= 100)
+			counter = 0;
+		if (counter < speed_val)
+		{
+			if (target_direction == direction::FORWARD)
+			{
+				pin_reverse.set_value(false);
+				pin_forward.set_value(true);
+			}
+			else
+			{
+				pin_forward.set_value(false);
+				pin_reverse.set_value(true);
+			}
+		}
+		else
+		{
+			pin_forward.set_value(false);
+			pin_reverse.set_value(false);
+		}
+		measurement.stop();
+		next_sleep_target += 500us;
+		this_thread::sleep_until(next_sleep_target);
+	}
+	pin_forward.set_value(false);
+	pin_reverse.set_value(false);
+}

kawaii/engine.hpp

@@ -0,0 +1,31 @@
+#ifndef ENGINE_HPP_
+#define ENGINE_HPP_
+
+#include <atomic>
+#include <thread>
+#include "gpio.hpp"
+#include "measure.hpp"
+
+class engine
+{
+public:
+	enum class direction
+	{
+		FORWARD,
+		REVERSE
+	};
+private:
+	std::atomic<int> speed;
+	gpio pin_forward;
+	gpio pin_reverse;
+	std::atomic<bool> stop_thread;
+	std::thread pwm_thread;
+	measure measurement;
+	void pwm_loop();
+public:
+	engine(gpio&& pin_forward, gpio&& pin_reverse);
+	~engine();
+	void set_speed(int speed);
+};
+
+#endif

kawaii/gpio.cpp

@@ -0,0 +1,109 @@
+#include "gpio.hpp"
+
+#include <fstream>
+#include <sstream>
+#include <stdexcept>
+#include <string>
+
+using namespace std;
+
+static string construct_filename(const string &prefix, int pin, const string &suffix)
+{
+	stringstream filename;
+	filename << prefix << pin << suffix;
+	return filename.str();
+}
+
+gpio::gpio(int pin, pin_direction direction, pin_type type, bool default_state)
+	: pin(pin),
+	  direction(direction),
+	  type(type),
+	  current_state(false)
+{
+	ofstream export_file("/sys/class/gpio/export");
+	export_file << pin;
+	export_file.close();
+	ofstream direction_file(construct_filename("/sys/class/gpio/gpio", pin, "/direction").c_str());
+	if (direction == pin_direction::INPUT)
+	{
+		direction_file << "in";
+	}
+	else
+	{
+		direction_file << "out";
+	}
+	direction_file.close();
+	if (direction == pin_direction::OUTPUT)
+	{
+		set_value(default_state, false);
+	}
+}
+
+gpio::gpio(gpio&& old)
+	: pin(old.pin),
+	  direction(old.direction),
+	  type(old.type),
+	  current_state(old.current_state)
+{
+	old.pin = -1;
+}
+
+gpio::~gpio()
+{
+	if (pin != -1)
+	{
+		ofstream unexport_file("/sys/class/gpio/unexport");
+		unexport_file << pin;
+	}
+}
+
+void gpio::set_value(bool on, bool use_cache)
+{
+	if (pin == -1)
+		throw logic_error("Usage of moved gpio");
+	if (direction == pin_direction::INPUT)
+	{
+		stringstream errormsg;
+		errormsg << "Cannot write to input pin " << pin;
+		throw logic_error(errormsg.str());
+	}
+	if (!use_cache || current_state != on)
+	{
+		bool value;
+		if (type == pin_type::HIGH_ON)
+			value = on;
+		else
+			value = !on;
+		ofstream value_file(construct_filename("/sys/class/gpio/gpio", pin, "/value"));
+		value_file << value;
+	}
+	current_state = on;
+}
+
+void gpio::set_value(bool on)
+{
+	set_value(on, true);
+}
+
+bool gpio::get_value()
+{
+	if (pin == -1)
+		throw logic_error("Usage of moved gpio");
+	if (direction == pin_direction::OUTPUT)
+		return current_state;
+	ifstream value_file(construct_filename("/sys/class/gpio/gpio", pin, "/value"));
+	bool value;
+	value_file >> value;
+	bool on;
+	if (type == pin_type::HIGH_ON)
+		on = value;
+	else
+		on = !value;
+	return on;
+}
+
+int gpio::get_pin() const
+{
+	return pin;
+}
+

kawaii/gpio.hpp

@@ -0,0 +1,34 @@
+#ifndef GPIO_HPP_
+#define GPIO_HPP_
+
+class gpio
+{
+public:
+	enum class pin_direction
+	{
+		INPUT,
+		OUTPUT
+	};
+	enum class pin_type
+	{
+		HIGH_ON,
+		LOW_ON
+	};
+
+private:
+	int pin;
+	pin_direction direction;
+	pin_type type;
+	bool current_state;
+	void set_value(bool on, bool use_cache);
+public:
+	gpio(int pin, pin_direction, pin_type=pin_type::HIGH_ON, bool default_state=false);
+	gpio(const gpio&) = delete;
+	gpio(gpio&&);
+	~gpio();
+	void set_value(bool on);
+	bool get_value();
+	int get_pin() const;
+};
+
+#endif

kawaii/kawaii-rs/.gitignore

@@ -0,0 +1,4 @@
+Cargo.lock
+target/
+**/*.rs.bk
+*.iml

kawaii/kawaii-rs/CMakeLists.txt

@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 3.0)
+project(kawaii-rs)
+
+include(ExternalProject)
+
+if (MEASURE)
+	set(CARGO_FEATURES "${CARGO_FEATURES} measure")
+endif (MEASURE)
+
+if (CARGO_FEATURES)
+	set(CARGO_ARGUMENTS --features "${CARGO_FEATURES}")
+endif(CARGO_FEATURES)
+
+file(GLOB RUST_SOURCE_FILES "${PROJECT_SOURCE_DIR}/src" *.rs)
+add_custom_target(${PROJECT_NAME}
+		COMMAND cargo build --color=never ${CARGO_ARGUMENTS}
+		COMMAND cargo build --color=never ${CARGO_ARGUMENTS} --release
+		DEPENDS ${RUST_SOURCE_FILES}
+		BYPRODUCTS target/debug/libkawaii.a target/release/libkawaii.a
+		WORKING_DIRECTORY "${PROJECT_SOURCE_DIR}"
+		SOURCES ${RUST_SOURCE_FILES})

kawaii/kawaii-rs/Cargo.toml

@@ -0,0 +1,19 @@
+[package]
+name = "kawaii-rs-api"
+version = "0.1.0"
+authors = ["Simon Wörner <git@simon-woerner.de>"]
+
+[lib]
+name = "kawaii"
+crate-type = ["staticlib"]
+
+[features]
+measure = [
+    "kawaii/measure",
+    "emergency_stop/measure",
+    "ultrasonic_irq/measure" ]
+
+[dependencies]
+emergency_stop = { git = "https://git.brn.li/kawaii-robotto/emergency-stop.git" }
+kawaii = { git = "https://git.brn.li/kawaii-robotto/kawaii-rs.git" }
+ultrasonic_irq = { git = "https://git.brn.li/kawaii-robotto/ultrasonic-irq.git" }

kawaii/kawaii-rs/dependencies/emergency-stop/.gitignore

@@ -0,0 +1,8 @@
+CMakeFiles/
+Makefile
+cmake_install.cmake
+emergency-stop-prefix/
+emergency-stop.cbp
+target/
+**/*.rs.bk
+*.iml

kawaii/kawaii-rs/dependencies/emergency-stop/Cargo.toml

@@ -0,0 +1,10 @@
+[package]
+name = "emergency_stop"
+version = "0.1.0"
+authors = ["Simon Wörner <git@simon-woerner.de>"]
+
+[features]
+measure = ["kawaii/measure"]
+
+[dependencies]
+kawaii = { git = "https://git.brn.li/kawaii-robotto/kawaii-rs.git" }

kawaii/kawaii-rs/dependencies/emergency-stop/src/lib.rs

@@ -0,0 +1,96 @@
+extern crate kawaii;
+
+use std::time::Duration;
+use std::thread;
+use std::thread::JoinHandle;
+use std::sync::Arc;
+use std::sync::atomic::{AtomicBool, Ordering};
+
+use self::kawaii::gpio::{AsyncPort, Edge, Value};
+
+#[cfg(feature = "measure")]
+use self::kawaii::Measure;
+
+#[derive(Debug)]
+pub struct EmergencyStop {
+    thread: Option<JoinHandle<()>>,
+    pub state: Arc<AtomicBool>,
+}
+
+impl EmergencyStop {
+    pub fn new(stop_port: u8) -> std::io::Result<Self> {
+        let name = format!("EmergencyStop(port = {})", stop_port);
+        let state = Arc::new(AtomicBool::new(false));
+        let mut port = AsyncPort::new(stop_port, Edge::Both)?;
+
+        let state_clone = state.clone();
+        let thread = thread::Builder::new()
+            .name(name)
+            .spawn(move || EmergencyStop::thread(&mut port, state_clone))?;
+
+        Ok(EmergencyStop {
+            thread: Some(thread),
+            state: state,
+        })
+    }
+
+    fn thread(port: &mut AsyncPort, state: Arc<AtomicBool>) {
+        #[cfg(feature = "measure")]
+        let mut measure = Measure::new(format!("EmergencyStop(port = {})", port.port.number));
+
+        // clear first value
+        port.poll(Some(Duration::new(0, 0))).is_ok();
+
+        while !state.load(Ordering::Relaxed) {
+            #[cfg(feature = "measure")]
+            measure.start();
+
+            let timeout = Some(Duration::new(1, 0));
+
+            #[cfg(not(feature = "measure"))]
+            let value = port.poll(timeout);
+
+            #[cfg(feature = "measure")]
+            let value = port.poll_measure(timeout, &mut measure);
+
+            // continue on timeout
+            match value {
+                Ok(value) => {
+                    if let Some(value) = value {
+                        match value {
+                            Value::High => {
+                                #[cfg(debug_assertions)]
+                                println!("EmergencyStop! ({:?})", value);
+
+                                state.store(true, Ordering::Relaxed);
+                            }
+                            _ => {
+                                #[cfg(debug_assertions)]
+                                println!("EmergencyStop ignored: ({:?})", value);
+                            }
+                        }
+                    }
+                }
+                Err(e) => {
+                    #[cfg(debug_assertions)]
+                    println!("EmergencyStop! ({:?})", e);
+
+                    state.store(true, Ordering::Relaxed);
+                }
+            }
+
+            #[cfg(feature = "measure")]
+            measure.stop();
+        }
+    }
+}
+
+impl Drop for EmergencyStop {
+    fn drop(&mut self) {
+        self.state.store(true, Ordering::Relaxed);
+
+        if let Some(thread) = self.thread.take() {
+            thread.join().is_ok();
+        }
+    }
+}

kawaii/kawaii-rs/dependencies/emergency-stop/src/main.rs

@@ -0,0 +1,21 @@
+extern crate emergency_stop;
+
+use emergency_stop::EmergencyStop;
+
+use std::time::Duration;
+use std::thread::sleep;
+use std::sync::atomic::Ordering;
+
+static GPIO_PORT_STOP: u8 = 25;
+
+fn main() {
+    let emergency_stop = EmergencyStop::new(GPIO_PORT_STOP).expect("Create Emergency Stop failed");
+
+    println!("{:?}", emergency_stop);
+
+    while !emergency_stop.state.load(Ordering::Relaxed) {
+        sleep(Duration::new(1, 0));
+    }
+
+    println!("Stopped.");
+}

kawaii/kawaii-rs/dependencies/kawaii-rs/.gitignore

@@ -0,0 +1,4 @@
+Cargo.lock
+target/
+**/*.rs.bk
+*.iml

kawaii/kawaii-rs/dependencies/kawaii-rs/Cargo.toml

@@ -0,0 +1,13 @@
+[package]
+name = "kawaii"
+version = "0.1.0"
+authors = ["Simon Wörner <git@simon-woerner.de>"]
+
+[features]
+measure = []
+
+[dependencies]
+nix = "0.8.1"
+regex = "^0.2"
+separator = "^0.3.1"
+time = "^0.1.36"

kawaii/kawaii-rs/dependencies/kawaii-rs/src/gpio.rs

@@ -0,0 +1,288 @@
+extern crate std;
+extern crate nix;
+
+use std::fmt;
+use std::time::Duration;
+use std::io::prelude::*;
+use std::io::{Error, ErrorKind, SeekFrom};
+use std::fs::File;
+use std::path::Path;
+
+use std::os::unix::io::RawFd;
+
+#[cfg(feature = "measure")]
+use measure::Measure;
+
+fn duration_to_ms(duration: Duration) -> u64 {
+    duration.as_secs() * 1_000u64 + duration.subsec_nanos() as u64 / 1_000_000u64
+}
+
+#[derive(Clone, Copy, Debug)]
+pub enum Direction {
+    Out,
+    In,
+}
+impl Direction {
+    pub fn from_str(s: &str) -> Option<Direction> {
+        match s {
+            "out" => Some(Direction::Out),
+            "in" => Some(Direction::In),
+            _ => None,
+        }
+    }
+
+    pub fn as_str(&self) -> &'static str {
+        match *self {
+            Direction::Out => "out",
+            Direction::In => "in",
+        }
+    }
+}
+
+#[derive(Clone, Copy, Debug)]
+pub enum Edge {
+    None,
+    Rising,
+    Falling,
+    Both,
+}
+impl Edge {
+    pub fn from_str(s: &str) -> Option<Edge> {
+        match s {
+            "none" => Some(Edge::None),
+            "rising" => Some(Edge::Rising),
+            "falling" => Some(Edge::Falling),
+            "both" => Some(Edge::Both),
+            _ => None,
+        }
+    }
+
+    pub fn as_str(&self) -> &'static str {
+        match *self {
+            Edge::None => "none",
+            Edge::Rising => "rising",
+            Edge::Falling => "falling",
+            Edge::Both => "both",
+        }
+    }
+}
+
+#[derive(Clone, Copy, Debug)]
+pub enum Value {
+    High,
+    Low,
+}
+impl Value {
+    pub fn from_str(s: &str) -> Option<Value> {
+        match s {
+            "1" => Some(Value::High),
+            "0" => Some(Value::Low),
+            _ => None,
+        }
+    }
+
+    pub fn from_buffer(b: &[u8; 1]) -> Option<Self> {
+        Value::from_char(b[0])
+    }
+
+    pub fn from_char(c: u8) -> Option<Self> {
+        match c {
+            48 => Some(Value::Low), // '0'
+            49 => Some(Value::High), // '1'
+            _ => None,
+        }
+    }
+
+    pub fn as_str(&self) -> &'static str {
+        match *self {
+            Value::High => "1",
+            Value::Low => "0",
+        }
+    }
+}
+
+#[derive(Debug)]
+pub struct Port {
+    pub number: u8,
+    pub direction: Direction,
+}
+
+#[derive(Debug)]
+pub struct SyncPort {
+    pub port: Port,
+    file: File,
+    buffer: [u8; 1],
+}
+pub struct AsyncPort {
+    pub port: Port,
+    pub edge: Edge,
+    file: RawFd,
+    fds: [nix::poll::PollFd; 1],
+    buffer: [u8; 1],
+}
+
+impl fmt::Debug for AsyncPort {
+    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        write!(f,
+               "AsyncPort {{ port: {:?}, edge: {:?}, file: {:?}, fds: [?], buffer: {:?} }}",
+               self.port,
+               self.edge,
+               self.file,
+               self.buffer)
+    }
+}
+
+impl Port {
+    pub fn new(number: u8, direction: Direction) -> std::io::Result<Port> {
+        let port = Port {
+            number: number,
+            direction: direction,
+        };
+
+        port.init()?;
+        Ok(port)
+    }
+
+    fn init(&self) -> std::io::Result<()> {
+        self.export().ok();
+        self.set_direction()?;
+
+        Ok(())
+    }
+
+    pub fn drop(&mut self) {
+        self.unexport().ok();
+    }
+
+    fn write_path(path: &str, value: &str) -> std::io::Result<()> {
+        File::create(Path::new(path))?
+            .write_all(value.as_bytes())
+    }
+
+    fn export(&self) -> std::io::Result<()> {
+        Port::write_path("/sys/class/gpio/export", self.number.to_string().as_str())
+    }
+    fn unexport(&self) -> std::io::Result<()> {
+        Port::write_path("/sys/class/gpio/unexport", self.number.to_string().as_str())
+    }
+
+    fn set_direction(&self) -> std::io::Result<()> {
+        Port::write_path(format!("/sys/class/gpio/gpio{}/direction", self.number).as_str(),
+                         self.direction.as_str())
+    }
+}
+
+impl SyncPort {
+    pub fn new(number: u8, direction: Direction) -> std::io::Result<SyncPort> {
+        Ok(SyncPort {
+               port: Port::new(number, direction)?,
+               file: SyncPort::open(number, direction)?,
+               buffer: [0; 1],
+           })
+    }
+
+    fn open(number: u8, direction: Direction) -> std::io::Result<File> {
+        let path = format!("/sys/class/gpio/gpio{}/value", number);
+        let path = Path::new(path.as_str());
+
+        Ok(match direction {
+               Direction::Out => File::create(path)?,
+               Direction::In => File::open(path)?,
+           })
+    }
+
+    pub fn read(&mut self) -> std::io::Result<Value> {
+        self.file.seek(SeekFrom::Start(0))?;
+        self.file.read_exact(&mut self.buffer)?;
+
+        Value::from_buffer(&self.buffer).ok_or(Error::new(ErrorKind::InvalidData,
+                                                          "Unrecognized GPIO Value"))
+    }
+
+    pub fn write(&mut self, value: Value) -> std::io::Result<()> {
+        self.file.write_all(value.as_str().as_bytes())
+    }
+}
+
+impl AsyncPort {
+    pub fn new(number: u8, edge: Edge) -> std::io::Result<AsyncPort> {
+        let port = Port::new(number, Direction::In)?;
+        let file = AsyncPort::open(number)?;
+        let port = AsyncPort {
+            port: port,
+            edge: edge,
+            file: file,
+            fds: [nix::poll::PollFd::new(file, nix::poll::POLLPRI, nix::poll::EventFlags::empty())],
+            buffer: [0; 1],
+        };
+
+        port.init()?;
+        Ok(port)
+    }
+    fn init(&self) -> std::io::Result<()> {
+        self.set_edge()?;
+
+        Ok(())
+    }
+
+    fn open(number: u8) -> std::io::Result<RawFd> {
+        nix::fcntl::open(format!("/sys/class/gpio/gpio{}/value", number).as_str(),
+                         nix::fcntl::O_RDONLY,
+                         nix::sys::stat::Mode::empty())
+                .or(Err(Error::new(ErrorKind::Other, "open failed")))
+    }
+
+    fn get_timeout(timeout: Option<Duration>) -> i32 {
+        match timeout {
+            None => -1,
+            Some(t) => duration_to_ms(t) as i32,
+        }
+    }
+
+    fn nix_poll(&mut self, timeout: i32) -> nix::Result<i32> {
+        nix::poll::poll(&mut self.fds, timeout)
+    }
+
+    fn poll_read(&mut self, poll: nix::Result<i32>) -> std::io::Result<Option<Value>> {
+        let poll = poll.or(Err(Error::new(ErrorKind::Other, "poll failed")))?;
+
+        if poll == 0 {
+            return Ok(None);
+        }
+
+        nix::unistd::lseek(self.file, 0, nix::unistd::Whence::SeekSet)
+            .or(Err(Error::new(ErrorKind::Other, "lseek failed")))?;
+
+        nix::unistd::read(self.file, &mut self.buffer)
+            .or(Err(Error::new(ErrorKind::Other, "read failed")))?;
+
+        Value::from_buffer(&self.buffer).map_or(Err(Error::new(ErrorKind::InvalidData,
+                                                               "Unrecognized GPIO Value")),
+                                                |v| Ok(Some(v)))
+    }
+
+    pub fn poll(&mut self, timeout: Option<Duration>) -> std::io::Result<Option<Value>> {
+        let poll = self.nix_poll(Self::get_timeout(timeout));
+
+        self.poll_read(poll)
+    }
+
+    #[cfg(feature = "measure")]
+    pub fn poll_measure(&mut self,
+                        timeout: Option<Duration>,
+                        measure: &mut Measure)
+                        -> std::io::Result<Option<Value>> {
+        let timeout = Self::get_timeout(timeout);
+
+        measure.pause();
+        let poll = self.nix_poll(timeout);
+        measure.start();
+
+        self.poll_read(poll)
+    }
+
+    fn set_edge(&self) -> std::io::Result<()> {
+        Port::write_path(format!("/sys/class/gpio/gpio{}/edge", self.port.number).as_str(),
+                         self.edge.as_str())
+    }
+}

kawaii/kawaii-rs/dependencies/kawaii-rs/src/lib.rs

@@ -0,0 +1,7 @@
+pub mod gpio;
+
+#[cfg(feature = "measure")]
+mod measure;
+
+#[cfg(feature = "measure")]
+pub use measure::Measure;

kawaii/kawaii-rs/dependencies/kawaii-rs/src/main.rs

@@ -0,0 +1,35 @@
+extern crate kawaii;
+
+use std::time::Duration;
+
+use kawaii::gpio::{SyncPort, AsyncPort, Direction, Edge, Value};
+
+#[cfg(feature = "measure")]
+use kawaii::measure::Measure;
+
+fn main() {
+    println!("Hello, world!");
+
+    #[cfg(feature = "measure")]
+    {
+        let mut measure = Measure::new(String::from("main({test: 1})"));
+
+        for _ in 0..5 {
+            measure.start();
+            std::thread::sleep(Duration::new(0, 1_000_000));
+            measure.stop();
+        }
+
+        println!("{:?}", measure);
+    }
+
+    let mut trigger = SyncPort::new(27, Direction::Out).expect("Create Trigger GPIO failed");
+    let mut echo = AsyncPort::new(28, Edge::Both).expect("Create Echo GPIO failed");
+
+    println!("trigger = {:?}", trigger.read());
+    trigger.write(Value::High).expect("write failed");
+    println!("trigger = {:?}", trigger.read());
+    trigger.write(Value::Low).expect("write failed");
+    println!("trigger = {:?}", trigger.read());
+    println!("echo = {:?}", echo.poll(Some(Duration::new(1, 0))));
+}

kawaii/kawaii-rs/dependencies/kawaii-rs/src/measure.rs

@@ -0,0 +1,111 @@
+extern crate regex;
+extern crate separator;
+extern crate time;
+
+use std;
+use std::fs::File;
+use std::io::prelude::*;
+use std::io::{Error, ErrorKind};
+
+use self::regex::Regex;
+use self::separator::Separatable;
+use self::time::precise_time_ns;
+
+#[derive(Debug)]
+pub struct Measure {
+    pub min: u64,
+    pub max: u64,
+    time: u64,
+    last: u64,
+    data: Vec<u64>,
+    pub name: String,
+}
+
+impl Measure {
+    pub fn new(name: String) -> Self {
+        Measure {
+            min: u64::max_value(),
+            max: 0u64,
+            time: 0u64,
+            last: 0u64,
+            data: Vec::with_capacity(1_000_000),
+            name: name,
+        }
+    }
+
+    pub fn start(&mut self) {
+        self.last = precise_time_ns();
+    }
+
+    pub fn pause(&mut self) {
+        if self.last == 0 {
+            #[cfg(debug_assertions)]
+            println!("WARNING: {:?} pause called without start!", self);
+
+            return;
+        }
+
+        self.time += self.time_diff();
+    }
+
+    pub fn stop(&mut self) {
+        if self.last == 0 {
+            #[cfg(debug_assertions)]
+            println!("WARNING: {:?} stop called without start!", self);
+
+            return;
+        }
+
+        self.time += self.time_diff();
+        self.data.push(self.time);
+
+        if self.time < self.min {
+            self.min = self.time;
+        }
+
+        if self.time > self.max {
+            self.max = self.time;
+        }
+
+        self.time = 0u64;
+        self.last = 0u64;
+    }
+
+    fn time_diff(&mut self) -> u64 {
+        let current_time = precise_time_ns();
+        let time_diff = current_time - self.last;
+        self.last = current_time;
+
+        time_diff
+    }
+
+    fn write_data(&self) -> std::io::Result<()> {
+        let re = Regex::new(r"[^\w\-.]")
+            .or(Err(Error::new(ErrorKind::Other, "Create filename regex failed.")))?;
+
+        let file_name = format!("measure_{}.txt", self.name);
+        let file_name = re.replace_all(file_name.as_str(), "_").to_string();
+        println!("{}: Write data to {}", self.name, file_name);
+
+        let mut file = File::create(file_name)?;
+
+        for value in &self.data {
+            file.write_fmt(format_args!("{}\n", value))?;
+        }
+
+        Ok(())
+    }
+}
+
+impl Drop for Measure {
+    fn drop(&mut self) {
+        println!("{}:\n\tmin: {} ns\n\tmax: {} ns",
+                 self.name,
+                 self.min.separated_string(),
+                 self.max.separated_string());
+
+        if let Err(e) = self.write_data() {
+            println!("{}: Write measure data failed: {}", self.name, e);
+        }
+    }
+}

kawaii/kawaii-rs/dependencies/ultrasonic-irq/.gitignore

@@ -0,0 +1,7 @@
+CMakeFiles/
+Makefile
+cmake_install.cmake
+ultrasonic-irq-prefix/
+ultrasonic-irq.cbp
+target/
+**/*.rs.bk

kawaii/kawaii-rs/dependencies/ultrasonic-irq/Cargo.toml

@@ -0,0 +1,12 @@
+[package]
+name = "ultrasonic_irq"
+version = "0.1.0"
+authors = ["kawaii <root@kawaii.home.lost-in-space.net>"]
+
+[features]
+measure = ["kawaii/measure"]
+
+[dependencies]
+time = "^0.1.36"
+shuteye = "^0.3.2"
+kawaii = { git = "https://git.brn.li/kawaii-robotto/kawaii-rs.git" }

kawaii/kawaii-rs/dependencies/ultrasonic-irq/src/lib.rs

@@ -0,0 +1,194 @@
+#![feature(integer_atomics)]
+
+extern crate time;
+extern crate shuteye;
+extern crate kawaii;
+
+use std::time::Duration;
+use std::thread;
+use std::thread::JoinHandle;
+use std::sync::mpsc::{Sender, Receiver};
+use std::sync::mpsc;
+use std::sync::Arc;
+use std::sync::atomic::{AtomicU32, Ordering};
+
+use self::kawaii::gpio::{SyncPort, AsyncPort, Direction, Edge, Value};
+
+#[cfg(feature = "measure")]
+use self::kawaii::Measure;
+
+use self::time::precise_time_ns;
+use self::shuteye::sleep;
+
+#[derive(Debug)]
+struct ThreadData<T> {
+    thread: JoinHandle<T>,
+    tx: Sender<()>,
+}
+
+#[derive(Debug)]
+pub struct UltrasonicEcho {
+    echo: AsyncPort,
+    temperature: u8,
+    timestamp: u64,
+    distance: Arc<AtomicU32>,
+}
+
+#[derive(Debug)]
+pub struct UltrasonicTrigger {
+    trigger: SyncPort,
+}
+
+#[derive(Debug)]
+pub struct Ultrasonic {
+    trigger: Option<ThreadData<()>>,
+    echo: Option<ThreadData<()>>,
+    pub distance: Arc<AtomicU32>,
+}
+
+impl Ultrasonic {
+    pub fn new(trigger_port: u8, echo_port: u8, temperature: u8) -> std::io::Result<Ultrasonic> {
+        let distance = Arc::new(AtomicU32::new(u32::max_value()));
+        let echo = UltrasonicEcho {
+            echo: AsyncPort::new(echo_port, Edge::Both)?,
+            temperature: temperature,
+            timestamp: 0,
+            distance: distance.clone(),
+        };
+        let (tx, rx): (Sender<()>, Receiver<()>) = mpsc::channel();
+        let name = format!("Ultrasonic::echo(port = {})", echo_port);
+        let echo = ThreadData::<()> {
+            thread: thread::Builder::new()
+                .name(name)
+                .spawn(move || echo.thread(rx))?,
+            tx: tx,
+        };
+
+        let trigger = UltrasonicTrigger { trigger: SyncPort::new(trigger_port, Direction::Out)? };
+        let (tx, rx): (Sender<()>, Receiver<()>) = mpsc::channel();
+        let name = format!("Ultrasonic::trigger(port = {})", trigger_port);
+        let trigger = ThreadData::<()> {
+            thread: thread::Builder::new()
+                .name(name)
+                .spawn(move || trigger.thread(rx))?,
+            tx: tx,
+        };
+
+        Ok(Ultrasonic {
+               trigger: Some(trigger),
+               echo: Some(echo),
+               distance: distance,
+           })
+    }
+}
+
+impl Drop for Ultrasonic {
+    fn drop(&mut self) {
+        if let Some(echo) = self.echo.take() {
+            echo.tx.send(()).is_ok();
+            echo.thread.join().is_ok();
+        }
+
+        if let Some(trigger) = self.trigger.take() {
+            trigger.tx.send(()).is_ok();
+            trigger.thread.join().is_ok();
+        }
+    }
+}
+
+impl UltrasonicTrigger {
+    fn thread(mut self, stop_rx: Receiver<()>) {
+        #[cfg(feature = "measure")]
+        let mut measure = Measure::new(format!("Ultrasonic::trigger(port = {})", self.trigger.port.number));
+
+        while stop_rx.try_recv().is_err() {
+            #[cfg(feature = "measure")]
+            measure.start();
+            self.trigger.write(Value::Low).is_ok();
+            #[cfg(feature = "measure")]
+            measure.pause();
+
+            // sleep 10us (min length to trigger)
+            sleep(Duration::new(0, 10_000));
+
+            #[cfg(feature = "measure")]
+            measure.start();
+            self.trigger.write(Value::High).is_ok();
+            #[cfg(feature = "measure")]
+            measure.stop();
+
+            // sleep 20ms (max trigger frequency)
+            sleep(Duration::new(0, 20_000_000));
+        }
+    }
+}
+
+impl UltrasonicEcho {
+    fn thread(mut self, stop_rx: Receiver<()>) {
+        #[cfg(feature = "measure")]
+        let mut measure = Measure::new(format!("Ultrasonic::echo(port = {})", self.echo.port.number));
+
+        while stop_rx.try_recv().is_err() {
+            #[cfg(feature = "measure")]
+            measure.start();
+
+            #[cfg(not(feature = "measure"))]
+            let value = self.echo.poll(Some(Duration::new(1, 0)));
+
+            #[cfg(feature = "measure")]
+            let value = self.echo.poll_measure(Some(Duration::new(1, 0)), &mut measure);
+
+            match value {
+                Ok(value) => {
+                    if let Some(value) = value {
+                        self.on_value_changed(value);
+                    }
+                }
+                Err(_e) => {
+                    #[cfg(debug_assertions)]
+                    println!("POLL failed: e = {:?}", _e);
+                }
+            }
+
+            #[cfg(feature = "measure")]
+            measure.stop();
+        }
+    }
+
+    fn on_value_changed(&mut self, value: Value) {
+        match value {
+            Value::High => {
+                // Switched from Value::High to Value::High
+                // possibly because of trigger after timeout and slow reading
+                if self.timestamp > 0 {
+                    #[cfg(debug_assertions)]
+                    println!("{:?}", self);
+
+                    self.calc_distance();
+                }
+
+                self.timestamp = precise_time_ns()
+            }
+            Value::Low => {
+                // Switched from Value::Low to Value::Low
+                if self.timestamp == 0 {
+                    #[cfg(debug_assertions)]
+                    println!("{:?}", self);
+                }
+
+                self.calc_distance();
+            }
+        }
+    }
+
+    fn calc_distance(&mut self) {
+        let time_diff = precise_time_ns() - self.timestamp;
+        let distance = (3315 + self.temperature as u64 * 6) * time_diff / 20_000_000;
+        self.distance.store(distance as u32, Ordering::Relaxed);
+
+        #[cfg(debug_assertions)]
+        println!("time diff: {}\tdistance: {}mm", time_diff, distance);
+
+        self.timestamp = 0u64;
+    }
+}

kawaii/kawaii-rs/dependencies/ultrasonic-irq/src/main.rs

@@ -0,0 +1,21 @@
+extern crate shuteye;
+
+extern crate ultrasonic_irq;
+
+use std::time::Duration;
+use std::thread::sleep;
+
+use ultrasonic_irq::Ultrasonic;
+
+static GPIO_PORT_TRIG: u8 = 23;
+static GPIO_PORT_ECHO: u8 = 24;
+static TEMPERATURE: u8 = 20;
+
+fn main() {
+    let ultrasonic = Ultrasonic::new(GPIO_PORT_TRIG, GPIO_PORT_ECHO, TEMPERATURE)
+        .expect("Create Ultrasonic failed");
+
+    println!("{:?}", ultrasonic);
+
+    sleep(Duration::new(10, 0));
+}

kawaii/kawaii-rs/src/emergency_stop.rs

@@ -0,0 +1,27 @@
+extern crate emergency_stop;
+
+use self::emergency_stop::EmergencyStop;
+
+use std::mem::transmute;
+use std::ptr::null_mut;
+use std::sync::atomic::Ordering;
+
+#[no_mangle]
+pub extern "C" fn emergency_stop_init(stop: u8) -> *mut EmergencyStop {
+    match EmergencyStop::new(stop) {
+        Err(_) => null_mut(),
+        Ok(emergency_stop) => unsafe { transmute(Box::new(emergency_stop)) },
+    }
+}
+
+#[no_mangle]
+pub extern "C" fn emergency_stop_clean(emergency_stop: *mut EmergencyStop) {
+    let _emergency_stop: Box<EmergencyStop> = unsafe { transmute(emergency_stop) };
+}
+
+#[no_mangle]
+pub extern "C" fn emergency_stop_get_state(emergency_stop: *mut EmergencyStop) -> bool {
+    let emergency_stop = unsafe { &mut *emergency_stop };
+
+    emergency_stop.state.load(Ordering::Relaxed)
+}

kawaii/kawaii-rs/src/lib.rs

@@ -0,0 +1,13 @@
+#![feature(integer_atomics)]
+
+mod emergency_stop;
+mod ultrasonic_irq;
+
+#[cfg(feature = "measure")]
+mod measure;
+
+pub use emergency_stop::*;
+pub use ultrasonic_irq::*;
+
+#[cfg(feature = "measure")]
+pub use measure::*;

kawaii/kawaii-rs/src/measure.rs

@@ -0,0 +1,48 @@
+extern crate kawaii;
+
+use self::kawaii::Measure;
+
+use std::ffi::CStr;
+use std::ptr::null_mut;
+use std::mem::transmute;
+use std::os::raw::c_char;
+
+#[no_mangle]
+pub extern "C" fn measure_init(name: *const c_char) -> *mut Measure {
+    if name.is_null() {
+        return null_mut();
+    }
+
+    unsafe {
+        match CStr::from_ptr(name).to_str() {
+            Err(_) => null_mut(),
+            Ok(name) => transmute(Box::new(Measure::new(String::from(name)))),
+        }
+    }
+}
+
+#[no_mangle]
+pub extern "C" fn measure_clean(measure: *mut Measure) {
+    let _measure: Box<Measure> = unsafe { transmute(measure) };
+}
+
+#[no_mangle]
+pub extern "C" fn measure_start(measure: *mut Measure) {
+    let measure = unsafe { &mut *measure };
+
+    measure.start();
+}
+
+#[no_mangle]
+pub extern "C" fn measure_pause(measure: *mut Measure) {
+    let measure = unsafe { &mut *measure };
+
+    measure.pause();
+}
+
+#[no_mangle]
+pub extern "C" fn measure_stop(measure: *mut Measure) {
+    let measure = unsafe { &mut *measure };
+
+    measure.stop();
+}

kawaii/kawaii-rs/src/ultrasonic_irq.rs

@@ -0,0 +1,27 @@
+extern crate ultrasonic_irq;
+
+use std::mem::transmute;
+use std::ptr::null_mut;
+use std::sync::atomic::Ordering;
+
+use self::ultrasonic_irq::Ultrasonic;
+
+#[no_mangle]
+pub extern "C" fn ultrasonic_init(trigger: u8, echo: u8, temperature: u8) -> *mut Ultrasonic {
+    match Ultrasonic::new(trigger, echo, temperature) {
+        Err(_) => null_mut(),
+        Ok(ultrasonic) => unsafe { transmute(Box::new(ultrasonic)) },
+    }
+}
+
+#[no_mangle]
+pub extern "C" fn ultrasonic_clean(ultrasonic: *mut Ultrasonic) {
+    let _ultrasonic: Box<Ultrasonic> = unsafe { transmute(ultrasonic) };
+}
+
+#[no_mangle]
+pub extern "C" fn ultrasonic_get_distance(ultrasonic: *mut Ultrasonic) -> u32 {
+    let ultrasonic = unsafe { &mut *ultrasonic };
+
+    ultrasonic.distance.load(Ordering::Relaxed)
+}

kawaii/main.cpp

@@ -0,0 +1,262 @@
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <csignal>
+#include <cstdint>
+#include <iostream>
+#include <random>
+#include <vector>
+
+#include "engine.hpp"
+#include "measure.hpp"
+#include "rfid_reader.hpp"
+#include "ultrasound_sensor.hpp"
+#include "emergency_stop.h"
+
+using namespace std;
+
+typedef chrono::high_resolution_clock clocktype;
+
+const int tickrate = 100;
+const chrono::milliseconds tick_delay = 1000ms / tickrate;
+const uint32_t exit_tag = 0xAE0B1E2B;
+const int no_tags = 3;
+const uint32_t distance_until_turn = 180;
+chrono::milliseconds backoff_time = 100ms;
+chrono::milliseconds turn_right_follow_time = 150ms;
+chrono::milliseconds turn_right_backoff_time = 500ms;
+
+bool stop = false;
+
+enum class mmode
+{
+	STOP,
+	LABYRINTH_FIND_WALL,
+	LABYRINTH_BACKOFF,
+	LABYRINTH_TURN_RIGHT,
+	LABYRINTH_TURN_AWAY,
+	LABYRINTH_FOLLOW,
+	SEARCH_STRAIGHT,
+	SEARCH_TURN_RIGHT,
+	SEARCH_TURN_LEFT,
+	SEARCH_TAG_STOP,
+	HAPPY
+};
+
+void signal_handler(int sig)
+{
+	stop = true;
+}
+
+int main()
+{
+	signal(SIGINT, signal_handler);
+	signal(SIGTERM, signal_handler);
+	engine right(
+			gpio(13, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON),
+			gpio(20, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON)
+			);
+	engine left(
+			gpio(19, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON),
+			gpio(26, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON)
+			);
+	gpio bwleftleft(18, gpio::pin_direction::INPUT, gpio::pin_type::HIGH_ON);
+	gpio bwleftright(4, gpio::pin_direction::INPUT, gpio::pin_type::HIGH_ON);
+	gpio bwrightleft(3, gpio::pin_direction::INPUT, gpio::pin_type::HIGH_ON);
+	gpio bwrightright(2, gpio::pin_direction::INPUT, gpio::pin_type::HIGH_ON);
+	emergency_stop stop_button(25);
+	ultrasound_sensor ultrasoundright(23, 24);
+	ultrasound_sensor ultrasoundleft(12, 6);
+	rfid_reader rfid;
+	// TODO This random is unseeded
+	minstd_rand random;
+
+	clocktype::time_point mmode_end = clocktype::now();
+	mmode mode = mmode::STOP;
+	mmode target_mode = mmode::LABYRINTH_FIND_WALL;
+	clocktype::time_point mode_reset_time;
+
+	int last_tag = 0x00000000;
+	vector<uint32_t> found_tags;
+	found_tags.reserve(no_tags);
+
+	measure measurement("main");
+
+	clocktype::time_point next_tick = clocktype::now();
+	while (!stop)
+	{
+		measurement.start();
+		clocktype::time_point tick_start = clocktype::now();
+		switch (mode)
+		{
+		case mmode::LABYRINTH_FOLLOW:
+		case mmode::LABYRINTH_BACKOFF:
+		case mmode::LABYRINTH_TURN_RIGHT:
+		case mmode::LABYRINTH_TURN_AWAY:
+		case mmode::LABYRINTH_FIND_WALL:
+		{
+			if (tick_start >= mode_reset_time)
+			{
+				switch (mode)
+				{
+				case mmode::LABYRINTH_BACKOFF:
+					target_mode = mmode::LABYRINTH_TURN_AWAY;
+					mode_reset_time = tick_start + turn_right_backoff_time;
+					break;
+				case mmode::LABYRINTH_TURN_AWAY:
+					target_mode = mmode::LABYRINTH_FIND_WALL;
+					break;
+				case mmode::LABYRINTH_TURN_RIGHT:
+					target_mode = mmode::LABYRINTH_FOLLOW;
+					break;
+				}
+			}
+			if (bwrightleft.get_value() || bwrightright.get_value())
+			{
+				target_mode = mmode::LABYRINTH_BACKOFF;
+				mode_reset_time = tick_start + backoff_time;
+			}
+			else if (bwleftleft.get_value() || bwleftright.get_value())
+			{
+				if (mode != mmode::LABYRINTH_BACKOFF)
+				{
+					target_mode = mmode::LABYRINTH_TURN_RIGHT;
+					mode_reset_time = tick_start + turn_right_follow_time;
+				}
+			}
+			int last_id = rfid.last_id();
+			if (last_id == exit_tag)
+			{
+				target_mode = mmode::SEARCH_STRAIGHT;
+				last_tag = last_id;
+			}
+		}
+			break;
+		case mmode::SEARCH_STRAIGHT:
+			{
+				auto usoundright = ultrasoundright.get_value();
+				auto usoundleft = ultrasoundleft.get_value();
+				if (min(usoundright, usoundleft) < distance_until_turn)
+				{
+					if (std::abs((long long int) usoundright - usoundleft) < 50)
+					{
+						int angle = random() % 180 + 90;
+						if (angle <= 180)
+						{
+							target_mode = mmode::SEARCH_TURN_RIGHT;
+						}
+						else
+						{
+							target_mode = mmode::SEARCH_TURN_LEFT;
+							angle = 360 - angle;
+						}
+						mode_reset_time = tick_start + 3ms * angle;
+					}
+					else
+					{
+						int angle = random() % 90 + 90;
+						mode_reset_time = tick_start + 3ms * angle;
+						if (usoundright < usoundleft)
+							target_mode = mmode::SEARCH_TURN_LEFT;
+						else
+							target_mode = mmode::SEARCH_TURN_RIGHT;
+					}
+				}
+				int last_id = rfid.last_id();
+				if (last_id != last_tag && last_id != exit_tag)
+				{
+					last_tag = last_id;
+					if (find(found_tags.begin(), found_tags.end(), last_tag) == found_tags.end())
+					{
+						found_tags.push_back(last_tag);
+						target_mode = mmode::SEARCH_TAG_STOP;
+						mode_reset_time = tick_start + 200ms;
+						if (found_tags.size() == no_tags)
+						{
+							target_mode = mmode::HAPPY;
+							mode_reset_time = tick_start + 2s;
+						}
+					}
+				}
+			}
+			break;
+		case mmode::SEARCH_TURN_LEFT:
+		case mmode::SEARCH_TURN_RIGHT:
+		case mmode::SEARCH_TAG_STOP:
+			if (tick_start >= mode_reset_time)
+			{
+				target_mode = mmode::SEARCH_STRAIGHT;
+			}
+			break;
+		case mmode::HAPPY:
+			if (tick_start >= mode_reset_time)
+			{
+				target_mode = mmode::STOP;
+				stop = true;
+			}
+			break;
+		}
+
+		if (stop_button.get_state())
+		{
+			target_mode = mmode::STOP;
+			stop = true;
+		}
+
+		if (target_mode != mode)
+		{
+			mode = target_mode;
+			switch (mode)
+			{
+			case mmode::STOP:
+				left.set_speed(0);
+				right.set_speed(0);
+				break;
+			case mmode::LABYRINTH_FIND_WALL:
+				left.set_speed(60);
+				right.set_speed(60);
+				break;
+			case mmode::LABYRINTH_BACKOFF:
+				left.set_speed(-100);
+				right.set_speed(-100);
+				break;
+			case mmode::LABYRINTH_TURN_AWAY:
+			case mmode::LABYRINTH_TURN_RIGHT:
+				left.set_speed(100);
+				right.set_speed(-60);
+				break;
+			case mmode::LABYRINTH_FOLLOW:
+				left.set_speed(0);
+				right.set_speed(100);
+				break;
+			case mmode::SEARCH_STRAIGHT:
+				left.set_speed(100);
+				right.set_speed(100);
+				break;
+			case mmode::HAPPY:
+			case mmode::SEARCH_TURN_LEFT:
+				left.set_speed(-100);
+				right.set_speed(100);
+				break;
+			case mmode::SEARCH_TURN_RIGHT:
+				left.set_speed(100);
+				right.set_speed(-100);
+				break;
+			case mmode::SEARCH_TAG_STOP:
+				left.set_speed(-100);
+				right.set_speed(-100);
+				break;
+			}
+		}
+
+		measurement.stop();
+
+		if (!stop)
+		{
+			next_tick += tick_delay;
+			this_thread::sleep_until(next_tick);
+		}
+	}
+
+	return 0;
+}

kawaii/measure.cpp

@@ -0,0 +1,50 @@
+#include "measure.hpp"
+
+#include <cstddef>
+
+#ifdef MEASURE
+extern "C" {
+	void *measure_init(const char *name);
+	void measure_clean(void *measure);
+	void measure_start(void *measure);
+	void measure_pause(void *measure);
+	void measure_stop(void *measure);
+}
+#endif
+
+measure::measure(const std::string &name)
+{
+#ifdef MEASURE
+	measure_ptr = measure_init(name.c_str());
+#endif
+}
+
+measure::~measure()
+{
+#ifdef MEASURE
+    measure_clean(measure_ptr);
+	measure_ptr = NULL;
+#endif
+
+}
+
+void measure::start()
+{
+#ifdef MEASURE
+	measure_start(measure_ptr);
+#endif
+}
+
+void measure::pause()
+{
+#ifdef MEASURE
+	measure_pause(measure_ptr);
+#endif
+}
+
+void measure::stop()
+{
+#ifdef MEASURE
+	measure_stop(measure_ptr);
+#endif
+}

kawaii/measure.hpp

@@ -0,0 +1,22 @@
+#ifndef MEASURE_HPP_
+#define MEASURE_HPP_
+
+#include <string>
+
+class measure
+{
+private:
+#ifdef MEASURE
+	void *measure_ptr;
+#endif
+public:
+	measure(const std::string &);
+	measure(const measure &) = delete;
+	measure(measure &&) = delete;
+	~measure();
+	void start();
+	void stop();
+	void pause();
+};
+
+#endif

kawaii/remote.cpp

@@ -0,0 +1,123 @@
+#include <boost/asio.hpp>
+
+#include <sys/socket.h>
+#include <netdb.h>
+#include <cstdint>
+#include <thread>
+#include <atomic>
+#include <queue>
+#include <mutex>
+#include <chrono>
+#include <condition_variable>
+
+#include "engine.hpp"
+
+using namespace std;
+using boost::asio::ip::tcp;
+using boost::asio::io_service;
+
+template <typename T>
+class prodcon
+{
+private:
+	mutex mut;
+	condition_variable condition;
+	queue<T> q;
+public:
+	void push(T data)
+	{
+		unique_lock<mutex> lock(mut);
+		q.push(data);
+		condition.notify_one();
+	}
+	template<typename _Rep, typename _Period>
+	bool pop(T &result, const chrono::duration<_Rep, _Period> __rtime)
+	{
+		unique_lock<mutex> lock(mut);
+		bool success = condition.wait_for(lock, __rtime, [this] {return !this->q.empty();});
+		if (!success)
+			return false;
+		result = q.front();
+		q.pop();
+		return true;
+	}
+};
+
+class remote_server
+{
+private:
+	tcp::acceptor acceptor;
+	io_service &io;
+	thread engine_thread;
+	atomic<bool> stop_thread;
+	prodcon<pair<int8_t, int8_t>> commands;
+public:
+	remote_server(io_service &io_service);
+	void run();
+	void engine_loop();
+};
+
+int main()
+{
+	io_service io;
+	remote_server server(io);
+	server.run();
+	return 0;
+}
+
+remote_server::remote_server(io_service &io_service)
+	: acceptor(io_service),
+	  io(io_service),
+	  stop_thread(false)
+{
+	// Nothing to do
+}
+
+void remote_server::run()
+{
+	tcp::endpoint endpoint(tcp::v4(), 1337);
+	acceptor.open(endpoint.protocol());
+	acceptor.set_option(tcp::acceptor::reuse_address(true));
+	acceptor.bind(endpoint);
+	acceptor.listen(1);
+	while (true)
+	{
+		tcp::socket socket(io);
+		acceptor.accept(socket);
+		stop_thread = false;
+		engine_thread = thread(&remote_server::engine_loop, this);
+		while (true)
+		{
+			int8_t speed_left, speed_right;
+			socket.read_some(boost::asio::buffer(&speed_left, 1));
+			socket.read_some(boost::asio::buffer(&speed_right, 1));
+			commands.push(pair<int8_t, int8_t>(speed_left, speed_right));
+		}
+	}
+}
+
+void remote_server::engine_loop()
+{
+	engine left(
+			gpio(13, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON),
+			gpio(20, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON)
+			);
+	engine right(
+			gpio(19, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON),
+			gpio(26, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON)
+			);
+	while (!stop_thread)
+	{
+		pair<int8_t, int8_t> command;
+		if (commands.pop(command, 1s))
+		{
+			left.set_speed(command.first);
+			right.set_speed(command.second);
+		}
+		else
+		{
+			left.set_speed(0);
+			right.set_speed(0);
+		}
+	}
+}

kawaii/rfid_reader.cpp

@@ -0,0 +1,61 @@
+#include "rfid_reader.hpp"
+#include <unistd.h>
+#include <iostream>
+#include <chrono>
+#include <ctime>
+#include <chrono>
+
+rfid_reader::rfid_reader()
+	: measurement("rfid_reader")
+{
+	mfrc.PCD_Init();
+	stop_thread = false;
+	uid = 0;
+	thread = std::thread(&rfid_reader::loop, this);
+}
+
+rfid_reader::~rfid_reader()
+{
+	 using namespace std;
+	 stop_thread = true;
+     thread.join();
+}
+
+uint32_t rfid_reader::last_id() const
+{
+	return uid.load(std::memory_order::memory_order_relaxed);
+}
+
+void rfid_reader::loop()
+{
+	using namespace std::chrono;
+	
+	stop_thread = false;
+	while(!stop_thread)
+	{
+		std::this_thread::sleep_for(50ms);
+		measurement.start();
+		if(!mfrc.PICC_IsNewCardPresent())
+		{
+			continue;
+		}
+		if(!mfrc.PICC_ReadCardSerial())
+		{
+			continue;
+		}
+		
+
+		uid.store(int((unsigned char)(mfrc.uid.uidByte[0]) << 24 |
+		      (unsigned char)(mfrc.uid.uidByte[1]) << 16 |
+		      (unsigned char)(mfrc.uid.uidByte[2]) << 8  |
+		      (unsigned char)(mfrc.uid.uidByte[3])), std::memory_order::memory_order_relaxed);
+
+#ifndef NDEBUG
+		printf("\n");
+		std::time_t result = std::time(nullptr);
+		std::cout << std::asctime(std::localtime(&result));
+		printf("%X\n", last_id());
+#endif
+		measurement.stop();
+	}
+}

kawaii/rfid_reader.hpp

@@ -0,0 +1,31 @@
+#ifndef RFIDREADER_HPP_
+#define RFIDREADER_HPP_
+
+#include <cstdint>
+#include <thread>
+#include <chrono>
+#include <atomic>
+
+#include "measure.hpp"
+#include "MFRC522.h"
+
+class rfid_reader
+{
+public:
+	uint32_t last_id() const;
+	rfid_reader();
+	rfid_reader(const rfid_reader &) = delete;
+	rfid_reader(const rfid_reader &&) = delete;
+	~rfid_reader();
+	void loop();
+private:
+	MFRC522 mfrc;
+	std::atomic<uint32_t> uid;
+	std::thread thread;
+	bool stop_thread;
+	measure measurement;
+};
+
+
+
+#endif

kawaii/test.cpp

@@ -0,0 +1,97 @@
+#include <thread>
+#include <chrono>
+#include <sstream>
+#include <iostream>
+
+#include "engine.hpp"
+#include "ultrasound_sensor.hpp"
+
+using namespace std;
+
+enum class drive_mode
+{
+	LEFT,
+	RIGHT,
+	FIND_TURN,
+};
+
+int main()
+{
+	engine right(
+			gpio(13, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON),
+			gpio(20, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON)
+			);
+	engine left(
+			gpio(19, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON),
+			gpio(26, gpio::pin_direction::OUTPUT, gpio::pin_type::LOW_ON)
+			);
+	gpio bwright(2, gpio::pin_direction::INPUT, gpio::pin_type::HIGH_ON);
+	gpio bw1(3, gpio::pin_direction::INPUT, gpio::pin_type::HIGH_ON);
+	gpio bw2(4, gpio::pin_direction::INPUT, gpio::pin_type::HIGH_ON);
+	gpio bwleft(17, gpio::pin_direction::INPUT, gpio::pin_type::HIGH_ON);
+
+	ultrasound_sensor ultrasound(23, 24);
+
+	left.set_speed(-50);
+	right.set_speed(-50);
+	while (ultrasound.get_value() > 200000)
+	{
+		cout << ultrasound.get_value() << endl;
+		this_thread::sleep_for(500ms);
+	}
+	left.set_speed(0);
+	right.set_speed(0);
+
+/*	auto stop_time = chrono::high_resolution_clock::now() + 1min;
+	drive_mode mode = drive_mode::LEFT;
+	right.set_speed(50);
+	left.set_speed(10);
+	while (stop_time > chrono::high_resolution_clock::now())
+	{
+		int found_corners = bwright.get_value() + bwleft.get_value() + bw1.get_value() + bw2.get_value();
+		if (mode == drive_mode::FIND_TURN)
+		{
+			if (!bwright.get_value())
+			{
+				right.set_speed(100);
+				left.set_speed(0);
+				mode = drive_mode::LEFT;
+			}
+			else if (!bwleft.get_value())
+			{
+				right.set_speed(0);
+				left.set_speed(100);
+				mode = drive_mode::RIGHT;
+			}
+		}
+		else if (found_corners >= 3 || (bwright.get_value() && bwleft.get_value()))
+		{
+			left.set_speed(100);
+			right.set_speed(100);
+			mode = drive_mode::FIND_TURN;
+		}
+		else if (mode == drive_mode::LEFT)
+		{
+			if (bwright.get_value())
+			{
+				left.set_speed(100);
+				right.set_speed(0);
+				mode = drive_mode::RIGHT;
+			}
+		}
+		else if (mode == drive_mode::RIGHT)
+		{
+			if (bwleft.get_value())
+			{
+				left.set_speed(0);
+				right.set_speed(100);
+				mode = drive_mode::LEFT;
+			}
+		}
+		this_thread::sleep_for(1ms);
+	}
+	right.set_speed(0);
+	left.set_speed(0);
+*/
+	return 0;
+}

kawaii/ultrasound_sensor.cpp

@@ -0,0 +1,25 @@
+#include "ultrasound_sensor.hpp"
+
+#include <cstddef>
+
+extern "C" {
+	void *ultrasonic_init(uint8_t trigger, uint8_t echo, uint8_t temperature);
+	void ultrasonic_clean(void *ultrasonic);
+	uint32_t ultrasonic_get_distance(void *ultrasonic);
+}
+
+ultrasound_sensor::ultrasound_sensor(uint8_t trigger, uint8_t echo, uint8_t temprature)
+{
+	ultrasonic = ultrasonic_init(trigger, echo, temprature);
+}
+
+ultrasound_sensor::~ultrasound_sensor()
+{
+    ultrasonic_clean(ultrasonic);
+	ultrasonic = NULL;
+}
+
+uint32_t ultrasound_sensor::get_value() const
+{
+	return ultrasonic_get_distance(ultrasonic);
+}

kawaii/ultrasound_sensor.hpp

@@ -0,0 +1,19 @@
+#ifndef ULTRASOUND_SENSOR_HPP_
+#define ULTRASOUND_SENSOR_HPP_
+
+#include <cstdint>
+
+class ultrasound_sensor
+{
+private:
+	void *ultrasonic;
+public:
+	ultrasound_sensor(uint8_t trigger, uint8_t echo, uint8_t temprature = 20);
+	ultrasound_sensor(const ultrasound_sensor &) = delete;
+	ultrasound_sensor(ultrasound_sensor &&) = delete;
+	uint32_t get_value() const;
+	~ultrasound_sensor();
+};
+
+
+#endif