|
楼主 |
发表于 2011-10-22 20:44:46
|
显示全部楼层
回复【1楼】gzhuli 咕唧霖
-----------------------------------------------------------------------
热的有烧开水的味道了,想起了当年玩智能车时的33886,哈哈。还有个问题,就四个线圈一定要放在环形磁铁的环内么?今天测得时候感觉线圈输出的总不大对,看来还得把原理搞搞清楚,再慢慢调,我的程序是按照你的stm8改的。
#include "includes.h"
void PWM_Init(void)
{
TCCR1B = 0x00; //停止定时器
TIMSK |= (1<<TOIE1);
TCNT1H = 0x00;
TCNT1L = 0x00;
OCR1AL = 0xff;
OCR1BL = 0xff;
TCCR1A |= (1<<COM1A1)|(1<<COM1B1)| //PD4,PD5 output PWM
(1<< WGM11)|(1<< WGM10);
TCCR1B |= (1<< WGM12)|(1<<CS10); //快速PWM,计数上限值TOP=0x1ff
//使用系统时钟,8分频
}
void Port_Init(void)
{
PORTD |= (1<<4)|(1<<5);
DDRD |= (1<<4)|(1<<5);
PORTA = 0x00;
DDRA = 0x00; //AD输入口
PORTC = 0x00;
DDRC = 0xff;
}
void ADC_Init(void)
{
ADCSRA = 0x00;
//ADMUX =(ADC_CHANNEL_2&0x1f)|(1<<REFS0)|(1<<REFS1); //参考电压为内部2.56
//ADMUX =(adc_mux&0x1f)|(1<<REFS0); //参考电压为内部AVCC
ADMUX =(ADC_CHANNEL_2&0x1f); //参考电压为引脚AREF
ADCSRA=(1<<ADEN)|(1<<ADIE)|(1<<ADPS1)|(1<<ADPS0) ;//8分频
}
void ADC_Start(void)
{
ADCSRA |= (1<<ADSC);//启动AD转换
}
#include "includes.h"
#define X_DIRECTION_FLAG 0x01
#define Y_DIRECTION_FLAG 0x02
#define TIM1_OCPOLARITY_LOW 0x01
#define TIM1_OCPOLARITY_HIGH 0x02
unsigned char currentChannel;
int xPos, yPos;
int txPos,tyPos;
PID xPID, yPID;
/******************************************************************************/
/******************************************************************************/
void TIM1_OC1PolarityConfig(unsigned char config)
{
switch(config)
{
case TIM1_OCPOLARITY_LOW :
TCCR1A |=(1<<COM1A1);
TCCR1A &= ~(1<<COM1A0);
case TIM1_OCPOLARITY_HIGH:
TCCR1A |=(1<<COM1A1)|(1<<COM1A0);
default:
break;
}
}
void TIM1_OC2PolarityConfig(unsigned char config)
{
switch(config)
{
case TIM1_OCPOLARITY_LOW :
TCCR1B |=(1<<COM1B1);
TCCR1B &= ~(1<<COM1B0);
case TIM1_OCPOLARITY_HIGH:
TCCR1B |=(1<<COM1B1)|(1<<COM1B0);
default:
break;
}
}
void TIM1_SetCompare1(int pwm)
{
OCR1A = pwm;
}
void TIM1_SetCompare2(int pwm)
{
OCR1B = pwm;
}
int calcPID(PID *pid, int error)
{
int output;
if (pid->Ki != 0)
{
pid->integrationError += error;
// Limit the maximum integration error
if (pid->integrationError > MAX_INTEGRATION_ERROR)
{
pid->integrationError = MAX_INTEGRATION_ERROR;
}
else if (pid->integrationError < -MAX_INTEGRATION_ERROR)
{
pid->integrationError = -MAX_INTEGRATION_ERROR;
}
}
output = pid->Kp * error + pid->Ki * pid->integrationError + pid->Kd * (error - pid->prevError);
// Limit the maximum output
if (output > MAX_PID_OUTPUT)
{
output = MAX_PID_OUTPUT;
}
else if (output < -MAX_PID_OUTPUT)
{
output = -MAX_PID_OUTPUT;
}
pid->prevError = error;
return output;
}
unsigned int Get_ad(void)
{
return ((ADCH << 8 ) | ADCL);
}
#pragma interrupt_handler time1_isr:iv_TIMER1_OVF
void time1_isr( void )
{
static unsigned char direction;
static unsigned char count=0;
int xError, yError;
int xPWM, yPWM;
if(++count>=100)
{
count = 0;
PORTC ^=(1<<6);
}
// Change X direction
if (direction & X_DIRECTION_FLAG)
{
SETC0_HIGH();
}
else
{
SETC0_LOW();
}
// Change Y direction
if (direction & Y_DIRECTION_FLAG)
{
SETC1_HIGH();
}
else
{
SETC1_LOW();
}
// PID calculation
xError = xPID.targetValue - xPos;
yError = yPID.targetValue - yPos;
xPWM = calcPID(&xPID, xError);
yPWM = calcPID(&yPID, yError);
// Set the direction and OC polarity based on the PID output (takes effect on next PWM cycle)
if (xPWM >= 0)
{
direction &=~X_DIRECTION_FLAG;
TIM1_OC1PolarityConfig(TIM1_OCPOLARITY_LOW);
}
else
{
direction |= X_DIRECTION_FLAG;
xPWM = -xPWM;
TIM1_OC1PolarityConfig(TIM1_OCPOLARITY_HIGH);
}
if (yPWM >= 0)
{
direction &= ~Y_DIRECTION_FLAG;
TIM1_OC2PolarityConfig(TIM1_OCPOLARITY_LOW);
}
else
{
direction |= Y_DIRECTION_FLAG;
yPWM = -yPWM;
TIM1_OC2PolarityConfig(TIM1_OCPOLARITY_HIGH);
}
// Update the PWM (takes effect on next PWM cycle)
TIM1_SetCompare1(xPWM);
TIM1_SetCompare2(yPWM);
//TCNT1H = 0x00;
//TCNT1L = 0x00;
TIFR |= (1<<OCF1A);
//TIM1_ClearITPendingBit(TIM1_IT_UPDATE);
}
/******************************************************************************/
/******************************************************************************/
//ADC完成中断
//#pragma vector = interrupt_handler adc_isr:iv_ADC
// define the interrupt handler
#pragma interrupt_handler adc_isr:iv_ADC
void adc_isr( void )
{
static unsigned char xFilterPos,yFilterPos;
static int xFilterBuf[8]={0,0,0,0,0,0,0,0};
static int yFilterBuf[8]={0,0,0,0,0,0,0,0};
int adcValue,acc = 0;
unsigned char i;
//adcValue = Get_ad();
adcValue = ((ADCH << 8 ) | ADCL);
if (currentChannel == ADC_CHANNEL_2)
{
xFilterBuf[xFilterPos & 7] = adcValue;
xFilterPos++;
if(xFilterPos>=8)
xFilterPos = 0;
for (i = 0; i < 8; i++)
{
acc += xFilterBuf;
}
xPos = acc >> 3;
currentChannel = ADC_CHANNEL_3;
}
else
{
yFilterBuf[yFilterPos & 7] = adcValue;
yFilterPos++;
if(yFilterPos>=8)
yFilterPos = 0;
for (i = 0; i < 8; i++)
{
acc += yFilterBuf;
}
yPos = acc >> 3;
currentChannel = ADC_CHANNEL_2;
}
//ADCSRA &= ~(1<<ADIF);
ADMUX = (currentChannel&0x1f);
ADCSRA |= (1<<ADSC);//启动AD转换
} |
|