float g_fLeftMotorOut; float g_fRightMotorOut; float AnglePWMControlOut; #define MOTOR_OUT_DEAD_VAL 1 int g_nAngleControlFlag; int g_nCarControlFlag; #define ANGLE_CONTROL_STOP g_nAngleControlFlag = 0 #define ANGLE_CONTROL_START g_nAngleControlFlag = 1 #define CAR_CONTROL_SET g_nCarControlFlag = 1 #define CAR_CONTROL_CLEAR g_nCarControlFlag = 0 #define MOTOR_OUT_MAX 1.0 #define MOTOR_OUT_MIN -1.0 #define ANGLE_CONTROL_OUT_MAX MOTOR_OUT_MAX * 10 #define ANGLE_CONTROL_OUT_MIN MOTOR_OUT_MIN * 10 #define CAR_FAILURE_ANGLE_MAX 45.0 #define CAR_FAILURE_ANGLE_MIN -45.0 void SetMotorVoltage(float fLeftVoltage, float fRightVoltage) { // Voltage : > 0 : Move forward; // < 0 : Move backward /* short nPeriod; int nOut; nPeriod = (short)getReg(PWM_PWMCM); */ //-------------------------------------------------------------------------- if(fLeftVoltage > 1.0) fLeftVoltage = 1.0; else if(fLeftVoltage < -1.0) fLeftVoltage = -1.0; if(fRightVoltage > 1.0) fRightVoltage = 1.0; else if(fRightVoltage < -1.0) fRightVoltage = -1.0; //-------------------------------------------------------------------------- if(fLeftVoltage > 0) { //setReg(PWM_PWMVAL1, 0); PWMDTY01=0; //nOut = (int)(fLeftVoltage * nPeriod); //setReg(PWM_PWMVAL0, nOut); PWMDTY23=(int)(fLeftVoltage * 100); } else { //setReg(PWM_PWMVAL0, 0); PWMDTY23=0; fLeftVoltage = -fLeftVoltage; //nOut = (int)(fLeftVoltage * nPeriod); //setReg(PWM_PWMVAL1, nOut);
PWMDTY01=(int)(fLeftVoltage * 100); } //-------------------------------------------------------------------------- if(fRightVoltage > 0) { //setReg(PWM_PWMVAL2, 0); PWMDTY45=1; //nOut = (int)(fRightVoltage * nPeriod); //setReg(PWM_PWMVAL3, nOut); PWMDTY67=(int)(fRightVoltage * 100); } else { //setReg(PWM_PWMVAL3, 0); PWMDTY67=0; fRightVoltage = -fRightVoltage; //nOut = (int)(fRightVoltage * nPeriod); //setReg(PWM_PWMVAL2, nOut); PWMDTY45=(int)(fRightVoltage * 100); } } void MotorSpeedOut(void) { float fLeftVal, fRightVal; fLeftVal = g_fLeftMotorOut; fRightVal = g_fRightMotorOut; if(fLeftVal > 0) fLeftVal += MOTOR_OUT_DEAD_VAL; else if(fLeftVal < 0) fLeftVal -= MOTOR_OUT_DEAD_VAL; if(fRightVal > 0) fRightVal += MOTOR_OUT_DEAD_VAL; else if(fRightVal < 0) fRightVal -= MOTOR_OUT_DEAD_VAL; if(fLeftVal > MOTOR_OUT_MAX) fLeftVal = MOTOR_OUT_MAX; if(fLeftVal < MOTOR_OUT_MIN) fLeftVal = MOTOR_OUT_MIN; if(fRightVal > MOTOR_OUT_MAX) fRightVal = MOTOR_OUT_MAX; if(fRightVal < MOTOR_OUT_MIN) fRightVal = MOTOR_OUT_MIN; SetMotorVoltage(fLeftVal, fRightVal); } void MotorOutput(void) { float fLeft, fRight; //fLeft = g_fAngleControlOut - g_fSpeedControlOut - g_fDirectionControlOut; fLeft = g_fAngleControlOut; //fRight = g_fAngleControlOut - g_fSpeedControlOut + g_fDirectionControlOut;
fRight = g_fAngleControlOut; if(fLeft > MOTOR_OUT_MAX) fLeft = MOTOR_OUT_MAX; if(fLeft < MOTOR_OUT_MIN) fLeft = MOTOR_OUT_MIN; if(fRight > MOTOR_OUT_MAX) fRight = MOTOR_OUT_MAX; if(fRight < MOTOR_OUT_MIN) fRight = MOTOR_OUT_MIN; g_fLeftMotorOut = fLeft; g_fRightMotorOut = fRight; MotorSpeedOut(); } void AngleCalculate(void) { float fDeltaValue; // Define Angle g_fGravityAngle = (VOLTAGE_GRAVITY - GRAVITY_OFFSET) * GRAVITY_ANGLE_RATIO; g_fGyroscopeAngleSpeed = (VOLTAGE_GYRO - GYROSCOPE_OFFSET) * GYROSCOPE_ANGLE_RATIO; g_fCarAngle = g_fGyroscopeAngleIntegral; fDeltaValue = (g_fGravityAngle - g_fCarAngle) / GRAVITY_ADJUST_TIME_CONSTANT; //#if SPEED_CONTROL_METHOD == 0 g_fGyroscopeAngleIntegral += (g_fGyroscopeAngleSpeed + fDeltaValue) / GYROSCOPE_ANGLE_SIGMA_FREQUENCY; //#else if SPEED_CONTROL_METHOD == 1 // g_fGyroscopeAngleIntegral += (g_fGyroscopeAngleSpeed + g_fAngleControlSpeedFeedback) / GYROSCOPE_ANGLE_SIGMA_FREQUENCY; //#endif // SPEED_CONTROL_METHOD } void AngleControl(void) { float fValue; if(g_nAngleControlFlag == 0) { g_fAngleControlOut = 0; return; } //-------------------------------------------------------------------------- fValue = (CAR_ANGLE_SET - g_fCarAngle) * ANGLE_CONTROL_P + (CAR_ANGLE_SPEED_SET - g_fGyroscopeAngleSpeed) * ANGLE_CONTROL_D; if(fValue > ANGLE_CONTROL_OUT_MAX) fValue = ANGLE_CONTROL_OUT_MAX; else if(fValue < ANGLE_CONTROL_OUT_MIN) fValue = ANGLE_CONTROL_OUT_MIN;