Ball Follower

#include "hFramework.h"
#include "hCloudClient.h"
#include "Hitechnic_IRSeeker.h"

using namespace hFramework;
using namespace hSensors;

//setting default PID values
float Kp = 1.4;
float Ki = 0.0;
float Kd = 0.002;
float Kp2 = 1.0;
float Ki2 = 0.0;
float Kd2 = 0.0;

void statusTask()
{
	while (1) {
		platform.ui.label("lb_bat").setText("%2f V", sys.getSupplyVoltage());
		platform.ui.progressBar("pb_bat").setValue(sys.getSupplyVoltageMV() / 15);
		platform.ui.label("l1").setText("hMot1 enc = %d", hMot1.getEncoderCnt());
		platform.ui.label("l2").setText("hMot3 enc = %d", hMot3.getEncoderCnt());

		sys.delay(200);
	}
}

void cfgHandler()
{
	auto l1 = platform.ui.label("l1");
	auto l2 = platform.ui.label("l2");
	auto b = platform.ui.button("btn1");

	platform.ui.loadHtml({Resource::WEBIDE, "/ui.html"});
	platform.ui.video.enable();
}

void onKeyEvent(KeyEventType type, KeyCode code)
{
	//steering PID values
	switch (code) {
	case KeyCode::Key_T:
		if (type == KeyEventType::Pressed) {
			platform.ui.console("cl1").printf(" pressed");
			hServoModule.s2.setWidth(1300); // 1.3 ms width
		} else {
			platform.ui.console("cl1").printf(" released");
			hServoModule.s2.setWidth(2300); // 2.3 ms width
		}

		break;
	//P,L,I,J,D,X changing angle PID values
	case KeyCode::Key_P:
		if (type == KeyEventType::Pressed) {
			Kp += 0.1;
		}
		break;
	case KeyCode::Key_L:
		if (type == KeyEventType::Pressed) {
			Kp -= 0.1;
		}
		break;
	case KeyCode::Key_I:
		if (type == KeyEventType::Pressed) {
			Ki += 0.001;
		}
		break;
	case KeyCode::Key_J:
		if (type == KeyEventType::Pressed) {
			Ki -= 0.001;
		}
		break;
	case KeyCode::Key_D:
		if (type == KeyEventType::Pressed) {
			Kd += 0.001;
		}
		break;
	case KeyCode::Key_X:
		if (type == KeyEventType::Pressed) {
			Kd -= 0.001;
		}
		break;
	//O,K,U,H,S,Z changing distance PID values
	case KeyCode::Key_O:
		if (type == KeyEventType::Pressed) {
			Kp2 += 0.1;
		}
		break;
	case KeyCode::Key_K:
		if (type == KeyEventType::Pressed) {
			Kp2 -= 0.1;
		}
		break;
	case KeyCode::Key_U:
		if (type == KeyEventType::Pressed) {
			Ki2 += 0.001;
		}
		break;
	case KeyCode::Key_H:
		if (type == KeyEventType::Pressed) {
			Ki2 -= 0.001;
		}
		break;
	case KeyCode::Key_S:
		if (type == KeyEventType::Pressed) {
			Kd2 += 0.001;
		}
		break;
	case KeyCode::Key_Z:
		if (type == KeyEventType::Pressed) {
			Kd2 -= 0.001;
		}
		break;
	default: break;
	}
}

void hMain()
{
	platform.begin(&RPi);
	platform.ui.configHandler = cfgHandler;
	platform.ui.onKeyEvent = onKeyEvent;
	platform.ui.setProjectId("b5ec86bafdf797fb");

	sys.setLogDev(&Serial);
	sys.disableSyslog();

	sys.taskCreate(statusTask, 2, 1000, 0);

	hMot1.setSlewRate(0.1);
	hMot2.setSlewRate(0.1);
	hMot3.setSlewRate(0.1);
	hMot4.setSlewRate(0.1);

	hServoModule.enablePower();

	//creating sensor object
	hLegoSensor_i2c s(hSens1);
	Hitechnic_IRSeeker sensor(s);

	//creating PID objects
	hPID pidAngle;        //angle PID
	hPID pidDistance;     //distance PID
	float v = 0;

	for (;;) {
		int mot1 = 0;   //values steering motors
		int mot2 = 0;

		sys.delay(50);  // 50ms
		LED2.toggle();
		//geting values from sensor
		Hitechnic_IRSeeker::data val;
		if (sensor.read(&val) == Hitechnic_IRSeeker::ERROR_PROTO)
			continue;

		LED1.toggle();
		//summing up signal from all five sensors
		float suma = 0;
		float nowe[5];
		for (int i = 0; i < 5; i++) {
			suma += val.dcValues[i];
		}
		//normalizing sensors data
		for (int i = 0; i < 5; i++) {
			nowe[i] = (val.dcValues[i]) / suma;
		}
		//calculate average signal strength
		v = 0.65 * v + 0.35 * suma;
		float wynik;
		//calculate average signal from all five sensors
		wynik = (-2 * nowe[0]) +
		        (-1 * nowe[1]) +
		        (1 * nowe[3]) +
		        (2 * nowe[4]);

		//writing values to PID regulators
		pidAngle.setKP(Kp);
		pidAngle.setKI(Ki);
		pidAngle.setKD(Kd);
		pidDistance.setKP(Kp2);
		pidDistance.setKI(Ki2);
		pidDistance.setKD(Kd2);

		//generating steering signal value (angle PID)
		float angle = pidAngle.update(wynik, 50);

		//generatinc steering signal value (distance PID)
		float distance = pidDistance.update(v - 25, 50);

		//writing steering signals to property variable
		mot1 = angle * 300 + distance * 20;
		mot2 = angle * 300 + -distance * 20;

		//sendind steering values to motors
		hMot1.setPower(mot1);
		hMot2.setPower(mot2);
		hMot3.setPower(mot1);
		hMot4.setPower(mot2);

		//writing in robots concole PID values
		platform.ui.console("cl1").printf(
		    "\r\n odczyty: %d %d %d %d %d",
		    val.dcValues[0],
		    val.dcValues[1],
		    val.dcValues[2],
		    val.dcValues[3],
		    val.dcValues[4]);
	}
}