Line Follower

#include <hFramework.h>
#include <stdio.h>
#include "hCloudClient.h"

#include "Mindsensors_LineLeader.h"

using namespace hFramework;
using namespace hSensors;

//Setting default PIDs' values
uint8_t Kp = 40;
uint8_t Ki = 1;
uint8_t Kd = 8;
uint8_t Kpf = 19;
uint8_t Kif = 255;
uint8_t Kdf = 33;
float Kp2 = 4.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 onKeyEvent(KeyEventType type, KeyCode code)
{
	if (type != KeyEventType::Pressed)
		return;
	//steering PID values
	switch (code) {
	//P,L,I,J,D,X changing rotation PID values
	case KeyCode::Key_P: ++Kp;
		break;
	case KeyCode::Key_L: --Kp;
		break;
	case KeyCode::Key_I: ++Ki;
		break;
	case KeyCode::Key_J: --Ki;
		break;
	case KeyCode::Key_D: ++Kd;
		break;
	case KeyCode::Key_X: --Kd;
		break;
	//O,K,U,H,S,Z changing rotation PID's facotrs values
	case KeyCode::Key_O: ++Kpf;
		break;
	case KeyCode::Key_K: --Kpf;
		break;
	case KeyCode::Key_U: ++Kif;
		break;
	case KeyCode::Key_H: --Kif;
		break;
	case KeyCode::Key_S: ++Kdf;
		break;
	case KeyCode::Key_Z: --Kdf;
		break;
	//Y,G,T,F,R,D changing velocity PID's values
	case KeyCode::Key_Y: Kp2 += +0.1;
		break;
	case KeyCode::Key_G: Kp2 -= 0.1;
		break;
	case KeyCode::Key_T: Ki2 += 0.01;
		break;
	case KeyCode::Key_F: Ki2 -= 0.01;
		break;
	case KeyCode::Key_R: Kd2 += 0.01;
		break;
	case KeyCode::Key_D: Kd2 -= 0.01;
		break;
	default: break;
	}
}

void hMain()
{
	platform.begin(&RPi);
	sys.disableSyslog();
	sys.setLogDev(&Serial);
	platform.ui.setProjectId("@@@PROJECT_ID@@@");
	platform.ui.onKeyEvent = onKeyEvent;

	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 sensors' objects
	hLegoSensor_i2c s(hSens1);
	Mindsensors_LineLeader sensor(s);

	hPID pidV;     //velocity PID

	for (;;) {

		//values to steering motors
		int mot1 = 0;
		int mot2 = 0;
		//values to reading rotation PID values
		uint8_t rkp, rkpf, rki, rkif, rkd, rkdf, setp;
		int8_t steering;

		LED1.toggle();

		//setting actual values to rotation PID
		sensor.setPoint(45);
		sensor.setKp(Kp, Kpf);
		sensor.setKi(Ki, Kif);
		sensor.setKd(Kd, Kdf);
		//checking if good values was writted
		sensor.readKp(rkp);
		sensor.readKpFactor(rkpf);
		sensor.readKi(rki);
		sensor.readKiFactor(rkif);
		sensor.readKd(rkd);
		sensor.readKdFactor(rkdf);
		//reading setPoint and steering values from the sensor
		sensor.readsetPoint(setp);
		sensor.readSteering(steering);
		platform.ui.console("cl1").printf("Kp: %d factor: %d\r\nKi: %d factor: %d\r\nKd: %d factor: %d\r\n", rkp, rkpf, rki, rkif, rkd, rkdf);
		platform.ui.console("cl2").printf("ster: %d\r\n", steering);

		//setting actual data to velocity PID
		pidV.setKP(Kp2);
		pidV.setKI(Ki2);
		pidV.setKD(Kd2);
		float vel = pidV.update(steering, 20);
		//calculating final values to motors
		mot1 = (200 - abs(vel) / 2) + 2 * steering;
		mot2 = (-(200 - abs(vel) / 2)) + 2 * steering;
		//sending data to motors
		if (setp == 45) {
			hMot1.setPower(mot1);
			hMot2.setPower(mot1);
			hMot3.setPower(mot2);
			hMot4.setPower(mot2);
		}
		sys.delay_ms(20);   //20ms
	}
}