searchline.c.svn-base

AVR单片机下

/*                ****ROBOCON 2009 | BUPT TEAM*******
 * ------------------------------------------------------------------------
 * FileName   : searchLine.c
 * Version    : 1.0
 * Brief      : search white line
 * Code by    : Leaf
 * Date       : Febuary 26 2008
 * Dependence : none
 * Parameters : SPD_MAX, SPD_MIN
 * Note       : 使用三个参数的PID调整, 公布接口为
 *              setKp, setKi, setKd, setAdjustmentDiv
 *              setMaxSpeed, setMinSpeed
 *              由于速度调整基于当前最大速度，所以需要除以adjustmentDiv
 *
 * ------------------------------------------------------------------------
 */


#include "lib_bupt.h"
#ifdef SEARCHLINE_H_INCLUDED
#ifndef SPD_MAX
# error SPD_MAX not defined
#endif
#ifndef SPD_MIN
# error SPD_MIN not defined
#endif
#define SENSOR_EXCEPTION  (OffsetType)-128
/* R- Stand for Right Speed Decrease
 * L- Stand for Left  Speed Decrease
 * Prev Meas Process as previous status
 */
const OffsetType   offsetTable[] = {
		SENSOR_EXCEPTION,  //0000
		-3,                //0001  R-
		-1,                //0010  R-
		-2,                //0011  R-
		1,                 //0100  L-
		SENSOR_EXCEPTION,  //0101  R-
		0,                 //0110  Center
		-1,                //0111  R-
		3,                 //1000  L-
		SENSOR_EXCEPTION,  //1001  Prev
		SENSOR_EXCEPTION,  //1010  L-
		SENSOR_EXCEPTION,  //1011  Prev
		2,                 //1100  L-
		SENSOR_EXCEPTION,  //1101  Prev
		1,                 //1110  L-
		SENSOR_EXCEPTION   //1111  Prev
};

static SPEED_T maxSpeed      = 0;
static SPEED_T minSpeed      = 0;
/* 当前为第k次采样，则offsets三个元素为
 * 0 - k-2 次采样
 * 1 - k-1 次采样
 * 2 - k   次采样
 */
static OffsetType    offsets[3] = {0, 0, 0};
static INT16   sumOffset = 0;
static INT16   sumTimes  = 1;
static INT8    kp = 10;
static INT8    ki = 0;
static INT8    kd = 0;
static SPEED_T adjustmentDiv = 2000;
static void offsetIntegerate(OffsetType offset);
static void getSample(UINT8 sens);
static SPEED_T calcAdjustment(SPEED_T baseSpeed);
static SpeedPair adjustSpeed(const SpeedPair *curSpeed);


/*
 * Search Line Function
 * Parameters: UINT8 sens -- 4-bits sensor value,
 *             curSpeed   -- current speed(left and right)
 */
SpeedPair searchline(UINT8 sens, const SpeedPair *curSpeed){
	getSample(sens);
	return adjustSpeed(curSpeed);
}

/*+ private methods */
/* 处理采样信息 */
static void getSample(UINT8 sens){
	OffsetType offset = offsetTable[ sens & 0x0f ];
	if( offset != SENSOR_EXCEPTION ){
		offsets[0] = offsets[1];
		offsets[1] = offsets[2];
		offsets[2] = offset;
		offsetIntegerate( offset );
	}
}

/* 偏差积分 */
static void offsetIntegerate(OffsetType offset){
	sumOffset += offset;
	sumTimes  ++;
	static UINT8 centerTimes = 0;
	if(offset == 0 && ++centerTimes > 3){
		centerTimes = 0;
		searchlineClearSumOffset();
	}
}

/* 积分项复位 */
void searchlineClearSumOffset(void){
	sumOffset = 0;
	sumTimes  = 1;
}

/* 用PID算法计算调整量 */
static SPEED_T calcAdjustment(SPEED_T baseSpeed){
	INT32 adjustment;
	adjustment  = baseSpeed;
	adjustment *= (
			       kp * offsets[2]
			     + ki * (sumOffset) / sumTimes
			     + kd * (offsets[2] - offsets[1] * 2 + offsets[0])
	);
	adjustment /= adjustmentDiv;
	return adjustment;
}

/*调整速度*/
static SpeedPair adjustSpeed(const SpeedPair *curSpeed){
	if(maxSpeed == 0){
		return (SpeedPair){0, 0};
	}
	SpeedPair calcSpeed = {maxSpeed, maxSpeed};
	SPEED_T lastAdjustment = curSpeed->right - curSpeed->left;
	lastAdjustment = ABS(lastAdjustment);
	SPEED_T adjustment = calcAdjustment( maxSpeed );
	if(adjustment < 0)
	{
		calcSpeed.right = maxSpeed + adjustment - lastAdjustment;
		if(calcSpeed.right < minSpeed)
			calcSpeed.right = minSpeed;  // set floor for speed
		calcSpeed.left   = maxSpeed;
	}
	else if(adjustment > 0)
	{
		calcSpeed.left = maxSpeed - adjustment - lastAdjustment;
		if(calcSpeed.left < minSpeed)
			calcSpeed.left = minSpeed;  // set floor for speed
		calcSpeed.right  = maxSpeed;
	}
	return calcSpeed;
}
/*- private methods */



/*+Accessor Routines*/
void searchlineReset(void){
	offsets[0] = 0;
	offsets[1] = 0;
	offsets[2] = 0;
	searchlineClearSumOffset();
}
void searchlineSetAdjustmentDiv(SPEED_T div){
	adjustmentDiv = div;
}
SPEED_T searchlineGetAdjustmentDiv(void){
	return adjustmentDiv;
}
void searchlineSetKi(INT8 k){
	ki = k;
}
INT8 searchlineGetKi(void){
	return ki;
}
void searchlineSetKp(INT8 k){
	kp = k;
}
INT8 searchlineGetKp(void){
	return kp;
}
void searchlineSetKd(INT8 k){
	kd = k;
}
INT8 searchlineGetKd(void){
	return kd;
}
void searchlineSetPID(const PIDParam *p){
	searchlineSetKp(p->kp);
	searchlineSetKi(p->ki);
	searchlineSetKd(p->kd);
	searchlineSetAdjustmentDiv(p->adjustmentDiv);
}
void searchlineSetMax(SPEED_T max){
	maxSpeed = max;
	searchlineSetMin( max / 3 );
}
SPEED_T searchlineGetMax(void){
	return maxSpeed;
}

void searchlineSetMin(SPEED_T val){
	minSpeed = val;
}

SPEED_T searchlineGetMin(void){
	return minSpeed;
}
INT16 searchlineGetSumOffset(void){
	return sumOffset;
}
/*- Accessor Routines*/

#endif