Commits

bsaid committed d5eb2fc

New math functions for stabilization.
(vectors and fractions)

Comments (0)

Files changed (4)

firmware/YuniflyLibraries/firmware.aps

-<AVRStudio><MANAGEMENT><ProjectName>firmware</ProjectName><Created>30-Sep-2011 18:15:16</Created><LastEdit>29-Oct-2011 19:44:12</LastEdit><ICON>241</ICON><ProjectType>0</ProjectType><Created>30-Sep-2011 18:15:16</Created><Version>4</Version><Build>4, 15, 0, 623</Build><ProjectTypeName>AVR GCC</ProjectTypeName></MANAGEMENT><CODE_CREATION><ObjectFile>default\firmware.elf</ObjectFile><EntryFile></EntryFile><SaveFolder>D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\</SaveFolder></CODE_CREATION><DEBUG_TARGET><CURRENT_TARGET>AVR Simulator</CURRENT_TARGET><CURRENT_PART>ATmega128.xml</CURRENT_PART><BREAKPOINTS></BREAKPOINTS><IO_EXPAND><HIDE>false</HIDE></IO_EXPAND><REGISTERNAMES><Register>R00</Register><Register>R01</Register><Register>R02</Register><Register>R03</Register><Register>R04</Register><Register>R05</Register><Register>R06</Register><Register>R07</Register><Register>R08</Register><Register>R09</Register><Register>R10</Register><Register>R11</Register><Register>R12</Register><Register>R13</Register><Register>R14</Register><Register>R15</Register><Register>R16</Register><Register>R17</Register><Register>R18</Register><Register>R19</Register><Register>R20</Register><Register>R21</Register><Register>R22</Register><Register>R23</Register><Register>R24</Register><Register>R25</Register><Register>R26</Register><Register>R27</Register><Register>R28</Register><Register>R29</Register><Register>R30</Register><Register>R31</Register></REGISTERNAMES><COM>Auto</COM><COMType>0</COMType><WATCHNUM>0</WATCHNUM><WATCHNAMES><Pane0></Pane0><Pane1></Pane1><Pane2></Pane2><Pane3></Pane3></WATCHNAMES><BreakOnTrcaeFull>0</BreakOnTrcaeFull></DEBUG_TARGET><Debugger><Triggers></Triggers></Debugger><AVRGCCPLUGIN><FILES><SOURCEFILE>firmware.c</SOURCEFILE><OTHERFILE>default\firmware.lss</OTHERFILE><OTHERFILE>default\firmware.map</OTHERFILE></FILES><CONFIGS><CONFIG><NAME>default</NAME><USESEXTERNALMAKEFILE>NO</USESEXTERNALMAKEFILE><EXTERNALMAKEFILE></EXTERNALMAKEFILE><PART>atmega128</PART><HEX>1</HEX><LIST>1</LIST><MAP>1</MAP><OUTPUTFILENAME>firmware.elf</OUTPUTFILENAME><OUTPUTDIR>default\</OUTPUTDIR><ISDIRTY>0</ISDIRTY><OPTIONS><OPTION><FILE>firmware.c</FILE><OPTIONLIST></OPTIONLIST></OPTION></OPTIONS><INCDIRS/><LIBDIRS/><LIBS/><LINKOBJECTS/><OPTIONSFORALL>-Wall -gdwarf-2   -x c++  -DF_CPU=8000000UL -Os -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums</OPTIONSFORALL><LINKEROPTIONS></LINKEROPTIONS><SEGMENTS/></CONFIG></CONFIGS><LASTCONFIG>default</LASTCONFIG><USES_WINAVR>1</USES_WINAVR><GCC_LOC>C:\WinAVR-20100110\bin\avr-gcc.exe</GCC_LOC><MAKE_LOC>C:\WinAVR-20100110\utils\bin\make.exe</MAKE_LOC></AVRGCCPLUGIN><ProjectFiles><Files><Name>D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\firmware.c</Name></Files></ProjectFiles><IOView><usergroups/><sort sorted="0" column="0" ordername="0" orderaddress="0" ordergroup="0"/></IOView><Files><File00000><FileId>00000</FileId><FileName>firmware.c</FileName><Status>1</Status></File00000><File00001><FileId>00001</FileId><FileName>uart.hpp</FileName><Status>1</Status></File00001><File00002><FileId>00002</FileId><FileName>accelerometer.hpp</FileName><Status>1</Status></File00002><File00003><FileId>00003</FileId><FileName>paralTransmitter.hpp</FileName><Status>1</Status></File00003><File00004><FileId>00004</FileId><FileName>fastPWM.hpp</FileName><Status>1</Status></File00004><File00005><FileId>00005</FileId><FileName>led.hpp</FileName><Status>1</Status></File00005><File00006><FileId>00006</FileId><FileName>stabilization.hpp</FileName><Status>1</Status></File00006></Files><Events><Bookmarks></Bookmarks></Events><Trace><Filters></Filters></Trace></AVRStudio>
+<AVRStudio><MANAGEMENT><ProjectName>firmware</ProjectName><Created>30-Sep-2011 18:15:16</Created><LastEdit>02-Nov-2011 18:08:47</LastEdit><ICON>241</ICON><ProjectType>0</ProjectType><Created>30-Sep-2011 18:15:16</Created><Version>4</Version><Build>4, 15, 0, 623</Build><ProjectTypeName>AVR GCC</ProjectTypeName></MANAGEMENT><CODE_CREATION><ObjectFile>default\firmware.elf</ObjectFile><EntryFile></EntryFile><SaveFolder>D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\</SaveFolder></CODE_CREATION><DEBUG_TARGET><CURRENT_TARGET>AVR Simulator</CURRENT_TARGET><CURRENT_PART>ATmega128.xml</CURRENT_PART><BREAKPOINTS></BREAKPOINTS><IO_EXPAND><HIDE>false</HIDE></IO_EXPAND><REGISTERNAMES><Register>R00</Register><Register>R01</Register><Register>R02</Register><Register>R03</Register><Register>R04</Register><Register>R05</Register><Register>R06</Register><Register>R07</Register><Register>R08</Register><Register>R09</Register><Register>R10</Register><Register>R11</Register><Register>R12</Register><Register>R13</Register><Register>R14</Register><Register>R15</Register><Register>R16</Register><Register>R17</Register><Register>R18</Register><Register>R19</Register><Register>R20</Register><Register>R21</Register><Register>R22</Register><Register>R23</Register><Register>R24</Register><Register>R25</Register><Register>R26</Register><Register>R27</Register><Register>R28</Register><Register>R29</Register><Register>R30</Register><Register>R31</Register></REGISTERNAMES><COM>Auto</COM><COMType>0</COMType><WATCHNUM>0</WATCHNUM><WATCHNAMES><Pane0></Pane0><Pane1></Pane1><Pane2></Pane2><Pane3></Pane3></WATCHNAMES><BreakOnTrcaeFull>0</BreakOnTrcaeFull></DEBUG_TARGET><Debugger><Triggers></Triggers></Debugger><AVRGCCPLUGIN><FILES><SOURCEFILE>firmware.c</SOURCEFILE><OTHERFILE>default\firmware.lss</OTHERFILE><OTHERFILE>default\firmware.map</OTHERFILE></FILES><CONFIGS><CONFIG><NAME>default</NAME><USESEXTERNALMAKEFILE>NO</USESEXTERNALMAKEFILE><EXTERNALMAKEFILE></EXTERNALMAKEFILE><PART>atmega128</PART><HEX>1</HEX><LIST>1</LIST><MAP>1</MAP><OUTPUTFILENAME>firmware.elf</OUTPUTFILENAME><OUTPUTDIR>default\</OUTPUTDIR><ISDIRTY>0</ISDIRTY><OPTIONS><OPTION><FILE>firmware.c</FILE><OPTIONLIST></OPTIONLIST></OPTION></OPTIONS><INCDIRS/><LIBDIRS/><LIBS/><LINKOBJECTS/><OPTIONSFORALL>-Wall -gdwarf-2   -x c++  -DF_CPU=8000000UL -Os -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums</OPTIONSFORALL><LINKEROPTIONS></LINKEROPTIONS><SEGMENTS/></CONFIG></CONFIGS><LASTCONFIG>default</LASTCONFIG><USES_WINAVR>1</USES_WINAVR><GCC_LOC>C:\WinAVR-20100110\bin\avr-gcc.exe</GCC_LOC><MAKE_LOC>C:\WinAVR-20100110\utils\bin\make.exe</MAKE_LOC></AVRGCCPLUGIN><ProjectFiles><Files><Name>D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\firmware.c</Name></Files></ProjectFiles><IOView><usergroups/><sort sorted="0" column="0" ordername="1" orderaddress="1" ordergroup="1"/></IOView><Files><File00000><FileId>00000</FileId><FileName>firmware.c</FileName><Status>1</Status></File00000><File00001><FileId>00001</FileId><FileName>uart.hpp</FileName><Status>1</Status></File00001><File00002><FileId>00002</FileId><FileName>accelerometer.hpp</FileName><Status>1</Status></File00002><File00003><FileId>00003</FileId><FileName>paralTransmitter.hpp</FileName><Status>1</Status></File00003><File00004><FileId>00004</FileId><FileName>fastPWM.hpp</FileName><Status>1</Status></File00004><File00005><FileId>00005</FileId><FileName>led.hpp</FileName><Status>1</Status></File00005><File00006><FileId>00006</FileId><FileName>stabilization.hpp</FileName><Status>1</Status></File00006></Files><Events><Bookmarks></Bookmarks></Events><Trace><Filters></Filters></Trace></AVRStudio>

firmware/YuniflyLibraries/firmware.aws

-<AVRWorkspace><IOSettings><CurrentRegisters/></IOSettings><part name="ATMEGA128"/><Files><File00000 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\firmware.c" Position="176 44 1512 824" LineCol="38 0" State="Maximized"/></Files></AVRWorkspace>
+<AVRWorkspace><IOSettings><CurrentRegisters/></IOSettings><part name="ATMEGA128"/><Files><File00000 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\firmware.c" Position="186 76 1260 594" LineCol="30 0"/><File00001 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\uart.hpp" Position="211 101 1302 614" LineCol="14 0"/><File00002 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\accelerometer.hpp" Position="236 126 1327 639" LineCol="38 0"/><File00003 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\paralTransmitter.hpp" Position="261 151 1352 664" LineCol="38 0"/><File00004 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\fastPWM.hpp" Position="286 176 1377 689" LineCol="38 0"/><File00005 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\led.hpp" Position="311 201 1402 714" LineCol="38 0"/><File00006 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\stabilization.hpp" Position="336 226 1427 739" LineCol="171 63"/></Files></AVRWorkspace>

firmware/YuniflyLibraries/firmware.c

 		debug.wait();
 		debug << endl;*/
 
-		_delay_ms(1);
+		//_delay_ms(1);
 		setFastPWM(0, RCrec[RCservoL]);
 		setFastPWM(1, RCrec[RCservoR]);
 		setFastPWM(2, RCrec[RCmotor]);

firmware/YuniflyLibraries/stabilization.hpp

 #define accNumberOfChannels 8
 #define historyLength 5
 
+
+int32_t GCD(int32_t u, int32_t v)
+{
+	if(v>u)
+	{
+		int32_t swap = u;
+		u = v;
+		v = swap;
+	}
+	while(v!=0)
+	{
+		int32_t r = u%v;
+		u = v;
+		v = r;
+	}
+	return u;
+}
+
+
+class Tfraction
+{
+public:
+	
+	int32_t numer;	//numerator
+	int32_t denom;	//denominator
+
+	Tfraction()
+	{
+		numer = 0;
+		denom = 1;
+	}
+
+	void set(int32_t cit, int32_t jmen)
+	{
+		if(jmen == 0)
+			jmen = 1;
+		numer = cit;
+		denom = jmen;
+	}
+
+	int32_t toInt()
+	{
+		return numer / denom;
+	}
+
+	void toBasicShape()
+	{
+		int32_t nejDelitel = GCD(numer, denom);
+		numer /= nejDelitel;
+		denom /= nejDelitel;
+	}
+
+	Tfraction operator + (Tfraction param)
+	{
+		Tfraction temp;
+		temp.numer = numer * param.denom + param.numer * denom;
+		temp.denom = denom * param.denom;
+		temp.toBasicShape();
+		return temp;
+	}
+
+	Tfraction operator - (Tfraction param)
+	{
+		Tfraction temp;
+		temp.numer = numer * param.denom - param.numer * denom;
+		temp.denom = denom * param.denom;
+		temp.toBasicShape();
+		return temp;
+	}
+
+	Tfraction operator * (Tfraction param)
+	{
+		Tfraction temp;
+		temp.numer = numer * param.numer;
+		temp.denom = denom * param.denom;
+		temp.toBasicShape();
+		return temp;
+	}
+
+	Tfraction operator / (Tfraction param)
+	{
+		Tfraction temp;
+		temp.numer = numer * param.denom;
+		temp.denom = denom * param.numer;
+		temp.toBasicShape();
+		return temp;
+	}
+
+	void operator = (int32_t param)
+	{
+		numer = param;
+		denom = 1;
+	}
+};
+
+
+Tfraction sqrt(Tfraction param)
+{
+	return param;	//todo: this function
+}
+
+Tfraction arcusCos(Tfraction param)
+{
+	//todo: this function
+	return param;
+}
+
+
+class Tvector
+{
+public:
+	
+	Tfraction x;
+	Tfraction y;
+	Tfraction z;
+
+	Tvector(int32_t a=0, int32_t b=0, int32_t c=0)
+	{
+		x = a;
+		y = b;
+		z = c;
+	}
+	
+	void set(Tfraction a, Tfraction b, Tfraction c)
+	{
+		x = a;
+		y = b;
+		z = c;
+	}
+
+	Tfraction size()
+	{
+		return sqrt( x*x + y*y + z*z );
+	}
+
+	Tvector operator + (Tvector param)
+	{
+		Tvector temp;
+		temp.x = x + param.x;
+		temp.y = y + param.y;
+		temp.z = z + param.z;
+		return temp;
+	}
+};
+
+
+Tfraction scalarProduct(Tvector u, Tvector v)
+{
+	return u.x * v.x + u.y * v.y + u.z * v.z;
+}
+
+
+Tfraction getAngle(Tvector u, Tvector v)
+{
+	Tfraction temp;
+	temp = scalarProduct(u,v) / ( u.size() * v.size() );
+	return arcusCos(temp);
+}
+
+
+Tvector getRotation(Tvector u, Tvector v)
+{
+	Tvector out;
+	Tvector tempU;
+	Tvector tempV;
+
+	tempU.set(0, u.y, u.z);		//todo: integer constant can be added
+	tempV.set(0, v.y, v.z);
+	out.x = getAngle(tempU, tempV);
+
+	tempU.set(u.x, 0, u.z);
+	tempV.set(v.x, 0, v.z);
+	out.y = getAngle(tempU, tempV);
+
+	tempU.set(u.x, u.y, 0);
+	tempV.set(v.x, v.y, 0);
+	out.z = getAngle(tempU, tempV);
+
+	return out;
+}
+
+
 class TaccHistory
 {
 	int history[historyLength][accNumberOfChannels];