 |
 |
| View previous topic :: View next topic |
| Author |
Message |
Thiago Alves
Guest
|
| DC Motor position using Rotary Encoder and PWM |
 Posted: Fri Oct 02, 2009 9:27 am |
 |
|
Hi!
This is my first post here on the forum. I'm trying to make a positioning system for my DC motor at college. I'm using a rotary encoder attached to the motor to give me the position of it. The encoder is giving me the quadrature signals on the RB0 and RB4 pins of the PIC (I'm using the PIC16F877 - and I also have a 18F452 available if it would be necessary).
The code posted by Ttelmah for counting encoder pulses works great. I'm posting it here to explain better what I want:
| Code: |
#include <18F458.h>
#use delay(clock=40000000)
#fuses H4,NOWDT,PUT,BROWNOUT,WRT,NOLVP,BORV42
#use rs232(baud=9600,parity=N,xmit=PIN_C6,rcv=PIN_C7)
#use fast_io(B)
/*
RB4,RB5=quadrature inputs
*/
signed int32 position;
//Note using int32, otherwise routine will run off the end very quickly.
//Also 'signed', so the unit can handle the shaft rotating backwards at
//the start...
#byte portb=0xF81
#bit bchanged=0xFF2.0
#int_global
void myint(void) {
static int old;
static int new;
static int value;
//Here I have an edge on one of the quadrature inputs
new=portb;
/*Now I have to decode the quadrature changes. There are four
possibilities:
I can have a rising or falling edge, on each of the two inputs. I have to
look at the state of the other bit, and increment/decrement according to
this. */
value=new^old;
//'value', now has the bit set, which has changed
if (value & 0x10) {
//Here the low bit has changed
if (new & 0x10) {
//Here a rising edge on A
if (new & 0x20) --position;
else ++position;
}
else {
//Here a falling edge on A
if (new & 0x20) ++position;
else --position;
}
}
else {
//Here the high bit (B) must have changed
if (new & 0x20) {
//Here a rising edge on B
if (new & 0x10) ++position;
else --position;
}
else {
//Here a falling edge on B
if (new & 0x10) --position;
else ++position;
}
}
old=new;
bchanged=0;
#asm
//Here the 'fast' exit.
RETFIE 1
#ENDASM
}
//Main code loop
void main(void) {
signed int32 lastpos;
port_b_pullups(true);
//Note that many of the quadrature encoders _require_ pull-ups on
//their lines, having open collector outputs.
setup_adc_ports(NO_ANALOGS);
setup_spi(FALSE);
set_tris_B(0x30); //Input on bit 4,5 for quad
enable_interrupts(INT_RB);
enable_interrupts(global);
lastpos=position;
while (true) {
if(lastpos!=position){
lastpos=position;
printf("\fcount = %5ld",lastpos);
//I would recommend a delay here. The interrupt will still keep
//counting edges, but without a delay, the string will be 'non stop'
//for any reasonable movement rate.
}
}
} |
But it works only for that (I mean, only 1 interrupt being used). In my case I also need to use Timer2 interruption to generate my PWM.
He said when he was posting the code that:
| Quote: | | The code replaces the _int global_ handler, and only supports the RB interrupt. If you have any other interrupts needed or specified, I can post a version that shows how to do this. You need INT_RB enabled, and INT_global enabled, but you _must not_ have an int_RB handler present. |
So how can I have more than one interruption working on this code? Actually I need 3 interruptions working: RB (to count encoder pulses), RTCC (to calculate my PID state) and PWM (to generate the PWM based on my position).
Thanks! |
|
 |
Ttelmah
Guest
|
|
| Posted: Fri Oct 02, 2009 9:46 am |
|
|
Seriously, you don't need/want an interrupt for the PWM. The PWM is generated for you in hardware, and no interrupt is used.
Don't do anything like 'calculating PID state' in the interrupt. You can add handling of more interrupts to the code, _but_ you _must_ get out of the interrupt handler in the 'worst case', in less than the interval between encoder pulses. Calculating the PID, takes _much too long_.
Remember you can use the hardware interrupt flags, _without_ using the interrupts. My own servo code, sits in a tight loop, I use a 'tick', every six cyles of the PWM (just set the timer2 interrupt to occur every six loops), then I test the interrupt flag. When it 'sets', I clear it, calculate the PID terms, and loop. The only thing handled by the hardware interrupt, other than the opto encoder, is the serial receive into a buffer. While waiting for the 'tick, if serial data is seen, I step through a parser state machine, till a command is recogised.
Best Wishes |
|
|
Thiago Alves
Guest
|
|
| Posted: Fri Oct 02, 2009 10:57 am |
|
|
Thank you for your help! Could you please post here your code for your servo, or at least part of it so that I can understand better what are you saying? There are weeks that I'm trying to do this PWM DC Motor Control and I still can't do it very well.
Thank you dude. |
|
|
Thiago Alves
Guest
|
|
| Posted: Fri Oct 02, 2009 11:11 am |
|
|
By now, the only way I solved this was using 2 16F877 PICs. One is getting the signals from the encoder and acting like a counter and the other is getting the pulses from the "counter pic" and calculating PID terms and controlling the motor with PWM. I'm posting here the code for the 2 pics (I hope that I'm not flooding here too much...)
Code of the first PIC (the one used to count pulses)
| Code: |
#include <16f877.h>
#FUSES NOWDT //SEM CAO DE GUARDA
#FUSES HS //CRYSTAL ACIMA DE 4MHZ
#FUSES NOPUT //No Power Up Timer
#FUSES NOPROTECT //Code not protected from reading
#FUSES NODEBUG //No Debug mode for ICD
#FUSES NOBROWNOUT //No brownout reset
#FUSES NOLVP //No low voltage prgming, B3(PIC16) or B5(PIC18) used for I/O
#FUSES NOCPD //No EE protection
#FUSES NOWRT //Program memory not write protected
#use delay(clock=4000000) // cristal de 4Mhz
#use rs232(baud=19200,parity=N,xmit=PIN_C6,rcv=PIN_C7,bits=8) // 19200 bps em rs232
#use fast_io(B)
/////////////////////////////////////////////////////////////
//#include <stdlib.h>
//#include <math.h>
//#include <input.c>
//#byte portb=0xF81
#bit bchanged=0xFF2.0
int velho=0, novo=0, valor=0;
#int_global
void bisr(void)
{
novo=input_b();
valor=novo^velho; //valor armazena o bit que mudou usando comparacao XOR;
if (valor & 0x10) //se o bit que mudou for RB4
{
if (novo & 0x10) //Se RB4 estiver em ALTO (borda de subida em RB4)
{
if (novo & 0x1) //Se tambem o RB0 estiver ALTO
{
output_low(PIN_A2);
output_high(PIN_A2); //--posicao
}
if (!(novo & 0x1)) //borda de subida em RB4, mas RB0 esta BAIXO
{
output_low(PIN_A1);
output_high(PIN_A1); //++posicao
}
}
if (!(novo & 0x10)) //Se RB4 estiver em BAIXO (borda de descida em RB4)
{
if (novo & 0x1)
{
output_low(PIN_A1);
output_high(PIN_A1); //++posicao
}
if (!(novo & 0x1))
{
output_low(PIN_A2);
output_high(PIN_A2); //--posicao
}
}
}
if (valor & 0x1) //nesse caso quem mudou foi o RB0
{
if (novo & 0x1) //borda de subida em RB0
{
if (novo & 0x10) //RB4 esta ALTO
{
output_low(PIN_A1);
output_high(PIN_A1); //++posicao
}
if (!(novo & 0x10))
{
output_low(PIN_A2);
output_high(PIN_A2); //--posicao
}
}
if (!(novo & 0x1)) //borda de descida em RB0
{
if (novo & 0x10) //RB4 em ALTO
{
output_low(PIN_A2);
output_high(PIN_A2); //--posicao
}
if (!(novo & 0x10))
{
output_low(PIN_A1);
output_high(PIN_A1); //++posicao
}
}
}
velho=novo;
bchanged=0;
//#asm
// RETFIE 1
//#ENDASM //I commented these lines because 16F877 doesn't support it
}
void main()
{
set_tris_B(0x11);
output_b(0);
output_a(0);
enable_interrupts(INT_RB);
enable_interrupts(GLOBAL);
while(1)
{
}
}
|
Code of the second PIC (used to calculate PID terms and generate PWM)
| Code: |
#include <16f877.h>
#FUSES NOWDT //SEM CAO DE GUARDA
#FUSES HS //CRYSTAL ACIMA DE 4MHZ
#FUSES NOPUT //No Power Up Timer
#FUSES NOPROTECT //Code not protected from reading
#FUSES NODEBUG //No Debug mode for ICD
#FUSES NOBROWNOUT //No brownout reset
#FUSES NOLVP //No low voltage prgming, B3(PIC16) or B5(PIC18) used for I/O
#FUSES NOCPD //No EE protection
#FUSES NOWRT //Program memory not write protected
#use delay(clock=20000000) // cristal de 20Mhz
#use rs232(baud=19200,parity=N,xmit=PIN_C6,rcv=PIN_C7,bits=8) // 19200 bps em rs232
#use fast_io(B)
/////////////////////////////////////////////////////////////
#include <stdlib.h>
#include <math.h>
#include <input.c>
//------------------------ VARIAVEIS DO CONTROLE PID ------------------------//
float pid_d_state=0;
float pid_i_state=0;
float pid_i_max=0, pid_i_min=0;
int pid_i_gain=0, pid_p_gain=0, pid_d_gain=0;
//------------------------ VARIAVEIS DO ENCODER ----------------------------//
signed int16 encoder_pulsos;
float encoder_resolucao;
signed int16 posicao_atual=0, posicao_set=0, erro=0;
int input=0;
int16 duty=256;
signed int16 updatepid=0;
int1 configurado=0;
int16 update_pid()
{
float p_term, i_term, d_term;
p_term = pid_p_gain * erro;
pid_i_state += erro;
if(pid_i_state > pid_i_max)
pid_i_state = pid_i_max;
else if(pid_i_state < pid_i_min)
pid_i_state = pid_i_min;
i_term = pid_i_gain * pid_i_state;
d_term = pid_d_gain * (posicao_set - pid_d_state); //dar uma olhada nisso aqui
pid_d_state = posicao_set;
updatepid = p_term + i_term - d_term;
}
void configura_controle(void)
{
pid_d_state = 0.0;
pid_i_state = 0.0;
pid_i_max = 256.0;
pid_i_min = -256.0;
printf("\r\nConfiguracao do controle PID\r\n----------------------");
printf("\r\nGanho Integral: ");
pid_i_gain=get_int();
printf("\r\nGanho Proporcional: ");
pid_p_gain=get_int();
printf("\r\nGanho Derivativo: ");
pid_d_gain=get_int();
printf("\r\n");
}
void configura_encoder(void)
{
encoder_pulsos = 0;
printf("\r\nConfiguracao do Encoder\r\n----------------------");
printf("\r\nResolucao (*10): ");
input=get_int();
encoder_resolucao=input*10.0;
printf("\r\n");
}
void configura_posicao(void)
{
printf("\r\nConfiguracao da Posicao\r\n----------------------");
printf("\r\nPosicao: ");
posicao_set=get_int();
printf("\r\n\r\n");
printf("Configuracao final:");
printf("\r\nGanho Integral: %d", pid_i_gain);
printf("\r\nGanho Proporcional: %d", pid_p_gain);
printf("\r\nGanho Derivativo: %d", pid_d_gain);
printf("\r\n\r\nResolucao do Encoder: %f", encoder_resolucao);
printf("\r\nPosicao Final: %ld", posicao_set);
input=get_int();
}
#INT_TIMER2
void isr(void)
{
if (configurado) {
if (input(PIN_B1))
{
posicao_atual++;
while(input(PIN_B1)) {}
}
if (input(PIN_B2))
{
posicao_atual--;
while(input(PIN_B2)) {}
}
}}
#int_RTCC
void rtccisr(void)
{
if(configurado) {
erro = (posicao_set - posicao_atual);
update_pid();
if (updatepid < (-256)) updatepid = -256;
duty = 256 + updatepid;
if(duty > 511)
{
duty = 511;
}
else if(duty < 1)
{
duty = 1;
}
set_pwm1_duty(duty);
printf("\r\nPos: %ld\r\nErr: %ld", posicao_atual, erro);
}}
void main()
{
output_b(0);
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_32|RTCC_8_bit);
setup_timer_2(T2_DIV_BY_4,127,1);
enable_interrupts(INT_TIMER2);
enable_interrupts(INT_RTCC);
enable_interrupts(GLOBAL);
configura_controle();
configura_encoder();
configura_posicao();
configurado=1;
setup_ccp1(CCP_PWM);
set_pwm1_duty(duty); //[0 - 512]
while(1)
{
}
}
|
Well... is there a way to "merge" these 2 codes into 1 so that I can use only one PIC? I'm running out of resources on the lab, since I need to control a lot of DC motors. |
|
|
Thiago Alves
Guest
|
|
| Posted: Sun Oct 04, 2009 8:30 pm |
|
|
| No idea what should I do? |
|
|
PCM programmer
Joined: 06 Sep 2003
Posts: 21708
|
|
| Posted: Sun Oct 04, 2009 9:00 pm |
|
|
| Quote: | #include <16f877.h>
//#byte portb=0xF81
#bit bchanged=0xFF2.0 |
These register addresses are not for the 16F-series. They are for 18F.
They need to be changed. Look in the 16F877 data sheet to find
the correct register addresses. |
|
|
Thiago Alves
Guest
|
|
| Posted: Mon Oct 05, 2009 9:05 am |
|
|
| But it is working anyway, each program separatedly in 2 different pics. (I think that it's working because I'm using the input() instead of reading the registers). What I wanna know is if there is a way to run these two programs in just one pic. |
|
|
Ttelmah
Guest
|
|
| Posted: Mon Oct 05, 2009 9:46 am |
|
|
Yes, it is definately 'possible' to do the whole of this in one PIC, but you are going to have to write it. At the end of the day, we will give advice, but we are not going to write your code for you. Also it is far harder for us, since you have the hardware involved. A lot of the difficulty, is working out how fast to run the servo loop (will depend on your hardware), and what else needs to be done. I'd suggest that give the amount of serial code you seem to be running, you _need_ to look at running just the serial TX/RX, and the encoder, in your interrupts.
You will need to add the defines, and since you are on a 16 chip, restore the W, status etc., registers (this code is for a 18 chip, and uses RETFIE 1 to do this), but something like:
| Code: |
//This is the decoder for a change in the quadrature inputs. Called on port B change
//interrupt, using the fast return stack.
void quad(void) {
static int old;
static int new;
static int value;
//Here I have an edge on one of the quadrature inputs
new=portb;
//Now I have to decode the quadrature changes. There are four possibilities:
//I can have a rising or falling edge, on each of the two inputs. I have to
//look at the state of the other bit, and increment/decrement according to
//this.
value=new^old;
//'value', now has the bit set, which has changed
if (value & 0x10) {
//Here the low bit has changed
if (new & 0x10) {
//Here a rising edge on A
if (new & 0x20) --position;
else ++position;
}
else {
//Here a falling edge on A
if (new & 0x20) ++position;
else --position;
}
}
else {
//Here the high bit (B) must have changed
if (new & 0x20) {
//Here a rising edge on B
if (new & 0x10) ++position;
else --position;
}
else {
//Here a falling edge on B
if (new & 0x10) --position;
else ++position;
}
}
old=new;
udflag=true;
}
void TBE_isr(void) {
//Here the RS232 transmit buffer has emptied - send next character if one is waiting
if (rcnt>0) {
TXREG=rbuff[rout];
rout=++rout % SRBUFF;
--rcnt;
}
else
disable_interrupts(INT_TBE);
#asm
BCF PIR1,TXIE
#endasm
}
void RDA_isr(void) {
//Here a RS232 receive character has arrived - store and set flag. Also clear 'overrun' if flagged.
rflag=true;
rchr=RCREG;
if (OERR) {
CREN=false;
CREN=true;
}
#asm
BCF PIR1,RCIE
#endasm
}
void TIMER2_isr(void) {
//Here I update the power requirements. Since I am in the 'dead' moment at the start of the pulse,
//I avoid changing 'mid pulse', and getting odd output changes.
set_pwm1_duty(power);
tick=tick^1;
#asm
BCF PIR1,TMR2
#endasm
}
#int_global
void myint(void) {
static int INT_scratch[10];
#ASM
//Here for maximum speed, I test the RB interrupt - since it is allways
//enabled, I don't have to test the enable bit
BTFSS INTCON,RBIF
GOTO NXT
#endasm
quad(); //Quadrature handler.
#asm
BCF INTCON,RBIF
GOTO FEXIT //Use the fast stack for exit
NXT:
//Now I save the registers for the other interrupt handlers
MOVFF FSR0L,INT_scratch
MOVFF FSR0H,INT_scratch+1
MOVFF FSR1L,INT_scratch+2
MOVFF FSR1H,INT_scratch+3
MOVFF FSR2L,INT_scratch+4
MOVFF FSR2H,INT_scratch+5
MOVFF scratch,INT_scratch+6
MOVFF scratch1,INT_scratch+7
MOVFF scratch2,INT_scratch+8
MOVFF scratch3,INT_scratch+9
NEXTA:
//Now for Timer 2
BTFSS PIE1,TMR2
GOTO NEXT1
BTFSC PIR1,TMR2
#endasm
TIMER2_isr();
#asm
NEXT1:
//Receive data available
BTFSS PIE1,RCIE
GOTO NEXT2
BTFSC PIR1,RCIE
#endasm
RDA_isr();
#asm
NEXT2:
//Transmit buffer empty
BTFSS PIE1,TXIE
GOTO EXIT
BTFSC PIR1,TXIE
#endasm
TBE_isr();
#asm
EXIT:
//Restore registers
MOVFF INT_scratch,FSR0L
MOVFF INT_scratch+1,FSR0H
MOVFF INT_scratch+2,FSR1L
MOVFF INT_scratch+3,FSR1H
MOVFF INT_scratch+4,FSR2L
MOVFF INT_scratch+5,FSR2H
MOVFF INT_scratch+6,scratch
MOVFF INT_scratch+7,scratch1
MOVFF INT_scratch+8,scratch2
MOVFF INT_scratch+9,scratch3
//Here the 'fast' exit.
FEXIT:
RETFIE 1
#ENDASM
}
|
Now, this _gets out of each interrupt quickly_, but shows the basic structure of handling multiple interrupts, with speed.
The main code, monitors 'tick', and when it changes, performs the PID calculations, then the PWM is updated by the timer as you do. This is unecessary though, since the PWM is automatically buffered, and not updated till the next cycle anyway...
Best Wishes |
|
|
Thiago Alves
Guest
|
|
| Posted: Mon Oct 05, 2009 10:34 am |
|
|
I'm working with pics for just a few months and this is part of my graduation studies. So I'm pretty newbie here... =)
But your code pointed me out what I really need to do. I will study it more and see how exactly it works.
Thank you very much! |
|
|
Jaimearctico
Guest
|
| doubt |
| Posted: Fri Jan 29, 2010 6:05 am |
|
|
Hi,
Well, I would like to know what really means this lines:
#byte portb=0xF81
#bit bchanged=0xFF2.0
thanks. |
|
|
Ttelmah
Guest
|
|
| Posted: Fri Jan 29, 2010 6:14 am |
|
|
Try reading the compiler manual, and the data sheet.....
Best Wishes |
|
|
Jaimearctico
Guest
|
|
| Posted: Sun Jan 31, 2010 8:44 am |
|
|
Thanks. I did.
After many changes and mistakes the routines has worked for me.
I have used a PIC18F4550 and a rotary incremental encoders serie 825s GPI, with 6000 maximum line count on disc.
I will make more fixes and I will post the code then.
Thanks for the discussion here.
Jaime |
|
|
maryp
Joined: 30 Jun 2010
Posts: 6
|
| encoder problems |
| Posted: Wed Jun 30, 2010 11:00 am |
|
|
Could somebody PLEASE explain what the "#byte PORTB" & "#bit bchanged" statements do? I don't have the CCS compiler...
thank you. |
|
|
PCM programmer
Joined: 06 Sep 2003
Posts: 21708
|
|
| Posted: Wed Jun 30, 2010 1:24 pm |
|
|
| Quote: | Could somebody PLEASE explain "#byte PORTB
|
#byte is a C language extension of the CCS compiler, that allows you
to assign a symbolic name to a specific RAM address in the PIC. It's
typically used to assign a name to an i/o or peripheral register in the
PIC. In this case, PortB is at address 0xF81. This information is given
in the 18F458 data sheet, in this table:
| Quote: | TABLE 4-1: SPECIAL FUNCTION REGISTER MAP
|
After you have assigned the name "PortB" to its register address, you
can then read or write directly to PortB with a line of C code in your
program. Example:
| Code: |
PortB = 0x55; // Write 0x55 to Port B
|
The #bit statement works the same way, except that it's typically used
to assign names to individual bits in the PIC peripheral registers. |
|
|
maryp
Joined: 30 Jun 2010
Posts: 6
|
|
| Posted: Wed Jun 30, 2010 3:27 pm |
|
|
Thanks a lot for the explanation, though I'm seemingly having some problems getting that code to work perfectly- using PIC18F4550 with 40MHz clock. At a glance it looks like its working fine, but its actually mis-counting the steps- either too much or too little.
Tried playing around with some delays, but that seemingly has no effect. Don't have any other interrupts either. |
|
|
|
|
You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot vote in polls in this forum
|
Powered by phpBB © 2001, 2005 phpBB Group
|
FAQ 
Search
Register
Profile 
Log in to check your private messages 
Log in
