PosCurrentCtl

/***
 *    _________     _____ __________  ___________    .__            _____                  _______      _______  ____
 *    \_   ___ \   /  _  \\______   \ \__    ___/___ |  |   ____   /     \ _____    ____   \   _  \     \   _  \/_   |
 *    /    \  \/  /  /_\  \|    |  _/   |    |_/ __ \|  | _/ __ \ /  \ /  \\__  \  /    \  /  /_\  \    /  /_\  \|   |
 *    \     \____/    |    \    |   \   |    |\  ___/|  |_\  ___//    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 "Controllers/CurrentController.h"
#include "utils/MainMenu.h"
#include "drivers/buttons.h"
#include "ConfigTiva.h"
#include "Communication/UARTModule.h"
#include "Storage/USBStickModule.h"
#include "Sensors/CurrentMeas.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();

	// Current Transducer config
	CurrentT_Config();

	uint32_t currCtlFS = 1000;

	ClassPWM pwmModule;
	PWM_Construct(&pwmModule, 0, 30000, 12, currCtlFS);

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

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

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

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

//	float speedKP = 6;
//	float speedKI = 10;
//	float speedSat = 11.4;
//	float speedISat = 11.4;
//
//	ClassSpeedCtl speedCtl;
//	SpeedCtl_Construct(&speedCtl, currCtlFS, speedKP, speedKI, speedSat,
//			speedISat);

//	float currKP = 9;
//	float currKI = 0;
//	float currSat = 2;
//	float currISat = 2;
//
//	ClassPosCtl posCtl;
//	PosCtl_Construct(&posCtl, currCtlFS / 10, currKP, currKI, currSat, currISat);
//

	float currKP = 1;
	float currKI = 6;
	float currSat = 12;
	float currISat = 12;

	ClassCurrentCtl currCtl;
	CurrentCtl_Construct(&currCtl, currCtlFS, currKP, currKI, currSat,
			currISat);

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

	CtlScheme_Connect(&ctlScheme, &currCtl._currentHandler,
			&pwmModule._pwmHandler);

	float CurrRef = -2; //[A]

	currCtl._currentIOPorts._inPortsAdder[0] = &CurrRef;
	currCtl._currentIOPorts._inAdderPortsNum++;

	USBStick_AddVar2Stream(&stickStorage, &currCtl._currentPID._ctlVar,
			"Current");
	USBStick_AddVar2Stream(&stickStorage, &currCtl._currentPID._ctlSignalI,
			"ICurr");
	USBStick_AddVar2Stream(&stickStorage, &currCtl._currentPID._ctlSignalP,
			"Pcurr");
	USBStick_AddVar2Stream(&stickStorage, &currCtl._currentPID._ctlVarRef,
				"Pref");

//	USBStick_AddVar2Stream(&stickStorage, &posCtl._posPID._ctlVar, "Pos");
//	USBStick_AddVar2Stream(&stickStorage, &posCtl._posPID._ctlSignalI, "Ipos");
//	USBStick_AddVar2Stream(&stickStorage, &speedCtl._speedPID._ctlVar, "Speed");
//	USBStick_AddVar2Stream(&stickStorage, &speedCtl._speedPID._ctlSignalI, "Isp");

	//USBStick_AddVar2Stream(&stickStorage, &speedCtl._speedPID._ctlSignal, "DCV");

	//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);

	while (1) {

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

			USBStick_Run(&stickStorage);
		}

		runMenu(&ctlScheme);

		if (refIter >= 300000 * 1 && CtlScheme_IsRunning(&ctlScheme)) {

			refIter = 0;

			CurrRef *= -1;

		} else {

			refIter++;
		}
	}
}