示例#1
0
void setupAllMotorPoewrLevel(unsigned short CW1, unsigned short CW2,
                             unsigned short CCW1, unsigned short CCW2) {

    setupCcw1MotorPoewrLevel(CCW1);
    setupCcw2MotorPoewrLevel(CCW2);
    setupCw1MotorPoewrLevel(CW1);
    setupCw2MotorPoewrLevel(CW2);
}
/**
 *  this function controls motors by PID output
 *
 * @param
 * 		void
 *
 * @return 
 *		void
 *
 */
void motorControler() {

	float rollRateOutput = 0.f;
	float rollCcw1 = 0.f;
	float rollCcw2 = 0.f;
	float rollCw1 = 0.f;
	float rollCw2 = 0.f;

	float pitchRateOutput = 0.f;
	float pitchCcw1 = 0.f;
	float pitchCcw2 = 0.f;
	float pitchCw1 = 0.f;
	float pitchCw2 = 0.f;

	float yawRateOutput = 0.f;
	float yawCcw1 = 0.f;
	float yawCcw2 = 0.f;
	float yawCw1 = 0.f;
	float yawCw2 = 0.f;

	float outCcw1 = 0.f;
	float outCcw2 = 0.f;
	float outCw1 = 0.f;
	float outCw2 = 0.f;

	float maxLimit = 0.f;
	float minLimit = 0.f;
	float accelThrottleOffset = 0.f;
	float altholdThrottleOffset = 0.f;
	float throttleOffset = 0.f;
	float centerThrottle = 0.f;

	altholdThrottleOffset = (getEnableAltHold() && getAltHoldIsReady()) ? getThrottleOffsetByAltHold(updateAltHold()) : 0.f;
	accelThrottleOffset = getThrottleOffsetByAcceleration();
	throttleOffset = altholdThrottleOffset + accelThrottleOffset;

	centerThrottle = (float) getThrottlePowerLevel() + throttleOffset;

	maxLimit = (float) min(centerThrottle + getAdjustPowerLeveRange(),
			getMaxPowerLeve());
	minLimit = (float) max(centerThrottle - getAdjustPowerLeveRange(),
			getMinPowerLevel());

	getAttitudePidOutput();
	getRatePidOutput(&rollRateOutput, &pitchRateOutput, &yawRateOutput);

	/*
	 *	 rollCa>0
	 *	    -  CCW2   CW2   +
	 *	            	   X
	 *	    -   CW1    CCW1  +
	 *	            	   F
	 *
	 *	 rollCa<0
	 *	    +  CCW2   CW2    -
	 *	            	   X
	 *	    +   CW1    CCW1  -
	 *	            	   F
	 */
	rollCcw1 = rollRateOutput;
	rollCcw2 = -rollRateOutput;
	rollCw1 = -rollRateOutput;
	rollCw2 = rollRateOutput;
	
	/*
	 *	 pitchCa>0
	 *	    +  CCW2   CW2    +
	 *	            	   X
	 *	    -  CW1      CCW1   -
	 *	           	   F
	 *
	 *	pitchCa<0
	 *	    -  CCW2   CW2   -
	 *	                 X
	 *	    +   CW1   CCW1  +
	 *	           	   F
	 */
	pitchCcw1 = -pitchRateOutput;
	pitchCcw2 = pitchRateOutput;
	pitchCw1 = -pitchRateOutput;
	pitchCw2 = pitchRateOutput;
	
	/*
	 *	 yawCa>0
	 *	    +   CCW2   CW2    -
	 *	            	    X
	 *	    -    CW1   CCW1   +
	 *	                  F
	 *
	 *	 yawCa<0
	 *	    -  CCW2    CW2  +
	 *	            	   X
	 *	    +   CW1   CCW1  -
	 *	            	   F
	 */
	yawCcw1 = yawRateOutput;
	yawCcw2 = yawRateOutput;
	yawCw1 = -yawRateOutput;
	yawCw2 = -yawRateOutput;

	outCcw1 = centerThrottle
			+ LIMIT_MIN_MAX_VALUE(rollCcw1 + pitchCcw1 + yawCcw1,
					-getPidOutputLimitation(), getPidOutputLimitation());
	outCcw2 = centerThrottle
			+ LIMIT_MIN_MAX_VALUE(rollCcw2 + pitchCcw2 + yawCcw2,
					-getPidOutputLimitation(), getPidOutputLimitation());
	outCw1 = centerThrottle
			+ LIMIT_MIN_MAX_VALUE(rollCw1 + pitchCw1 + yawCw1,
					-getPidOutputLimitation(), getPidOutputLimitation());
	outCw2 = centerThrottle
			+ LIMIT_MIN_MAX_VALUE(rollCw2 + pitchCw2 + yawCw2,
					-getPidOutputLimitation(), getPidOutputLimitation());

	outCcw1 = getMotorGain(
			SOFT_PWM_CCW1) * LIMIT_MIN_MAX_VALUE(outCcw1, minLimit, maxLimit);
	outCcw2 = getMotorGain(
			SOFT_PWM_CCW2) * LIMIT_MIN_MAX_VALUE(outCcw2, minLimit, maxLimit);
	outCw1 = getMotorGain(
			SOFT_PWM_CW1) * LIMIT_MIN_MAX_VALUE(outCw1, minLimit, maxLimit);
	outCw2 = getMotorGain(
			SOFT_PWM_CW2) * LIMIT_MIN_MAX_VALUE(outCw2, minLimit, maxLimit);

	setupCcw1MotorPoewrLevel((unsigned short) outCcw1);
	setupCcw2MotorPoewrLevel((unsigned short) outCcw2);
	setupCw1MotorPoewrLevel((unsigned short) outCw1);
	setupCw2MotorPoewrLevel((unsigned short) outCw2);
#if 0
	_DEBUG(DEBUG_NORMAL,"outCcw1=%d,outCcw2=%d,outCw1=%d,outCw2=%d\n" ,
		(unsigned short)outCcw1,
		(unsigned short)outCcw2,
		(unsigned short)outCw1,
		(unsigned short)outCw2);
#endif
}