#include?“BalanceCar.h“

//////////////////速度PI////////////////////
double?BalanceCar::speedpiout（double?kpsdouble?kisdouble?kdsint?fint?bdouble?p0）
{
	float?speeds?=?（pulseleft?+?pulseright）?*?1.0;??????????????????????????????????????????????//車速?脈沖值
	pulseright?=?pulseleft?=?0;
	speeds_filterold?*=?0.7;?????????????????????????????????????????????????????????????????????//一階互補濾波
	float?speeds_filter?=?speeds_filterold?+?speeds?*?0.3;
	speeds_filterold?=?speeds_filter;
	positions?+=?speeds_filter;
	positions?+=?f;?????????????????????????????????????????????????????????????????????????//全進控制量融合
	positions?+=?b;?????????????????????????????????????????????????????????????????????????//全進控制量融合
	positions?=?constrain（positions?-25502550）;??????????????????????????????????????????//抗積分飽和
	double?output?=?kis?*?（p0?-?positions）?+?kps?*?（p0?-?speeds_filter）;?????????????????????//速度環(huán)控制?PI
	if（flag1==1）
	{
		positions=0;
	}

	return?output;
}
//////////////////速度PI////////////////////

float?BalanceCar::turnspin（int?turnleftflagint?turnrightflagint?spinleftflagint?spinrightflagdouble?kpturndouble?kdturnfloat?Gyroz）
{
	int?spinonce?=?0;
	float?turnspeed?=?0;
	float?rotationratio?=?0;
	float?turnout_put?=?0;
	
	if?（turnleftflag?==?1?||?turnrightflag?==?1?||?spinleftflag?==?1?||?spinrightflag?==?1）
	{
		if?（spinonce?==?0）?????????????????????????????//旋轉前判斷當前車速，增強小車適應性。
		{
			turnspeed?=?（?pulseright?+?pulseleft）;??????????????????????//小車當前車速?脈沖表示
			spinonce++;
		}

		if?（turnspeed?		{
			turnspeed?=?-turnspeed;
		}

		if（turnleftflag==1||turnrightflag==1）
		{
			turnmax=7;???????//5->7
			turnmin=-7;
		}
		if（spinleftflag==1||spinrightflag==1）
		{
			turnmax=10;
			turnmin=-10;
		}
		rotationratio?=?55?/?turnspeed;????????????????????????//根據(jù)小車速度設定值
		if?（rotationratio?		if?（rotationratio?>?5）rotationratio?=?5;
	}
??else
??{
????rotationratio?=?0.5;
????spinonce?=?0;
????turnspeed?=?0;
??}
??if?（turnleftflag?==?1?||?spinleftflag?==?1）//根據(jù)方向參數(shù)疊加
??{
????turnout?+=?rotationratio;
??}
??else?if?（turnrightflag?==?1?||?spinrightflag?==?1）//根據(jù)方向參數(shù)疊加
??{
????turnout?-=?rotationratio;
??}
??else?turnout?=?0;
??if?（turnout?>?turnmax）?turnout?=?turnmax;//幅值最大值設置
??if?（turnout?
??turnout_put?=?-turnout?*?kpturn?-?Gyroz?*?kdturn;//旋轉PD算法控制?融合速度和Z軸旋轉定位。
	return?turnout_put;
}

void?BalanceCar::pwma（double?speedoutputfloat?rotationoutputfloat?anglefloat?angle6int?turnleftflagint?turnrightflagint?spinleftflagint?spinrightflag
	int?fint?bfloat?accelzint?Pin1int?Pin2int?Pin3int?Pin4int?PinPWMAint?PinPWMB）
{
	pwm1?=?-angleoutput?-?speedoutput?+?rotationoutput;?//Left電機PWM輸出值
	pwm2?=?-angleoutput?-?speedoutput?-?rotationoutput;?//Right電機PWM輸出值


??//幅度值限制
??if?（pwm1?>?255）?pwm1?=?255;
??if?（pwm1???if?（pwm2?>?255）?pwm2?

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件??????33405??2018-02-09?06:09??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Circuit\Self?Balancing?Robot?Circuit?Diagram.pdf

?????文件??????10788??2018-02-09?06:15??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Code\Self_Balancing_Robot\Self_Balancing_Robot.ino

?????文件???????4129??2017-06-02?12:12??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Libraries\BalanceCar\BalanceCar.cpp

?????文件???????1100??2017-06-02?12:13??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Libraries\BalanceCar\BalanceCar.h

?????文件???????6515??2012-02-13?09:16??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Libraries\Flexitimer2\FlexiTimer2.cpp

?????文件????????518??2012-02-13?09:16??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Libraries\Flexitimer2\FlexiTimer2.h

?????文件??????56402??2017-03-09?21:50??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Libraries\I2Cdev\I2Cdev.cpp

?????文件??????12084??2017-03-09?21:50??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Libraries\I2Cdev\I2Cdev.h

?????文件???????2042??2017-06-02?12:14??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Libraries\KalmanFilter\KalmanFilter.cpp

?????文件????????862??2017-06-02?12:14??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Libraries\KalmanFilter\KalmanFilter.h

?????文件???????4883??2017-03-09?21:49??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Libraries\MPU6050\helper_3dmath.h

?????文件??????38714??2017-03-09?21:49??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Libraries\MPU6050\JJ_MPU6050_DMP_6Axis.h

?????文件?????124718??2017-03-09?21:49??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Libraries\MPU6050\MPU6050.cpp

?????文件??????40751??2017-03-09?21:49??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Libraries\MPU6050\MPU6050.h

?????文件??????39077??2017-03-09?21:49??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Libraries\MPU6050\MPU6050_6Axis_MotionApps20.h

?????文件??????46665??2017-03-09?21:49??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Libraries\MPU6050\MPU6050_9Axis_MotionApps41.h

?????目錄??????????0??2018-02-09?06:16??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Code\Self_Balancing_Robot

?????目錄??????????0??2018-02-09?06:17??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Libraries\BalanceCar

?????目錄??????????0??2018-02-09?06:17??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Libraries\Flexitimer2

?????目錄??????????0??2018-02-09?06:17??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Libraries\I2Cdev

?????目錄??????????0??2018-02-09?06:17??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Libraries\KalmanFilter

?????目錄??????????0??2018-02-09?06:17??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Libraries\MPU6050

?????目錄??????????0??2018-02-09?06:10??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Circuit

?????目錄??????????0??2018-02-09?06:16??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Code

?????目錄??????????0??2018-02-09?06:17??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal\Libraries

?????目錄??????????0??2018-02-09?06:17??Self?Balancing?Robot?Circuit?and?Code-?Tapendra?Mandal

-----------?---------??----------?-----??----

???????????????422653????????????????????26