CCS :: View topic - Any help to modify code for servomotor of PIC16F877 pls??

FAQ

Forum Help

Official CCS Support

Profile Log in to check your private messages Log in

CCS does not monitor this forum on a regular basis.

Please do not post bug reports on this forum. Send them to CCS Technical Support

Any help to modify code for servomotor of PIC16F877 pls??

CCS Forum Index -> General CCS C Discussion

View previous topic :: View next topic

Author

Message

kingseow

Joined: 22 Jun 2008
Posts: 3
Location: Malaysia

Any help to modify code for servomotor of PIC16F877 pls??

Posted: Sun Jun 22, 2008 10:02 am

hi, i'm currently working on humanoid robot and now searching for the best microcontroller that can work with my algorithm.

I am new with PIC16F877a for this projects and as far as i'm concern,
the design consist 5 servo motors that should run independently at the same time.This is the servomotor i will use (HS-55):
http://www.hobbyhorse.com/hitec_hs55.shtml

i have search throughout this forum and i didn't find anything related about it, or maybe i did miss it. hope all u guys can give me any ideas and suggestions.

thanks.
This is the code i have try myself...but it seen like doesn't work =.=

Code:

unsigned cnt1;
unsigned short cnt2,cnt3,cnt4;
unsigned short move_axis1,move_axis2,move_axis3,move_axis4,move_axis5;
unsigned short position_axis1,position_axis2,position_axis3,position_axis4,position_axis5;
unsigned short enable_axis1, enable_axis2, enable_axis3, enable_axis4, enable_axis5;
unsigned short read_axis1, read_axis2, read_axis3, read_axis4, read_axis5;
unsigned short step;
unsaxis1ort speed_axis1,speed_axis2,speed_axis3,speed_axis4,speed_axis5;
unsigned short mode;

void interrupt() {
//interrupt Routine
cnt1++; // 0.065ms
TMR0 = 232;
INTCON = 0x20; // Set T0IE, clear T0IF
}//~

void main() {
OPTION_REG = 0x80; // Assign prescaler to TMR0
TRISB = 0; // PORTB is output
PORTB = 0xFF; // Initialize PORTB
TRISD = 0;
PORTD = 0;
TMR0 = 232;
INTCON = 0xA0; // Enable TMRO interrupt
cnt1 = 0; // Initialize cnt
cnt2 = 0;
cnt3 = 0;
move_axis1 = 9;
move_axis2 = 9;
move_axis3 = 9;
move_axis4 = 9;
move_axis5 = 9;
position_axis1 = 9;
position_axis2 = 9;
position_axis3 = 9;
position_axis4 = 9;
position_axis5 = 9;
enable_axis1 =0;
enable_axis2 =0;
enable_axis3 =0;
enable_axis4 =0;
enable_axis5 =0;
speed_axis1 = 4;
speed_axis2 = 4;
speed_axis3 = 4;
speed_axis4 = 4;
speed_axis5 = 4;
mode=1;
step=1;
EEprom_Write(1, 0); //for immediate serial execution; refer from senior
Delay_ms(20);
EEprom_Write(2, 0);
Delay_ms(20);
EEprom_Write(3, 0);
Delay_ms(20);
EEprom_Write(4, 0);
Delay_ms(20);
EEprom_Write(5, 0);
Delay_ms(20);
EEprom_Write(51, 141);
Delay_ms(20);
EEprom_Write(52, 138);
Delay_ms(20);
EEprom_Write(53, 134);
Delay_ms(20);
EEprom_Write(54, 138);
Delay_ms(20);
EEprom_Write(55, 141);
Delay_ms(20);
EEprom_Write(101, 0);
Delay_ms(20);
EEprom_Write(102, 0);
Delay_ms(20);
EEprom_Write(103, 0);
Delay_ms(20);
EEprom_Write(104, 0);
Delay_ms(20);
EEprom_Write(105, 0);
Delay_ms(20);
EEprom_Write(151, 0);
Delay_ms(20);
EEprom_Write(152, 0);
Delay_ms(20);
EEprom_Write(153, 0);
Delay_ms(20);
EEprom_Write(154, 0);
Delay_ms(20);
EEprom_Write(155, 0);
Delay_ms(20);
EEprom_Write(201, 0);
Delay_ms(20);
EEprom_Write(202, 0);
Delay_ms(20);
EEprom_Write(203, 0);
Delay_ms(20);
EEprom_Write(204, 0);
Delay_ms(20);
EEprom_Write(205, 0);
Delay_ms(20);

//PWM: inverted high to low
do {
if (cnt1>(position_axis1+14) && enable_axis1 ==1){
PORTD.F0=1;
}
if (cnt1>(position_axis2+14) && enable_axis2 ==1){
PORTD.F1=1;
}
if (cnt1>(position_axis3+14) && enable_axis3 ==1){
PORTD.F2=1;
}
if (cnt1>(position_axis4+14) && enable_axis4 ==1){
PORTD.F3=1;
}
if (cnt1>(position_axis5+14) && enable_axis5 ==1){
PORTD.F4=1;
}

//Time frame
if (cnt1 > 400) {
cnt1=0;
cnt2++; //20ms
PORTD=0;

//Speed Control routine
if (cnt2%speed_axis1==0) {
if (position_axis1 > move_axis1) {
position_axis1--;
}
if (position_axis1 < move_axis1) {
position_axis1++;
}
}
if (cnt2%speed_axis2==0) {
if (position_axis2 > move_axis2) {
position_axis2--;
}
if (position_axis2 < move_axis2) {
position_axis2++;
}
}
if (cnt2%speed_axis3==0) {
if (position_axis3 > move_axis3) {
position_axis3--;
}
if (position_axis3 < move_axis3) {
position_axis3++;
}
}
if (cnt2%speed_axis4==0) {
if (position_axis4 > move_axis4) {
position_axis4--;
}
if (position_axis4 < move_axis4) {
position_axis4++;
}
}
if (cnt2%speed_axis5==0) {
if (position_axis5 > move_axis5) {
position_axis5--;
}
if (position_axis5 < move_axis5) {
position_axis5++;
}
}

//Time frame and demonstration code
if (cnt2>50){ //1s
PORTB = ~PORTB; // Toggle PORTB LEDs
cnt2 = 0; // Reset cnt

if (mode ==1) {
cnt3++;
if (cnt3>30) {
cnt3=0;
}
if (cnt3==1){
enable_axis1=1;
enable_axis2=0;
enable_axis3=0;
enable_axis4=0;
enable_axis5=0;
move_axis1=8;
}
if (cnt3==3){
move_axis1=15;
}
if (cnt3==5){
move_axis1=8;
}
if (cnt3==7){
enable_axis1=0;
enable_axis2=1;
move_axis2=13;
}
if (cnt3==9){
move_axis2=6;
}
if (cnt3==11){
move_axis2=13;
}
if (cnt3==13){
enable_axis2=0;
enable_axis3=1;
move_axis3=13;
}
if (cnt3==15){
move_axis3=3;
}
if (cnt3==17){
move_axis3=13;
}
if (cnt3==19){
enable_axis3=0;
enable_axis4=1;
move_axis4=15;
}
if (cnt3==21){
move_axis4=3;
}
if (cnt3==23){
move_axis4=15;
}
if (cnt3==25){
enable_axis4=0;
enable_axis5=1;
move_axis5=13;
}
if (cnt3==27){
move_axis5=3;
}
if (cnt3==29){
move_axis5=13;
}
}

}

}

} while(1); //infinite loop
}//end program

Display posts from previous:

	CCS Forum Index -> General CCS C Discussion	All times are GMT - 6 Hours
Page 1 of 1

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