//===================================================================// // SERVOTST.C // //===================================================================// // By: Dale Botkin <dale@botkin.org> // // Date: 3/15/2000 // //-------------------------------------------------------------------// // Simple C program for the PIC MPU, pretty much any variety. Will // // exercise up to four R/C servos connected to pins RB0:3. I tested // // this with three, but it's pretty obvious you need to power the // // servos separately from the PIC to cut down on glitches. Very // // easy to modify for up to 8 servos, though every servo you add // // sucks up another 10% of available CPU time. Since all we're // // doing is exercising them, though, using 80% of CPU time running // // the pulses is no biggie. // //-------------------------------------------------------------------// // Copyright (C)2000, Dale Botkin, all rights reserved. You may use // // this code freely for personal, non-commercial use. In the highly // // unlikely event that you want to use this code for commercial use, // // email me to make arrangements. // //-------------------------------------------------------------------// #include "16C711.H" // Change for your CPU #include "int16CXX.h" #pragma config WDTE=off, FOSC=XT, BODEN=on #pragma config |= 0x3FB0 // Code protect off char servo_pin; // Variable for port pins char servo[4]; // Servo position values char servo_num; // Pointer to current servo position value char position; // Working servo position value char window; // 2ms - position value char boogies; // Interrupt counter char loops; // Loop counter char current; // same as servo_num, but outside of interrupt bit direction; // direction of travel flag #pragma origin 4 // With .1ms resolution, we can control the servo in 10% steps: // 10 = 0 degrees (or 100% backward motion) 1.0ms // 11 = 18 degrees (or 80% backward motion) 1.1ms // 12 = 36 degrees (or 60% backward motion) 1.2ms // 13 = 54 degrees (or 40% backward motion) 1.3ms // 14 = 72 degrees (or 20% backward motion) 1.4ms // 15 = 90 degrees (or stopped) 1.5ms // 16 = 108 degrees (or 20% forward motion) 1.6ms // 17 = 126 degrees (or 40% forward motion) 1.7ms // 18 = 144 degrees (or 60% forward motion) 1.8ms // 19 = 162 degrees (or 80% forward motion) 1.9ms // 20 = 180 degrees (or 100% forward motion) 2.0ms // So our pulse routine needs to run 1ms + x tenths. // Next version will (maybe) use hundredths, from 100 to 200. interrupt scan_servos() { PORTA = 2; // indicate where we are T0IF = 0; // Clear TMR0 interrupt flag int_save_registers // Save status & W reg char fsr; fsr = FSR; // Save FSR OPTION = 8; // No TMR0 prescaler servo_pin = 1; // Set up for first servo for(servo_num = 0; servo_num <=3; servo_num++) { position = servo[servo_num]; // get pulse width value window = 30 - position; // Figure out remainder of 2ms window PORTB = servo_pin; // Turn on servo pin TMR0 = 0; // Clear TMR0 do { TMR0 = 11; // Adjust for execution time while ( TMR0 < 100 ); // wait 100 uS } while( --position > 0 ); // for as many as we need PORTB = 0; // Turn off all servo pins // Now we have to burn up the remainder of the 2ms total pulse // window... if(window > 0) { do { TMR0 = 11; // Adjust for execution time while ( TMR0 < 100 ); // wait 100 uS } while(--window > 0); // for as many as we need } // And set us up for the next servo in line... servo_pin = servo_pin*2; // Set up for next servo in line } // Now we set up for an 8ms delay until the ext interrupt. // We just spent 8ms pulsing the servos, so in another 12 we // need to do it again. int_restore_registers // Retrieve the registers FSR = fsr; // Restore FSR OPTION = 5; // 256 prescaler for TMR0 TMR0 = 187; // Set up for 12ms interrupt boogies++; // Increment boogie counter PORTA = 0; } void main(){ T0CS = 0; // T0 on instruction cycle T0SE = 1; // rising edge clearRAM(); // start with clear RAM PORTA = 0; PORTB = 0; // All outputs off TRISA = 0; TRISB = 0; // Port B is all output OPTION = 5; // Prescaler set to 256 direction = 0; // Set all servo position registers to the 90 degree mid point. for(current = 0; current < 4; current++) { servo[current] = 15; } TMR0 = 131; // Set up timer for 8ms T0IE = 1; // Enable TMR0 interrupt GIE = 1; // Duh. // The boogie counter gets updated every 20ms, so 50 boogies // equals one second. Tip of the hat to bogomips. // First wait 2 seconds to let us see how the servos do at deadband while(boogies < 100); // Loop for 2 seconds // Now do 60 loops of 10% steps once a second, full motion // range from 10 to 20 and back. Should take 1 minute. direction = 1; loops = 0; PORTA = 1; do { boogies = 0; // clear boogie counter while( boogies < 10 ); // Wait 1/5 second for(current=0; current < 4; current++) { if(servo[current] == 25) direction = 0; if(servo[current] == 5) direction = 1; if(direction == 0) servo[current]--; if(direction == 1) servo[current]++; } } while(++loops < 60); // Now recenter the servos for 2 seconds for(current=0; current < 4; current++) { servo[current] = 15; } boogies = 0; while(boogies < 100); // Now we finish up with 10 full-travel swings, 10 to 20, // with a 2-second delay in between. loops = 0; PORTA = 0xD; do { boogies = 0; while(boogies < 100); // 2 second delay for(current=0; current < 4; current++) { if(servo[current] == 25) direction = 0; else if(servo[current] == 5) direction = 1; else if(servo[current] > 5 && servo[current] < 25) direction = 0; if(direction == 0) servo[current] = 5; if(direction == 1) servo[current] = 25; } } while(++loops < 10); // And loop forever. main(); }
file: /Techref/microchip/servotst.c.htm, 24KB, , updated: 2000/11/28 14:12, local time: 2024/11/14 13:27,
3.136.236.3:LOG IN
|
©2024 These pages are served without commercial sponsorship. (No popup ads, etc...).Bandwidth abuse increases hosting cost forcing sponsorship or shutdown. This server aggressively defends against automated copying for any reason including offline viewing, duplication, etc... Please respect this requirement and DO NOT RIP THIS SITE. Questions? <A HREF="http://techref.massmind.org/techref/microchip/servotst.c.htm"> Colorized Source Code</A> |
Did you find what you needed? |
Welcome to massmind.org! |
Welcome to techref.massmind.org! |
.