Distance Keeper

#include <hFramework.h>
#include <hCloudClient.h>
#include "Mindsensors_IRDistance.h"
#include "Lego_Touch.h"

using namespace hSensors;

//setting default values
bool init = false;
bool dist = false;
int16_t set = 0;
int16_t error = 0;
float t;
float t2;
double t3;
double angletemp;
int16_t angle = 0;
int16_t angle_1 = 0;
float kat = 45;
int16_t tik;

int tt = 0;

void onKeyEvent(KeyEventType type, KeyCode code)
{
	switch (code) {

	//setting required distance between robot and object
	case KeyCode::Key_S:
		if (type == KeyEventType::Pressed) {
			if (dist == false) {
				dist = true;
			} else {
				dist = false;
				set = 0;
			}
		}
		break;
	//setting robot to starting position
	case KeyCode::Key_I:
		if (type == KeyEventType::Pressed) {
			init = false;
			tt = 0;
		}
		break;

	default: break;
	}
}

volatile int desiredTicks = 0;
void th()
{
	char buf[20];
	int pos = 0;
	for (;;) {
		char c;
		int r = Serial.read(&c, 1);
		if (r) {
			if (c == '@') {
				if (pos != 0) {
					buf[pos] = 0;
					desiredTicks = atoi(buf);
					platform.printf("got %s", buf);
				}
				pos = 0;
			} else {
				buf[pos++]  = c;
			}
		}
	}
}

void setangle(float x)
{
	int16_t tik = (x - 45.0) / (90.0 - 45.0) * (180.0 - 75.0) + 75.0;
	hMot1.rotAbs(-tik);
}
float ticktoAngle()
{
	float ticks = -hMot1.getEncoderCnt();
	return (ticks - 75.0) / (180.0 - 75.0) * (90.0 - 45.0) + 45.0;
}
void hMain()
{
	platform.begin(&RPi);
	sys.disableSyslog();
	sys.setLogDev(&Serial);
	platform.ui.setProjectId("@@@PROJECT_ID@@@");
	platform.ui.onKeyEvent = onKeyEvent;

	//creating sensors' objects
	hLegoSensor_i2c s(hSens1);
	Mindsensors_IRDistance sensor(s);

	hLegoSensor_serial s2(hSens3);
	Lego_Touch sensor2(s2);

	sys.taskCreate(th);

	float v = 0;

	hMot1.rotAbs(0);
	for (;;) {
		LED2.toggle();
		//getting value from encoder
		sys.delay(10);  // 50ms

		//if robot is not initialized set it to starting position
		if (!init) {
			tt++;
			hMot1.rotAbs(tt);
			if (sensor2.isReleased()) {

			}
			if (sensor2.isPressed()) {
				hMot1.resetEncoderCnt();
				init = true;
			}
		} else {

			int16_t realvalue;
			sensor.readReal(realvalue);
			float dist = (float)realvalue / 10.0f;

			float enc_angl = ticktoAngle();

			float a = 13;
			float d = dist - 15;

			float v1 = d / a + (cosf(enc_angl * 3.14 / 180.0));
			if (v1 > 1)
				v1 = 1;
			if (v1 < -1)
				v1 = -1;

			float v = acosf(v1) * 180.0 / 3.14;

			float dif = v - enc_angl;
			float mm = 3;
			if (dif > mm)
				dif = mm;
			if (dif < -mm)
				dif = -mm;

			Serial.printf("dist %f v1 %f ang-rotabs %f ang+enc %f\r\n",
			              dist, v1, v, enc_angl);
			setangle(enc_angl + dif);

		}
	}
}