/**
Allows positions to be passed in as ints (pixels). Wraps AngleControl(int, int)
@param desPos_x The desired X position (in pixels)
@param desPos_y The desired Y Position (in pixels)
@param curPos_x The current X position (in pixels)
@param curPos_y The current Y position (in pixels)
@return cv::Point A vector containing the desired angle for each axis
*/
cv::Point DualAxisController::AngleControl(int desPos_x, int desPos_y,
int curPos_x, int curPos_y)
{
setXYDesiredPosition_px(desPos_x, desPos_y);
return AngleControl(curPos_x, curPos_y);
}
void interrupt VectorNumber_Vpit0 pit0(void) //1ms中断
{
g_n1MsEventCount++;
g_nSpeedControlPeriod++;
g_nDirectionControlPeriod++;
SpeedControlOutput();
MotorOutput();
DirectionControlOutput();
if(g_n1MsEventCount >= 5)
{
g_n1MsEventCount = 0;
SpeedCalculate();
}
else if(g_n1MsEventCount == 1)
{
GetAngle(AngleResult);
}
else if(g_n1MsEventCount == 2)
{
AngleControl();
}
else if(g_n1MsEventCount == 3)
{
g_nSpeedControlCount++;
if(g_nSpeedControlCount >= 20)
{
SpeedControl();
g_nSpeedControlCount = 0;
g_nSpeedControlPeriod = 0;
}
}
else if(g_n1MsEventCount == 4)
{
g_nDirectionControlCount++;
if(g_nDirectionControlCount >= 2)
{
DirectionControl();
g_nDirectionControlCount = 0;
g_nDirectionControlPeriod = 0;
}
}
PITTF_PTF0 = 1;
}
/* is set to 'yes' (#pragma interrupt saveall is generated before the ISR) */
void TI1_OnInterrupt(void)
{
oneMsCounter++;
//-------------------------------------------
g_nspeedControPeriod ++;
speedControlOutput();
//___________________________________________
//-------------------------------------------
g_nDirectionControlPeriod ++;
DirectionControlOutput();
//___________________________________________
if(oneMsCounter == 1)
{
if(AD_Flag) updataSensor(); //读取ADC的值
}
//--------------------------------------------------
else if(oneMsCounter == 2)
{
AngleCalculate(); //计算角度
AngleControl(); //角度的PD值控制
motorControl(); //电机输出控制
}
//-----------------------------------------------
else if(oneMsCounter == 3)
{
g_nSpeedControlCount++;
oneHundredMsCounter++;
if(g_nSpeedControlCount >= SPEED_CONTROL_COUNT)
{
speedControl(); //速度控制
g_nSpeedControlCount=0;
g_nspeedControPeriod=0;
}
if(standFlag){
if(oneHundredMsCounter == 100)
{
if(CAR_SPEED_SET < speedNeed3)
CAR_SPEED_SET += 10;
else
{
CAR_SPEED_SET = speedNeed3;
speedStarEndFlag = 1;
}
oneHundredMsCounter = 0;
}
}
else
{
CAR_SPEED_SET = 0;
oneHundredMsCounter = 0;
speedStarEndFlag = 0;
}
}
//--------------------------------------------------
else if(oneMsCounter == 4)
{
g_nDirectionControlCount ++;
DianciCalculate();
DirectionVoltageSigma();
if(g_nDirectionControlCount >= DIRECTION_CONTROL_COUNT) {
DirectionControl();
g_nDirectionControlCount = 0;
g_nDirectionControlPeriod = 0;
}
}
//-------------------------------------------------
else if(oneMsCounter >= 5)
{
oneMsCounter = 0;
GetMotorPulse();
UartFlag = 1;
}
}
/**
Allows current positions to be passed in as ints. Wraps AngleControl(cv::Point)
@param curPos_x The current X position (in pixels)
@param curPos_y The current Y position (in pixels)
@return cv::Point A vector containing the desired angle for each axis
*/
cv::Point DualAxisController::AngleControl(int curPos_x, int curPos_y)
{
cv::Point xy = cv::Point(curPos_x, curPos_y);
return AngleControl(xy);
}
void PIT_Signal(void)
{
DisableInterrupts;
//--------------------------------------------------------------------------
g_nSpeedControlPeriod++;
SpeedControlOutput();
g_nDirectionControlPeriod ++;
DirectionControlOutput();
//---------------------------------------------------------------------------
g_nCarCount++;//中断服务片选
switch(g_nCarCount)
{
//---------------------------计数：0
case 1:
{
asm("nop");
}; break;
//---------------------------计数：1 AD采集 第1个1ms
case 2:
{
asm("nop");
AngleCalculate();
g_nDirectionGyro = (int16)LPLD_ADC_Get(ADC0, AD11); //方向陀螺仪采集
}; break;
//----------------------------计数：2 直立控制 电机PWM输出 第2个1ms
case 3:
{
asm("nop");
AngleControl();
MotorOutput();
}; break;
//----------------------------计数：3 车模速度控制 第3个1ms
case 4:
{
asm("nop");
g_nSpeedControlCount++;
if(g_nSpeedControlCount>=20) //控制是否进入速度更新环节
{
SpeedControl();
g_nSpeedControlCount = 0;
g_nSpeedControlPeriod = 0;
}
};break;
//----------------------------计数：4 车模方向控制 第4个1ms
case 5:
{
asm("nop");
g_nCarCount=0;
g_nDirectionControlCount ++;
DirectionVoltageSigma(); //电感采集、滤波
if( g_nDirectionControlCount >= 2)//控制是否进入方向更新环节
{
DirectionControl();
g_nDirectionControlCount = 0;
g_nDirectionControlPeriod = 0;
}
};break;
}
EnableInterrupts;
}
