 |
 |
| View previous topic :: View next topic |
| Author |
Message |
btklister
Joined: 16 Jul 2009
Posts: 14
|
| Multi servo controller. |
 Posted: Sat Apr 10, 2010 8:55 am |
 |
|
This code reads UART values and generates 50hz pwm, 50 steps for drive servo motors, 6 servos in this case, but you can change as many as you want by adding extra lines.
How it works:
Every 50us an interrupt is generated, so when passed 400 interrupts 400*50us=20ms required for servo. So by adding some counters to interrupt to count the time passed, we can control when IO port is pulled to 0. When the counter reach 400 we do the same again, creating a pwm.
This controller is a uart slave to control it is require send 3 bytes to it:
0xsomething > to trigger the ready pin (pin_c3)
+
0xID > ID of servo (0x00 to 0x05 in this case)
+
0xPOS > position of servo (0x00 to 0x32, 50 steps)
| Code: |
#include <16F688.h>
#device adc=8
#FUSES NOWDT //No Watch Dog Timer
#FUSES HS //High speed Osc (> 4mhz for PCM/PCH) (>10mhz for PCD)
#FUSES NOPROTECT //Code not protected from reading
#FUSES NOBROWNOUT //No brownout reset
#FUSES MCLR //Master Clear pin enabled
#FUSES NOCPD //No EE protection
#FUSES PUT //Power Up Timer
#FUSES IESO //Internal External Switch Over mode enabled
#FUSES FCMEN //Fail-safe clock monitor enabled
#use delay(clock=20000000)
#use rs232(baud=9600,parity=N,xmit=PIN_C4,rcv=PIN_C5,bits=8)
int16 end=0;
int8 aux=0,c0=25,s0=25,c1=0,s1=25,c2=0,s2=25,c3=0,s3=25,c4=0,s4=25,c5=0,s5=25;
int8 code[3];
int1 flag=0;
void brain();
void clear();
void read();
void config();
void getcode();
#int_TIMER0 //Interrupt every 50us
void TIMER0_isr(void)
{
brain(); //turn of the IO in respective time
}
void main()
{
setup_adc_ports(NO_ANALOGS|VSS_VDD);
setup_adc(ADC_OFF);
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
setup_timer_1(T1_DISABLED);
setup_comparator(NC_NC_NC_NC);
setup_vref(FALSE);
enable_interrupts(INT_TIMER0);
enable_interrupts(GLOBAL);
while(1)
{
if(kbhit()) //read uart if a char is arived
getcode();
read();
}
}
void clear()
{
//will reset the pulse
output_high(pin_c2);
output_high(pin_c1);
output_high(pin_c0);
output_high(pin_a2);
output_high(pin_a1);
output_high(pin_a0);
c0=0;c1=0;c2=0;c3=0;c4=0;c5=0;
end=0;
}
void brain()
{
if(c0==s0)
output_low(pin_c2);
if(c1==s1)
output_low(pin_c1);
if(c2==s2)
output_low(pin_c0);
if(c3==s3)
output_low(pin_a2);
if(c4==s4)
output_low(pin_a1);
if(c5==s5)
output_low(pin_a0);
if(end==400) //400*50us = 20ms (wave periodo required for drive a servo)
clear();
c0++;
c1++;
c2++;
c3++;
c4++;
c5++;
end++;
}
void read()
{
if(flag==1)
{
switch (code[0])
{
case 0:
s0=code[1];
break;
case 1:
s1=code[1];
break;
case 2:
s2=code[1];
break;
case 3:
s3=code[1];
break;
case 4:
s4=code[1];
break;
case 5:
s5=code[1];
break;
default:
break;
}
flag=0;
}
}
void getcode()
{
char c;
c=getc();
flag=0;
switch (aux)
{
case 0: //active ready bit with any char received (trash)
output_high(pin_c3);
break;
case 1:
code[0]=c; //store in array servo ID (0-5)
output_low(pin_c3);
break;
case 2:
if(c>=0 && c<=50) //sote in array servo position
code[1]=c;
flag=1;
break;
}
aux++;
if(flag==1)
aux=0;
//after the 3 char were received we have an array with trash+ID+POSITION
}
|
Cumpz
Fabio Rico |
|
 |
Ali_P
Joined: 18 Oct 2011
Posts: 7
Location: Pakistan
|
|
| Posted: Thu Oct 20, 2011 4:43 am |
|
|
Greetings btklister,
Great code ! I wanted to control 4 servos since your program has 6 servo control. It shouldn't be a problem, but when I simulate your code and send command from tx, 2 servos work properly. But if I add command for the 3rd servo, only the 3rd works and the other two doesn't work. Could you explain why? Or what should I do to counter it?
Best Regards
_________________
Ali |
|
|
John P
Joined: 17 Sep 2003
Posts: 331
|
|
| Posted: Fri Oct 28, 2011 6:29 am |
|
|
Even if this works, I don't think it's adequate. With a resolution of 50usec, given that a hobby servo has an "official" active range of 1msec, you only have 20 potential stopping positions. I wouldn't accept a driver that has such coarse control of the servo.
By "active range" I mean the time interval over which control is possible, i.e. 1msec < T < 2msec. Some servos will accept wider ranges for increased travel. |
|
|
btklister
Joined: 16 Jul 2009
Posts: 14
|
|
| Posted: Sun Nov 13, 2011 6:31 am |
|
|
| Usually simulators don't work properly with fast sitching pins, try to implement in a test board, it works for me. |
|
|
mbradley
Joined: 11 Jul 2009
Posts: 118
Location: California, USA
|
|
| Posted: Mon Nov 14, 2011 10:50 pm |
|
|
I am not trying to take away from any ones glory, but here is code for a 32, 16, or 8 chanel servo controller, all tested, opensource, CCS compiled, board available, etc....
http://www.mculabs.com/projects/ssx32.html
_________________
Michael Bradley
www.mculabs.com
Open Drivers and Projects |
|
|
hasimi20
Joined: 04 Sep 2010
Posts: 19
|
|
| Posted: Sun Mar 24, 2013 9:21 am |
|
|
| It works for me! my Q is, how can I increase the servo positioning angle from 0 to 50, to 0 to 100 maybe? |
|
|
|
|
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
