 |
 |
| View previous topic :: View next topic |
| Author |
Message |
nasbyc
Joined: 27 Apr 2009
Posts: 50
|
| pid and interrupt |
 Posted: Wed Oct 26, 2011 9:02 am |
 |
|
I have developed pid algorithms to control motor pwm. I trien to use an interrupt to read potentiometer (10 bit) to determine the position of the motor. The problem that I encounter is that the timer will always interrupt and never execute program in the main. I already change the prescaler but haven't manage to solve this problem.
Alternative method is that I removed the interrupt, and but the code in the main. the problem is that for pid: esp for integral and derivative term. sampling freq need to be known. Because by using interrupt you can just st when is it, it going to read.
| Code: |
#include <16f877a.h>
#device adc = 10
#use delay(clock=20000000)
#fuses hs,noprotect,nowdt,nolvp
#use rs232(baud=9600, xmit=PIN_C6, rcv=PIN_C7, parity=N)
#byte timer0flow = 0x01
void MotorFish_CW(); void MotorFish_CCW(); void MotorFish_Stop ();
int8 pidflag;
int16 Freqsamp = 304;
unsigned int16 Desired_Angle = 535;
signed int16 TError;
unsigned int16 Theta_out;
#int_rtcc
void timer0_isr() {
set_adc_channel(0);
//delay_us(10);
Theta_out = read_adc();
pidflag = 1;
TError = Desired_Angle - Theta_out;
if (Desired_Angle < Theta_out)
{
TError = (~TError +1);
TError =-TError;
}
if ( TError < -135 || TError >= 135 ){
pidflag = 0;
set_pwm2_duty(0);
}
}
void main ()
{
set_tris_b(0b11100001);
setup_port_a(ALL_ANALOG);
setup_adc(ADC_CLOCK_INTERNAL);
setup_ccp2(CCP_PWM);
setup_timer_2(T2_DIV_BY_4, 250, 1); //Frequency = 5kHz
setup_timer_0( RTCC_INTERNAL|RTCC_DIV_256);
enable_interrupts(INT_RTCC);
enable_interrupts(GLOBAL);
set_timer0(0);
signed int16 P_Error, I_Error, D_Error;
signed int16 Acc_Error, Prev_Error1,Prev_Error2,Prev_Error3, Scalling_U_Signal;
int16 Acc_Max, Acc_Min;
int8 P, I, D;
int16 numeric_val;
signed int16 U_Signal,U_Signal1;
unsigned int16 DutyCycle;
P_Error = 0;
I_Error = 0;
D_Error = 0;
U_Signal = 0;
Acc_Error = 0;
Prev_Error1 = 0;
Prev_Error2 = 0;
Prev_Error3 = 0;
Scalling_U_Signal = 0;
DutyCycle = 0;
P = 6;
I = 1;
D = 1;
Acc_Max = 30000;
Acc_Min = -29000;
do
{
if(pidflag == 1){
printf(" Exec\n");
//Calculate Proportional Term
P_Error = TError;
//Calculate Integral Term
Acc_Error = Acc_Error + TError;
//Check Integral Limit
if(Acc_Error >= Acc_Max){
Acc_Error = Acc_Max;
}
if(Acc_Error <= Acc_Min){
Acc_Error = Acc_Min;
}
I_Error = Acc_Error/Freqsamp;
//Calculate Derivative Term
D_Error = (P_Error - Prev_Error3);
//Check Derivative Limit
if(D_Error >= 30){
D_Error = 30;
}
if(D_Error <= -30){
D_Error = -30;
}
D_Error = D_Error*(Freqsamp/3);
//PID Controller
U_Signal = P_Error*P + I_Error*I + D_Error*D;
//Two's Complement
numeric_val = U_Signal & 0x7FFF;
if (U_Signal & 0x8000)
U_Signal = numeric_val - 32768 ;
//Check U_Signal Limit
if(U_Signal >= 32767){
U_Signal = 32768;
}
if(U_Signal <= -32768){
U_Signal = -32768;
}
//Scalling the Duty Cycle
Scalling_U_Signal = U_Signal*((float)1023/32767);
DutyCycle = abs(Scalling_U_Signal);
//Determine PWM Duty Cycle
if(DutyCycle >= 400)
{
DutyCycle = 400;
}
if(DutyCycle <= 220)
{
DutyCycle = 220;
}
//Determine Motor Direction
if(U_Signal > 300)
{
MotorFish_CW();
set_pwm2_duty(DutyCycle);
}
else if(U_Signal < -300)
{
MotorFish_CCW();
set_pwm2_duty(DutyCycle);
}
else
{
MotorFish_Stop();
DutyCycle = 0;
set_pwm2_duty(DutyCycle);
}
Prev_Error3 = Prev_Error2;
Prev_Error2 = Prev_Error1;
Prev_Error1 = TError;
P_Error = 0;
}
printf("Desired_Angle = %lu Potentiometer = %lu Error = %ld PIDflag = %d \n",Desired_Angle, Theta_out, TError,pidflag);
printf("P_Error = %ld I_Error = %ld D_Error = %ld \n",P_Error, I_Error, D_Error);
printf(" U_Signal = %ld U_Signal1 = %ld DutyCycle = %lu \n",U_Signal,U_Signal1, DutyCycle);
set_pwm2_duty(0);
pidflag = 0;
}while(1);
}
void MotorFish_CW()
{
output_low(PIN_B1);
output_high(PIN_B2);
}
void MotorFish_CCW()
{
output_high(PIN_B1);
output_low(PIN_B2);
}
void MotorFish_Stop()
{
output_low(PIN_B1);
output_low(PIN_B2);
}
|
|
|
 |
PCM programmer
Joined: 06 Sep 2003
Posts: 21708
|
|
| Posted: Wed Oct 26, 2011 12:32 pm |
|
|
This code does not look correct:
| Quote: |
signed int16 U_Signal,U_Signal1;
//Check U_Signal Limit
if(U_Signal >= 32767){
U_Signal = 32768;
}
if(U_Signal <= -32768){
U_Signal = -32768;
}
|
What does the CCS manual say about the range of data types ?
| Code: |
Basic and Special types
Unsigned Signed
int16 0 to 65535 -32768 to 32767
|
It says the maximum size of a signed int16 is 32767. But you are setting
it to 32768.
What does your code do ? Make a test program and see. The program
below gives this result:
| Quote: |
U_Signal = -32768
U_Signal = -32768
|
If a number is = 32767 (max positive), your code sets it to the opposite
limit (max negative). That result is probably not what you intended.
| Code: |
#include <16F877A.H>
#fuses XT, NOWDT, NOPROTECT, BROWNOUT, PUT, NOLVP
#use delay(clock=4000000)
#use rs232(baud=9600, xmit=PIN_C6, rcv=PIN_C7, ERRORS)
//==========================================
void main()
{
signed int16 U_Signal, U_Signal1;
U_Signal = 32767; // Set it to max positive value
//Check U_Signal Limit
if(U_Signal >= 32767){
U_Signal = 32768;
}
printf("U_Signal = %ld \r", U_Signal);
if(U_Signal <= -32768){
U_Signal = -32768;
}
printf("U_Signal = %ld \r", U_Signal);
while(1);
} |
|
|
|
nasbyc
Joined: 27 Apr 2009
Posts: 50
|
|
| Posted: Thu Oct 27, 2011 4:59 am |
|
|
| My mistake about the u_signal. But my problem is about the timer. It executes too fast thus ignores the program in the main loop. It just keep reading the pot. |
|
|
Ttelmah
Joined: 11 Mar 2010
Posts: 19506
|
|
| Posted: Thu Oct 27, 2011 5:53 am |
|
|
Think about the timing of reading the ADC. This takes typically 11cycles of the ADC clock, which is allowed to be 1.6uSec _minimum_ for your chip. ADC_CLOCK_iINTERNAL, will give nominally 4uSec _but_ is not recommended for chip operation above 1MHz, and can be up to 6uSec.....
Anyway, ignoring the 'not recommended' bit, the ADC conversion will then take up to about 66uSec, plus the time to read the register (at least another coouple of uSec). Then look at your other operations in the interrupt. 16bit subtraction 1.4uSec. 16bit comparison (about the same). Then a double signed comparison - probably 5uSec+. Then if the conditions are met, several other operations. Add the time to get into and out of the interrupt (about 12uSec), something of the order of 95uSec 'quickest', and probably worse. 1/10000th second....
First thing you can do to improve things _massively_, is to not perform the ADC conversion while you are actually waiting.
So:
At the start of main, set the adc clock to be FOSC/32, not ADC_CLOCK_INTERNAL.
Select the adc channel just once here.
wait 10uSec here, and send the command
read_adc(ADC_START_ONLY);
Then setup the interrupts and start them.
Then in your interrupt routine, get rid of the set_adc_channel (pointless, and wastes time), and use:
Theta_out=read_adc(ADC_READ_ONLY);
At the end of the interrupt code, as the last instruction use:
read_adc(ADC_START_ONLY);
Doing this the ADC conversion is performed _between_ the interrupts, with you reading the value from the last conversion at the start of the routine, and then starting a new conversion before the next interrupt - will probably double the speed of your interrupt routine in one go!....
Now, do you really need the signed values?.
If you tested for Desired_angle being less than Theta_out, you could set a flag for direction, and perform the arithmetic all on +ve values.
Your main loop is going to take an age to execute. Floating point arithmetic, prints, etc. etc.. Even with less time in the interrupt, it is going to be _slow_. If you look at code examples for PID, they will use _scaled_ integer arithmetic, rather than float.
Best Wishes |
|
|
nasbyc
Joined: 27 Apr 2009
Posts: 50
|
|
| Posted: Thu Oct 27, 2011 12:17 pm |
|
|
| Ttelmah wrote: |
Now, do you really need the signed values?.
If you tested for Desired_angle being less than Theta_out, you could set a flag for direction, and perform the arithmetic all on +ve values.
|
Yes, the signed values can determine the direction of motor. Morever I believed that the signed values are use to calculate the u signal (summation of pterm iterm and dterm). Or is there any other ways?
If the values all +ve the how to determine the error either +ve or -ve (esp in accumulation and derivative term). |
|
|
Ttelmah
Joined: 11 Mar 2010
Posts: 19506
|
|
| Posted: Thu Oct 27, 2011 2:54 pm |
|
|
No, you are missing the point.
You don't need signed values.
What you do is have a flag bit called (say) 'direction'. Then test just once in the operation, which number is greater than the other. and if (say) 'where you want to be' is below 'where you are', then set 'direction' to 'out' (so the motor is going up), and subtract where you want to be from where you are in your calculation using unsigned. Similarly if where you want to be is above where you are, set direction to 'in'. and instead subtract where you are from where you want to be. This way your calculations can all be _unsigned_, and a lot faster. Similarly then, 'limit tests', will always apply at the same point, on the single +ve 'offset' value, a lot faster. You can directly control the power bridge with the direction value, again reducing calculation.
Best Wishes |
|
|
nasbyc
Joined: 27 Apr 2009
Posts: 50
|
|
| Posted: Thu Nov 03, 2011 6:12 am |
|
|
Sorry for the late reply. I already changed my program, which disable my interrupt when met the condition (pidflag == 1) and enable it back when the calculation complete. It is because it never calculates in the main loop if I don't disable the interrupt even I changed all to unsigned calc.
About the unsigned value. Yes I understand about setting the dirrection of motor you just can compare whether the desired angle larger or smaller than the actual angle. But the one that confuses me is that with the differential term. Do I need to make it unsigned ? For the derivative term:
| Code: |
D_Error = (P_Error - Prev_Error);
|
If the D_Error is unsigned value than the value will always be +ve. Do I need to put the condition over here for example:
| Code: |
if(P_Error < Prev_Error){
D_Error = Prev_Error - P_Error;
} |
|
|
|
|
|
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
