J2S1-Kobuki/src/C++/Driver/src/KobukiDriver/CKobuki.h

//#pragma once
////*************************************************************************************
////*************************************************************************************
//// author Martin Dekan and Peter Beno mail: dekdekan@gmail.com, peter.beno@stuba.sk
////-------------------------------------------------------------------------------------
//// what it is:
//// class for working with the kobuki robot. it should have implemented data reading
//// and sending commands...
//// it does not contain any logic on what to do with the data, it is up to the user to do it in the callback function
//// it is basically just an implementation of communication with the hardware, driver
////*************************************************************************************
////*************************************************************************************
#ifndef KOBUKI_CLASS_123456789
#define KOBUKI_CLASS_123456789
#define PI  3.141592653589793238462643383279502884L /* pi */
#define MS_INSTRUCTION_DELAY 25

#include <stdio.h>
#include <stdlib.h>
#include <vector>
#include "pthread.h"
#include "unistd.h"
#include "fcntl.h"
#include "string.h"
#include <math.h>
#include <stdint.h>
#include <iostream>
#include <fstream>
#include <cmath>
#include <iomanip>
#include <chrono>
#include <sstream>
#include "KobukiParser.h"

using namespace std;

typedef long(*src_callback_kobuki_data) (void *user_data, TKobukiData &Kobuki_data);

class CKobuki
{
public:

	CKobuki() { 
		stopVlakno = 0; 
		std::cout << "kobuki instantiated" << std::endl;
		odometry_log.open("odometry.txt");
	};
	 virtual ~CKobuki() { 
		stopVlakno = 1; 
		close(HCom);
		pthread_cancel(threadHandle); 
		odometry_log.close();
	};
	
	void enableCommands(bool commands) {
		enabledCommands = commands;
	};


	long loop(void *user_data, TKobukiData &Kobuki_data);

	void startCommunication(char *portname,bool CommandsEnabled,void *userDataL);
	int measure(); //thread function, contains an infinite loop and reads data
	void setLed(int led1 = 0, int led2 = 0); //led1 green/red 2/1, //led2 green/red 2/1
	void setTranslationSpeed(int mmpersec);
	void setRotationSpeed(double radpersec);
	void setArcSpeed(int mmpersec,int radius);
	void setSound(int noteinHz, int duration);
	void setPower(int value);

	// control functions 
	void goStraight(long double distance);
	void forward(int speedvalue);
	void doRotation(long double th);
	void goToXy(long double xx, long double yy);
	void Rotate(int degrees);
	std::ofstream odometry_log;
	void robotSafety(std::string *pointerToMessage);
	void robotSafety(); //overload
	void sendNullMessage();
	bool safetyActive = false;
	KobukiParser parser;





private:

	int HCom;
	pthread_t threadHandle; // handle to the thread
	int threadID;  // thread id
	int stopVlakno;
	src_callback_kobuki_data callbackFunction;
	void *userData;
	bool enabledCommands;
	int connect(char *portname);
	unsigned char *readKobukiMessage();
	
	//--start measurement in a new thread (reading runs in a new thread. it needs to be stopped if we want to send a request)
	static void * KobukiProcess(void *param)
	{
		//std::cout << "Our thread Kobuki process started" << std::endl;
		CKobuki *hoku = (CKobuki*)param;
		int output = hoku->measure();
		
		return param;
	}


	// internal variables for robot control
	int prevLeftEncoder, prevRightEncoder; // [ticks]
	uint16_t prevTimestamp;// [ms]
	long totalLeft, totalRight = 0;
	int directionL = 0; // 1 = forward, 0 = undefined, -1 = backwards
	int directionR = 0;
	int iterationCount = 0;
	long double tickToMeter = 0.000085292090497737556558; // [m/tick]
//
	long double x = 0; // [m]
	long double y = 0;
//
	long double theta = 0; // [rad]
	long double b = 0.23; // wheelbase distance in meters, from kobuki manual https://yujinrobot.github.io/kobuki/doxygen/enAppendixProtocolSpecification.html
//
	long double prevGyroTheta = 0;
	long double gyroTheta = 0; // [rad]

	// utilities
	long double gyroToRad(signed short GyroAngle);

	// plot p;
	std::vector<float> vectorX;
	std::vector<float> vectorY;
	std::vector<float> vectorGyroTheta;


	double displacement = 0;
	double integratedGyroTheta = 0;
	double gx = 0;
	double gy = 0;


};

#endif