/***********************************
 * (C) 2008 by Tomasz bla Fortuna <bla@thera.be>.
 * License: GPL3+ (See Docs/LICENSE)
 *
 * Desc: Code snippet reading program configuration out ot EEPROM.
 */

/*
 * Example configuration creator in python:

#!/usr/bin/python

from struct import pack
from os import system

KA_P      = 280
KA_I      = 120
KA_D      = 1100
KA_ISize  = 1300

KM_P      = 0
KM_I      = 0
KM_D      = 0
KM_PP     = 0
KM_ISize  = 100

TravelAngle = 0
AnchorLength = 20
MaxSpeed = 20

data = pack("iiiiiiiiiii",
            KA_P, KA_I, KA_D, KA_ISize,
            KM_P, KM_I, KM_D, KM_PP, KM_ISize,
            TravelAngle, AnchorLength)

f = file("eeprom_image", "w")
f.write(data)
f.close()

system("srec_cat eeprom_image -Binary -Output eeprom_image.srec -Motorola")
system("uisp -dlpt=/dev/ppdev0 -dprog=dapa --segment=eeprom --upload if=eeprom_image.srec")

 */

namespace Cfg {

	/* Type used by balancing PID */
	typedef Long BType;

/* Good values
KA_P      = 280
KA_I      = 120
KA_D      = 1100
KA_ISize  = 1300
KADiv     = 1000
*/



#define FROM_EEPROM 1
#if (FROM_EEPROM)
	static struct {
		BType KA_P;
		BType KA_I;
		BType KA_D;
		BType KA_ISize; /* Limit of the size of a integral part */
		
		BType KM_P;
		BType KM_I;
		BType KM_Id;
		BType KM_D;
		BType KM_PP;
		BType KM_ISize;
		
		BType TravelAngle;
		BType AnchorLength;
	} Data;

	static BType &KA_P = Data.KA_P;
	static BType &KA_I = Data.KA_I;
	static BType &KA_D = Data.KA_D;
	static BType &KA_ISize = Data.KA_ISize; /* Limit of the size of a integral part */

	static BType &KM_P = Data.KM_P;
	static BType &KM_I = Data.KM_I;
	static BType &KM_Id = Data.KM_Id;
	static BType &KM_D = Data.KM_D;
	static BType &KM_PP = Data.KM_PP;
	static BType &KM_ISize = Data.KM_ISize;

	static BType &TravelAngle = Data.TravelAngle;
	static BType &AnchorLength = Data.AnchorLength;

#else
	static const BType KA_P = 150;
	static const BType KA_I = 120;
	static const BType KA_D = 400;
	static const BType KA_ISize = 1000; /* Limit of the size of a integral part */

	static const BType KM_P = 0;
	static const BType KM_I = 0;
	static const BType KM_D = 0;
	static const BType KM_PP = 0;
	static const BType KM_ISize = 255;

	static const BType TravelAngle = 100;
	static const BType AnchorLength = 10;

#endif


	/* Read configuration from EEPROM */
	static inline void Init(void)
	{
#if (FROM_EEPROM)
		eeprom_read_block(&Data, 0, sizeof(Data));
#endif
	}
}