Author Topic: About PIC16F877 programming for servo control  (Read 8341 times)

0 Members and 1 Guest are viewing this topic.

Offline kingseowTopic starter

  • Beginner
  • *
  • Posts: 1
  • Helpful? 0
About PIC16F877 programming for servo control
« on: July 02, 2008, 11:14:19 PM »
hi,i am currently doing a robot arm with PIC16F877.But i had facing some problem with programming to control five of the servo motor ...all the servo motor i use have 1.5 -1.9 pulse range(clockwise).Can u guys have a look for my programming which i modify from my brother previous mini project at school? Do correct me pls....! I m just a newbie for PIC programming which start learning 3week ago.

Basiclly the code just work like this...it start the whole process when the power ON ..and servo 1 start to rotate and back to original position,following by servo2 rotate and back to original position and as well as other 3 more servo..

This is the addition information about the maximum allowable range of motion for each servo motor. The value showed is the normalized servo control data for the GUI algorithm and the PIC 16F877 algorithm.

Body Part                         Extension   Flexion
Elbow                                  8   15
Wrist                                  6   13
Thumb(finger1)                   13   3
Index Finger(finger3)                   13   3
Grouped Fingers(finger2)   15   3

Code: [Select]
[size=10pt][size=10pt][size=10pt][size=10pt]unsigned cnt1;
unsigned short cnt2,cnt3,cnt4;
unsigned short move_s1,move_s2,move_s3,move_s4,move_s5;
unsigned short position_s1,position_s2,position_s3,position_s4,position_s5;
unsigned short enable_s1, enable_s2, enable_s3, enable_s4, enable_s5;
unsigned short speed_s1,speed_s2,speed_s3,speed_s4,speed_s5;
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_s1 = 9;
  move_s2 = 9;
  move_s3 = 9;
  move_s4 = 9;
  move_s5 = 9;
  position_s1 = 9;
  position_s2 = 9;
  position_s3 = 9;
  position_s4 = 9;
  position_s5 = 9;
  enable_s1 =0;
  enable_s2 =0;
  enable_s3 =0;
  enable_s4 =0;
  enable_s5 =0;
  speed_s1 = 4;
  speed_s2 = 4;
  speed_s3 = 4;
  speed_s4 = 4;
  speed_s5 = 4;
  mode=1

//PWM: inverted high to low( copy modify from internet )
  do {
    if (cnt1>(position_s1+14) && enable_s1 ==1){
       PORTD.F0=1;
       }
    if (cnt1>(position_s2+14) && enable_s2 ==1){
       PORTD.F1=1;
       }
    if (cnt1>(position_s3+14) && enable_s3 ==1){
       PORTD.F2=1;
       }
    if (cnt1>(position_s4+14) && enable_s4 ==1){
       PORTD.F3=1;
       }
    if (cnt1>(position_s5+14) && enable_s5 ==1){
       PORTD.F4=1;
       }
 

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

//Speed Control routine (slow down servo speed)
       if (cnt2%speed_s1==0) {
          if (position_s1 > move_s1) {
             position_s1--;
             }
          if (position_s1 < move_s1) {
            position_s1++;
            }
          }
       if (cnt2%speed_s2==0) {
          if (position_s2 > move_s2) {
             position_s2--;
             }
          if (position_s2 < move_s2) {
            position_s2++;
            }
          }
       if (cnt2%speed_s3==0) {
          if (position_s3 > move_s3) {
             position_s3--;
             }
          if (position_s3 < move_s3) {
            position_s3++;
            }
          }
       if (cnt2%speed_s4==0) {
          if (position_s4 > move_s4) {
             position_s4--;
             }
          if (position_s4 < move_s4) {
            position_s4++;
            }
          }
       if (cnt2%speed_s5==0) {
          if (position_s5 > move_s5) {
             position_s5--;
             }
          if (position_s5 < move_s5) {
            position_s5++;
            }
          }

//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_s1=1;     //elbow enable
                enable_s2=0;
                enable_s3=0;
                enable_s4=0;
                enable_s5=0;
                move_s1=8;      //elbow move 8
             }
             if (cnt3==3){
                move_s1=15;      //elbow move to 15
             }
             if (cnt3==5){
                move_s1=8;        //elbow move back to 8
                }


             if (cnt3==7){
                enable_s1=0;
                enable_s2=1;     //wrist enable
                move_s2=13;    //wrist move 13
                }
             if (cnt3==9){
                move_s2=6;          //wrist move to 6
                }
             if (cnt3==11){
                move_s2=13;      //wrist move back to 13
                }


             if (cnt3==13){
                enable_s2=0;
                enable_s3=1;        //finger1 enable
                enable_s4=1         //finger2 enable
                enable_s5=1        //finger3 enable
                move_s3=13;      //finger1 move 13
                move_s4=15;      //finger2 move 15
                move_s5=13;    //finger3 move 13
                }
             if (cnt3==15){
                move_s3=3;       //finger1 move to 3
                move_s4=3;        //finger2 move to 3
                move_s5=3;       //finger3 move 3
                }
             if (cnt3==17){
                move_s3=13;        //finger1 move back to 13
                move_s4=15;         //finger2 move back to 15
                move_s5=13;      //finger3 move back 13
                }


          }

       }

    }        //keep looping
  } while(1);
}//~![/size][/size][/size][/size]
http://
« Last Edit: July 03, 2008, 12:42:42 PM by kingseow »

Offline Admin

  • Administrator
  • Supreme Robot
  • *****
  • Posts: 11,657
  • Helpful? 169
    • Society of Robots
Re: About PIC16F877 programming for servo control
« Reply #1 on: July 20, 2008, 08:09:56 AM »
I'm not entirely sure the problem you are having.

Are the servos not moving at all?

A great way to debug microcontroller code is to hook up a UART and watch the output.

 


Get Your Ad Here