bsaid avatar bsaid committed 361ac6d

Teacher/pupil system for two transmitters (RC and BT).
Vector stabilization for constant speed driving.
Calibration for an engine regulator.
All will be tested on air.

Comments (0)

Files changed (7)

firmware/YuniflyLibraries/-fraction.hpp

+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;
+	}
+};

firmware/YuniflyLibraries/RCcapture.hpp

 #define RCservoR 7
 #define RCswitch 0
 
-int RCrec[8] = {0,0,0,0,0,0,0,0};
+#define Servo0 (((0-500)>>3)-128)
+int RCrec[8] = {Servo0,Servo0,Servo0,Servo0,Servo0,Servo0,Servo0,Servo0};
 volatile int countTimer = 0;
 
 

firmware/YuniflyLibraries/fastPWM.hpp

 
 int convertServoValue(int value)
 {
+	//((value-500)>>3)-128  obraceny convert
 	return ((value+128)<<3)+500;
 }
 

firmware/YuniflyLibraries/firmware.aws

-<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>
+<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="19 0" State="Maximized"/><File00001 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\uart.hpp" Position="209 99 1300 612" LineCol="14 0" State="Maximized"/><File00002 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\accelerometer.hpp" Position="234 124 1325 637" LineCol="38 0" State="Maximized"/><File00003 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\paralTransmitter.hpp" Position="259 149 1350 662" LineCol="5 19" State="Maximized"/><File00004 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\fastPWM.hpp" Position="284 174 1375 687" LineCol="21 41" State="Maximized"/><File00005 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\led.hpp" Position="309 199 1400 712" LineCol="38 0" State="Maximized"/><File00006 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\stabilization.hpp" Position="334 224 1425 737" LineCol="193 31" State="Maximized"/><File00007 Name="D:\Beda\Aktualni data\Yunifly\Yunifly\YuniflyLibraries\RCcapture.hpp" Position="359 249 1450 762" LineCol="11 71" State="Maximized"/></Files></AVRWorkspace>

firmware/YuniflyLibraries/firmware.c

+#define KUBAS_DEFAULT_STRING_SIZE 255	//todo: delete; only for rs232 freeze test
+
 #include "basic.hpp"
 #include "led.hpp"
 #include "uart.hpp"
 	initLEDs();
 	initFastPWMServo();
 	initRCcapture();
+	accInit();
+
+	Tstabilization plane;
+	setFastPWMServo(2, 120);
+	plane.calibration();
+	setFastPWMServo(2, -120);
 
 	blink();
 
 	for(;;)
 	{
-		/*_delay_ms(100);
-		for(int j=0;j<8;j++)
+		while(!accUpdate());
+		
+		if(RCrec[RCswitch] > convertServoValue(32) )
+		{
+			BTRCupdate();
+			setFastPWMServo(1, BTRCchannels[BTRC_VYSKA]/16 - BTRCchannels[BTRC_KLOP]/16);
+			setFastPWMServo(0, -(BTRCchannels[BTRC_VYSKA]/16 + BTRCchannels[BTRC_KLOP]/16) );
+			setFastPWMServo(2, BTRCchannels[BTRC_POWER]/16);
+		}
+		else if(RCrec[RCswitch] < convertServoValue(-32) )
+		{
+			setFastPWMServo(1, accData[ACC_X]/64-32 + accData[ACC_Y]/64);
+			setFastPWMServo(0, -(accData[ACC_X]/64-32 - accData[ACC_Y]/64) );
+			setFastPWM(2, RCrec[RCmotor]);
+		}
+		else
+		{
+			setFastPWM(0, RCrec[RCservoL]);
+			setFastPWM(1, RCrec[RCservoR]);
+			setFastPWM(2, RCrec[RCmotor]);
+		}
+
+		_delay_ms(10);
+
+
+		/*while(!accUpdate());
+		for(int i=0;i<8;i++)
 		{
 			debug.wait();
-			debug << (((RCrec[j]-500)>>3)-128) << " ";
+			debug << accData[i];
+			debug.wait();
+			debug << " ";
+			_delay_ms(1);
 		}
 		debug.wait();
 		debug << endl;*/
-
-		//_delay_ms(1);
-		setFastPWM(0, RCrec[RCservoL]);
-		setFastPWM(1, RCrec[RCservoR]);
-		setFastPWM(2, RCrec[RCmotor]);
-		debug << (((RCrec[RCmotor]-500)>>3)-128) << endl;
-		debug.wait();
 	}
 }

firmware/YuniflyLibraries/paralTransmitter.hpp

 #ifndef paralTransmitterParser
 #define paralTransmitterParser
 
+#define BTRC_POWER 2
+#define BTRC_KLOP 3
+#define BTRC_VYSKA 0
+#define BTRC_SMER 1
+
 int const BTRCnumberOfChannels = 8;		//only 6 PWMs and 4 channels on transmitter, use 8 for AVR232client displaying
 int const BTRCstartByte = 0xFF;
 int const BTRCservosPosition[BTRCnumberOfChannels] = {0,1,2,3,4,5};		//configuration of PWMs and transmitter channels

firmware/YuniflyLibraries/stabilization.hpp

 //#include "basic.hpp"
+#include <math.h>
 
 #define accNumberOfChannels 8
 #define historyLength 5
+#define ACC_X 6		//todo: is it correct?
+#define ACC_Y 7
+#define ACC_Z 5
 
+#define Tfraction double
 
-int32_t GCD(int32_t u, int32_t v)
+
+/*Tfraction sqrt(Tfraction param)
 {
-	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;
-}
+	return sqrt(param);	//todo: this function
+}*/
 
 
-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;
+	return acos(param);
 }
 
 
 }
 
 
+Tvector vector(Tfraction x, Tfraction y, Tfraction z)
+{
+	Tvector out;
+	out.set(x, y, z);
+	return out;
+}
+
+
 Tvector getRotation(Tvector u, Tvector v)
 {
 	Tvector out;
 }
 
 
-class TaccHistory
+class Thistory
 {
 	int history[historyLength][accNumberOfChannels];
 	int position;
 
 public:
 
-	TaccHistory()
+	Thistory()
 	{
 		position = 0;
 	}	
 	int limited(int value, int min = 0, int max = historyLength)	//todo: this function is incorrect
 	{
 		value -= min;
-		value = value % (max-min);
+		value = value % (max-min);	//todo: correct this
 		value += min;
 		return value;
 	}
 
 class Tstabilization
 {
-	TaccHistory history;
-	TaccHistory differences;
+	Thistory accHistory;
 
-	int gravity;
+	Tfraction g_size;
+	Tvector g_rot;
+	Tvector gravity;
+	Tvector a_vz;
 
 public:
 
 	void calibration()
 	{
-		history.push( accData );
-		//todo: compute differences
+		g_size = 0;
+		for(int i=0;i<128;i++)
+		{
+			while(!accUpdate());
+			accHistory.push( accData );
+			g_size += vector(accData[ACC_X], accData[ACC_Y], accData[ACC_Z]).size();
+			_delay_ms(10);
+		}
+		g_size /= 128;	//todo: as shift
 	}
 	
 	void update()
 	{
-		history.push(accData);	//todo: global accelerometers variables
+		while(!accUpdate());
+		accHistory.push(accData);	//todo: global accelerometers variables
+		//a^2+b^2+(c-x)^2=g^2  =>  x = c+-sqrt(-a^2-b^2+g^2)
+		a_vz.set(0, 0, accData[ACC_Z]-sqrt(-accData[ACC_X]*accData[ACC_X]-accData[ACC_Y]*accData[ACC_Y]+g_size*g_size) );	//todo: two roots +-sqrt()
+		gravity.set(accData[ACC_X], accData[ACC_Y], accData[ACC_Z] - a_vz.z);
+		g_rot = getRotation(gravity, a_vz);
+
+		/*debug.wait();
+		debug << (int32_t)(1000*g_rot.x) << " ";
+		debug.wait();
+		debug << (int32_t)(1000*g_rot.y) << " ";
+		debug.wait();
+		debug << (int32_t)(1000*g_rot.z) << endl;
+		debug.wait();*/
+
+		//setFastPWMServo(1, (int32_t)(16*g_rot.x) + (int32_t)(16*g_rot.y) );
+		//setFastPWMServo(0, -((int32_t)(16*g_rot.x) - (int32_t)(16*g_rot.y)) );
+
+		setFastPWMServo(1, (int32_t)(a_vz.z/128) );
+		debug << (int32_t)(a_vz.z/128) << endl;
+		debug.wait();
+
+		_delay_ms(1);
 	}
 };
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.