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
[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://