 |
 |
| View previous topic :: View next topic |
| Author |
Message |
jemmyduc
Joined: 22 May 2013
Posts: 18
|
| Control RC servo with ccp |
 Posted: Wed Dec 04, 2013 5:48 am |
 |
|
I have keyboard to control RC servo and 3 normal DC motor.
I use output copare of ccp1 to control RC servo 1ms-2ms folowing code in forum. I use power pwm to control 3 DC motor.
When press keyboard or scan key to perform one fucntion. RC servo dont work right, i think that the reason is interrupt of output compare, so i put it as FAST interrupt. But it still dont have right control.
This is my code
| Code: |
#include <Slave.h>
#include <math.h>
#include <RS232_Master.c>
#include <Read_ADC.c>
#include <Quet_phim.c>
//#priority timer1,rtcc //uu tien ngat theo thu tu
#include <I2C_Slave.c>
/////RC
#define SERVO_PIN PIN_C0
// These pulse duration values are for a 20 MHz PIC oscillator
// frequency and a Timer1 divisor of 8.
#define PWM_PERIOD 12500L // Gives 20ms PWM period (50 Hz)
#define PULSE_1MS 625L // Gives 1ms high level pulse//old:625
#define PULSE_2MS 825L // Gives 2ms high level pulse//old:1250
#define CENTER 725L
int16 pulse_high_duration;
int8 pulse_done_flag;
//RC-----------------------------------
int16 duty_lung=500,duty_dui=500,duty_chan=500;
int16 count=0;
float angle_zx, angle_zy;
//Keyboard
int8 key;
//Function------------------------------
//INTERRUPT
#INT_RTCC
void interrupt_timer0()
{
count++;
if(count=152)//0.5s
{
count=0;
//AN1
set_adc_channel(1);
ReadADC_Init();
adc_1 = read_adc();
RS232_Float(adc_1);
//AN2
set_adc_channel(2);
ReadADC_Init();
adc_2 = read_adc();
RS232_Float(adc_2);
//AN3
set_adc_channel(3);
ReadADC_Init();
adc_3 = read_adc();
RS232_Float(adc_3);
}
}
#int_ccp1 FAST
void ccp1_isr(void)
{
static int8 set_servo_pin_high = TRUE;
// If servo pin low level pulse time is finished,
// then do the high portion of the signal.
if(set_servo_pin_high)
{
output_high(SERVO_PIN);
set_servo_pin_high = FALSE;
CCP_1 += pulse_high_duration;
}
else // If high portion of signal is done, do the low part.
{
output_low(SERVO_PIN);
set_servo_pin_high = TRUE;
CCP_1 += (PWM_PERIOD - pulse_high_duration);
pulse_done_flag = TRUE;
}
}
//--------------------------------------
void main()
{
int16 i;
setup_timer_1(T1_DIV_BY_8 | T1_INTERNAL);
set_timer1(0);
// Setup CCP to generate CCP1 interrupt when Timer1
// matches the value in the CCP1 registers.
setup_ccp1(CCP_COMPARE_INT);
output_low(SERVO_PIN);
// Initialize the variables and CCP1 to give a 1 ms servo pulse.
pulse_high_duration = CENTER;
CCP_1 = PWM_PERIOD - pulse_high_duration; // Pulse Off time
// This flag tells the main loop code (below) when it's OK
// to change the pulse_high_duration variable.
pulse_done_flag = FALSE;
//INT
clear_interrupt(INT_CCP1);
enable_interrupts(INT_CCP1);
enable_interrupts(INT_RTCC);
enable_interrupts(INT_SSP);
enable_interrupts(GLOBAL);
//Set up ADC
setup_adc(ADC_CLOCK_INTERNAL);
setup_adc_ports(sAN0|sAN1|sAN4|VSS_VDD);
//Set up power pwm
// Setup the 4 Power PWM channels as ordinary pwm channels.
setup_power_pwm_pins(PWM_ODD_ON, PWM_ODD_ON, PWM_ODD_ON, PWM_ODD_ON);
// Mode = Free Run
// Postscale = 1 (1-16) Timebase output postscaler
// TimeBase = 0 (0-65355) Initial value of PWM Timebase
// Period = 2000 (0-4095) Max value of PWM TimeBase
// Compare = 0 (Timebase value for special event trigger)
// Compare Postscale = 1 (Postscaler for Compare value)
// Dead Time
setup_power_pwm(PWM_CLOCK_DIV_4|PWM_FREE_RUN,//modes
1, //postscale
0, //time_base
249, // Period // tan so la 5Khz, full duty = 1000
0, //compare
1, //compare_postscale
0); //dead_time
set_power_pwm0_duty(500);
set_power_pwm2_duty(500);
set_power_pwm4_duty(500);
output_high(pin_b0);
output_high(pin_b2);
output_high(pin_b5);
//Timer0
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_64);//old:64
set_timer0(0);//old:0
//TIMER 1 : behind all setup fucntions
//setup_timer_1(T1_INTERNAL|T1_DIV_BY_1);
//set_timer1(65035); //100us
while(1)
{
// Slowly increase the pulse from 1ms to 2ms.
/*for(i = PULSE_1MS; i < PULSE_2MS; i++)
{
while(!pulse_done_flag); // Wait until pulse is done
pulse_done_flag = FALSE;
disable_interrupts(GLOBAL);
pulse_high_duration = i;
enable_interrupts(GLOBAL);
}*/
if(input(pin_e0)==0)//nut Start
{
key=Scan_keyboard();
if(key==0)//nghieng phai
{
while(!pulse_done_flag); // Wait until pulse is done
pulse_done_flag = FALSE;
disable_interrupts(GLOBAL);
pulse_high_duration-=0.1;
if(pulse_high_duration>PULSE_2MS)
pulse_high_duration = PULSE_2MS;
if(pulse_high_duration<PULSE_1MS)
pulse_high_duration = PULSE_1MS;
enable_interrupts(GLOBAL);
}
if(key==1)//nghieng trai
{
while(!pulse_done_flag); // Wait until pulse is done
pulse_done_flag = FALSE;
disable_interrupts(GLOBAL);
pulse_high_duration+=0.1;
if(pulse_high_duration>PULSE_2MS)
pulse_high_duration = PULSE_2MS;
if(pulse_high_duration<PULSE_1MS)
pulse_high_duration = PULSE_1MS;
enable_interrupts(GLOBAL);
}
if(key==2)
{
duty_lung=1000;
set_power_pwm0_duty(duty_lung);
while(Scan_keyboard()==2);
set_power_pwm0_duty(500);
}
if(key==3)
{
duty_lung=0;
set_power_pwm0_duty(duty_lung);
while(Scan_keyboard()==3);
set_power_pwm0_duty(500);
}
if(key==4)
{
duty_dui=1000;
set_power_pwm2_duty(duty_dui);
while(Scan_keyboard()==4);
set_power_pwm2_duty(500);
}
if(key==5)
{
duty_dui=0;
set_power_pwm2_duty(duty_dui);
while(Scan_keyboard()==5);
set_power_pwm2_duty(500);
}
if(key==6)
{
duty_chan=1000;
set_power_pwm4_duty(duty_chan);
while(Scan_keyboard()==6);
set_power_pwm4_duty(500);
}
if(key==7)
{
duty_chan=0;
set_power_pwm4_duty(duty_chan);
while(Scan_keyboard()==7);
set_power_pwm4_duty(500);
}
}
}
}
|
I hope someone help me fix it. |
|
 |
Ttelmah
Joined: 11 Mar 2010
Posts: 19513
|
|
| Posted: Wed Dec 04, 2013 6:34 am |
|
|
Several things:
1) You are breaking the fundamental rule of keeping interrupts _quick_. The ADC approach is flawed. The ADC needs several mSec to acquire a signal (presumably in your init code), but all the time this is happening the system is locked in the interrupt handler. The approach is wrong. Use a state machine in the CCP. On the first state select a channel, and exit. Then on the next state, start the ADC conversion, and exit. Then on the third call read the already acquired result. Then loop back to the start for the next channel. This way you never pause in the RTCC interrupt.
2) Then your float routine, if I remember from a previous post, you are actually formatting to float and printing. Do not do this inside an interrupt. First, float arithmetic takes too long, and then in fact the result will almost certainly be that the code basically stays in the interrupt for ever. Serial is slow even if you are using a fast rate like 57600bps, it takes 200uSec to send a single character. To send a number, you are talking several mSec. This needs to be done out in the main, where it will not interfere with other interrupts. The odds are you are actually bottlenecking the chip, sticking in the interrupt and doing nothing else.....
3) Now 'FAST', removes the code to save and restore registers. Makes you get into the interrupt quickly, but then _you_ then need to save every register that is modified in this interrupt. It does not give an interrupt priority.
You really need to sort out the fundamental problems first.
However you don't tell us your chip?. If this is a PIC18, then you have give the CCP interrupt 'high priority', which will allow it to interrupt another interrupt. |
|
|
jemmyduc
Joined: 22 May 2013
Posts: 18
|
|
| Posted: Wed Dec 04, 2013 7:16 am |
|
|
How i can set priority for each interrupt? because i use timer to set time read ADC to control position of 3 DC motor.
You can guide me some priority such as how perform it?Do i notice some rule to use #priority |
|
|
jemmyduc
Joined: 22 May 2013
Posts: 18
|
|
| Posted: Wed Dec 04, 2013 7:26 am |
|
|
I try to read ADC in main fucntion . And RC servo have right control. Thank you so much.
So now, i try to contol PID 3 DC motor with 3 ADC feedback.
You can give me some advice with PID controller. |
|
|
temtronic
Joined: 01 Jul 2010
Posts: 9226
Location: Greensville,Ontario
|
|
| Posted: Wed Dec 04, 2013 7:47 am |
|
|
PID with a PIC is easy, maybe 15-20 lines of code.
Of course you need to know some basic things like
which PIC
speed of PIC
what DC motor
ADC resolution
hth
jay |
|
|
jemmyduc
Joined: 22 May 2013
Posts: 18
|
|
| Posted: Wed Dec 04, 2013 8:33 am |
|
|
I'm using PIC 18F4431, crystal 20MHz, DC motor 5KHz, ADC 10 bits. And I use 2 LM298 Drivers to control 3 DC motor with same kind.
You can guide me to reach a PID controller. |
|
|
asmboy
Joined: 20 Nov 2007
Posts: 2128
Location: albany ny
|
|
| Posted: Wed Dec 04, 2013 10:19 am |
|
|
is this an RC , where pulse duration commands servo position?
if so - what make and model servo motor are you using ??
--------------
very many modern so called 'digital' RC servos
DO NOT require constant refresh , and there are other ways
to assert control than with the PWM system. |
|
|
jemmyduc
Joined: 22 May 2013
Posts: 18
|
|
| Posted: Wed Dec 04, 2013 6:50 pm |
|
|
I use analog RC servo, MG996, it seem like all RC servo analog in control.
I have right PID for 3 DC motor, it so great. 
Thank for your help and attention. |
|
|
|
|
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
