PosSpeedCtl

/***
 *    _________     _____ __________  ___________    .__            _____                  _______      _______  ____
 *    \_   ___ \   /  _  \\______   \ \__    ___/___ |  |   ____   /     \ _____    ____   \   _  \     \   _  \/_   |
 *    /    \  \/  /  /_\  \|    |  _/   |    |_/ __ \|  | _/ __ \ /  \ /  \\__  \  /    \  /  /_\  \    /  /_\  \|   |
 *    \     \____/    |    \    |   \   |    |\  ___/|  |_\  ___//    Y    \/ __ \|   |  \ \  \_/   \   \  \_/   \   |
 *     \______  /\____|__  /______  /   |____| \___  >____/\___  >____|__  (____  /___|  /  \_____  / /\ \_____  /___|
 *            \/         \/       \/               \/          \/        \/     \/     \/         \/  \/       \/
 */

#include <stdint.h>
#include <stdbool.h>
#include <driverlib/rom.h>
#include "Sensors/QEIModule.h"
#include  "Actuators/PWMModule.h"
#include "utils/ControlScheme.h"
#include "utils/RGBDriver.h"
#include "Controllers/SpeedController.h"
#include "Controllers/PositionController.h"
#include "utils/MainMenu.h"
#include "drivers/buttons.h"
#include "ConfigTiva.h"
#include "Communication/UARTModule.h"
#include "Storage/USBStickModule.h"
#include "Communication/UARTCmds.h"

//#define TARGET_IS_BLIZZARD_RB1

uint32_t refIter = 0;

int main(void) {

	//
	// Enable lazy stacking for interrupt handlers.  This allows floating-point
	// instructions to be used within interrupt handlers, but at the expense of
	// extra stack usage.
	//
	ROM_FPULazyStackingEnable();
	ROM_FPUEnable();

	// Initialize system Clock
	TivaCfg_SysClk();

	// Config RGB
	RGB_Config();

	// Initialize buttons
	ButtonsInit();

	// Config QEI
	QEI_Config(COMPUTE_EACH_PULSE);

	// Current Transducer config
	//CurrentT_Config();

	uint32_t speedCtlFS = 100;

	ClassPWM pwmModule;
	PWM_Construct(&pwmModule, 0, 25000, 12, speedCtlFS);

	ClassCtlScheme ctlScheme;
	CtlScheme_Construct(&ctlScheme, &pwmModule, speedCtlFS);

	ClassUSBStick stickStorage;
	USBStick_Construct(&stickStorage, speedCtlFS, LOGGER);
	USBStick_Enable(&stickStorage);
	while (_deviceState != eSTATE_DEVICE_READY) {
		USBStick_Run(&stickStorage);
	}
	//USBStick_Disable(&stickStorage);

	ClassUART uartModule;
	UART_Construct(&uartModule, 1, 115200, SysCtlClockGet(), 500, LOGGER);

	CtlScheme_AddComModule(&ctlScheme, &stickStorage._USBStickHandler);
	//CtlScheme_AddComModule(&ctlScheme, &uartModule._uartHandeler);

	float speedKP = 3.2;
	float speedKI = 4;
	float speedSat = 11.4;
	float speedISat = 11.4;

	ClassSpeedCtl speedCtl;
	SpeedCtl_Construct(&speedCtl, speedCtlFS, speedKP, speedKI, 0, speedSat,
			speedISat);

	float posKP = 2;
	float posKI = 0;
	float posSat = 1;
	float posISat = 2;

	ClassPosCtl posCtl;
	PosCtl_Construct(&posCtl, speedCtlFS / 10, posKP, posKI, 0, posSat, posISat);

	CtlScheme_Connect(&ctlScheme, &speedCtl._speedHandler,
			&pwmModule._pwmHandler);
	CtlScheme_Connect(&ctlScheme, &posCtl._posHandler, &speedCtl._speedHandler);

	float posRef = -PI / 4;

	posCtl._posIOPorts._inPortsAdder[0] = &posRef;
	posCtl._posIOPorts._inAdderPortsNum++;

	USBStick_AddVar2Stream(&stickStorage, &ctlScheme._sysTime, "Time");
	USBStick_AddVar2Stream(&stickStorage, &posCtl._posPID._ctlVar, "Pos");
	USBStick_AddVar2Stream(&stickStorage, &posCtl._posPID._ctlSignalP, "Ppos");
	USBStick_AddVar2Stream(&stickStorage, &speedCtl._speedPID._ctlVar, "Speed");
	USBStick_AddVar2Stream(&stickStorage, &speedCtl._speedPID._ctlSignalI, "Isp");
	USBStick_AddVar2Stream(&stickStorage, &speedCtl._speedPID._ctlSignal, "DCV");

	CtlScheme_ConfigSafe(&ctlScheme, PI /2);
	//USBStick_AddVar2Stream(&stickStorage, &speedCtl._current, "Current");

//	UART_AddVar2Stream(&uartModule, &posCtl._posPID._ctlVar, "Pos");
//	UART_AddVar2Stream(&uartModule, &speedCtl._speedPID._ctlVar, "Speed");
//	UART_AddVar2Stream(&uartModule, &speedCtl._speedPID._ctlSignal, "DCV");

	UART_Enable(&uartModule);
//
//	UART_PrintMenu(&uartModule);
//	UART_PrintHeader(&uartModule);

	UARTCmd_Help(0,0);

	while (1) {

		if (CtlScheme_IsRunning(&ctlScheme) == false) {

			USBStick_Run(&stickStorage);

			UARTCmd_runCmdLine();
		}

		runMenu(&ctlScheme);

		if (refIter >= 300000 * 6) {

			refIter = 0;

			posRef *= -1;
		} else {

			refIter++;
		}
	}
}