Arduino Controlled Servo Exerciser
by kayakdiver in Circuits > Arduino
9397 Views, 53 Favorites, 0 Comments
Arduino Controlled Servo Exerciser
You got your Arduino and you got your Servo...
Here's a cool little program I wrote that will allow you to exercize that servo to your hearts' content. You may find some of the supporting code useful as well!
There's really no other Parts List, so we'll just jump right into the Set Up and then on to the Code.
In this example, I connected the Signal (Yellow) wire of the Servo to Pin 9 of the Arduino.
I picked +5VDC and Ground off the ICSP Connector: Pin 2: 5V and Pin 6: GND.
For my test purposes, I kept this Duemilinove connected to the USB Port of my computer. You can use a Wall-Wart or Battery Pack (for portability) to power your device as well.
Here's a cool little program I wrote that will allow you to exercize that servo to your hearts' content. You may find some of the supporting code useful as well!
There's really no other Parts List, so we'll just jump right into the Set Up and then on to the Code.
In this example, I connected the Signal (Yellow) wire of the Servo to Pin 9 of the Arduino.
I picked +5VDC and Ground off the ICSP Connector: Pin 2: 5V and Pin 6: GND.
For my test purposes, I kept this Duemilinove connected to the USB Port of my computer. You can use a Wall-Wart or Battery Pack (for portability) to power your device as well.
THE OPERATOR DISPLAY
The photo attached to this step depicts the Operator Display that the program will put up on the Monitor Screen in the Arduino Development System or in HyperTerminal. All you need to do is match the Terminal Speed to the Code; in this example it is 115200 BAUD.
Once the screen comes up, enter any key to begin. Note that in the Development System, you must click into the Output Field, type your output, and click on SEND. In HyperTerminal, you simply type away - in most cases the program responds to single key entries.
's' Reduces the Speed and 'S' Increases the Speed at which Sweeps Occur.
'i' and 'I' Control the Move Increment - How many uSecs the Servo is moved at each step.
'd' and 'D' Control the Delay between each Sweep.
'n' and 'N' Set the Minimum Sweep Position.
'm' and 'M' Set the Maximum Sweep Position.
<Space> Pauses Movement.
'?' Displays the Current Values of Min, Max, Speed, Increment, and Delay.
'C' Sets whether to Pause as the Servo passes the Center Position.
'l', 'L', 'r', and 'R' Activate Manual Mode. Automatic Sweeping is Suspended.
'l' and 'r' move the Servo Left and Right 1 uSec on each key-press.
'L' and 'R' move the Servo Left and Right 10 uSecs on each key-press.
'X' Resets all Variables to their Default Values.
'Z' Stores the Current Values to be Recalled upon the next Initialization of theProgram.
'P' Allows a Position to be Entered Manually. Automatic Sweeping is Suspended.
Once the screen comes up, enter any key to begin. Note that in the Development System, you must click into the Output Field, type your output, and click on SEND. In HyperTerminal, you simply type away - in most cases the program responds to single key entries.
's' Reduces the Speed and 'S' Increases the Speed at which Sweeps Occur.
'i' and 'I' Control the Move Increment - How many uSecs the Servo is moved at each step.
'd' and 'D' Control the Delay between each Sweep.
'n' and 'N' Set the Minimum Sweep Position.
'm' and 'M' Set the Maximum Sweep Position.
<Space> Pauses Movement.
'?' Displays the Current Values of Min, Max, Speed, Increment, and Delay.
'C' Sets whether to Pause as the Servo passes the Center Position.
'l', 'L', 'r', and 'R' Activate Manual Mode. Automatic Sweeping is Suspended.
'l' and 'r' move the Servo Left and Right 1 uSec on each key-press.
'L' and 'R' move the Servo Left and Right 10 uSecs on each key-press.
'X' Resets all Variables to their Default Values.
'Z' Stores the Current Values to be Recalled upon the next Initialization of theProgram.
'P' Allows a Position to be Entered Manually. Automatic Sweeping is Suspended.
Here is the entire program - Free for you to use as you like...
This code has been extensively tested and I am not aware of any bugs!
/*************************************************************/
/*** S W E E P U S E C S -- Servo Drive Program V.110308 ***/
/*** Mark Theobald -- www.DisabledAdventurers.com ***/
/*************************************************************/
#include <Servo.h>
#include <EEPROM.h>
Servo Servo1; //Create a Servo Object
int CPOS = 1500; //Current Position (in uSecs)
int SPEED = 2500; //Servo Speed (1-100 * 300)
int DELAY = 1000; //Delay Between Sweeps (0-5000 mSecs)
int INCR = 1; //Servo Increment (1-50 uSecs)
int CSTOP = 1; //Stop at Center (Yes or No)
int AMIN = 675; //Absolute Min (Minimum = 675)
int AMAX = 2325; //Absolute Max (Maximum = 2325)
int MIN, MAX; //Current MIN & MAX Values
int SPCP; //Sweep Passes Center Position
int Ct, LoopCount; //Some Global Variables
char String[80]; //A String Buffer
int Number; //A Number Variable
int ABYTE, EPAD; //for reading & Writing EEPROM
void StoreDefaults( void );
void RetrieveDefaults( void );
void CheckSerial( void );
void SweepTo( int Position );
void ManualRotation( void );
void ManualEntry( void );
void GetANumber( void );
/*************************************************************/
void setup() {
Servo1.attach( 9, AMIN, AMAX ); //Attach the SERVO to Pin 9
Servo1.writeMicroseconds( 1500 ); //Position Servo to Center Position
Serial.begin( 115200 ); //Match this in HyperTerminal!
Serial.print( "\n\r" ); //Display the Operator Menu
Serial.print( "-------------------------------------------- \n\r" );
Serial.print( " S W E E P U S E C S -- Servo Drive Program \n\r" );
Serial.print( "-------------------------------------------- \n\r" );
Serial.print( " s/S=Speed, i/I=Move Increment, d/D=Delay \n\r" );
Serial.print( " n/N=Min, m/M=Max, <Space>=Pause, ?=Display \n\r" );
Serial.print( " C=Center Stop, L/R=Manual Mode, X=Reset \n\r" );
Serial.print( " Z=Store Defaults, P=Enter Postion Manually \n\r" );
Serial.print( "-------------------------------------------- " );
MIN = AMIN; //Initialize MIN and MAX Values to ABSOLUTE MIN & MAX Values
MAX = AMAX;
RetrieveDefaults(); //Read any values that may have been stored in EEPROM
while ( ! Serial.available() ); //Wait for Any Key to Begin
Serial.read(); }
/*************************************************************/
void loop() {
sprintf( String, "\n\r%06d: ", ++LoopCount ); //Keep track of Loops
Serial.print( String );
SweepTo( MAX );
Serial.print( "CW " ); //Indicate last movement
CheckSerial();
delay ( DELAY );
if ( CSTOP && SPCP ) { //We want to stop at center &&
SweepTo( 1500 ); //Sweep passes center position
Serial.print( "Center " );
CheckSerial();
delay ( DELAY ); }
SweepTo( MIN );
Serial.print( "CCW " );
CheckSerial();
delay ( DELAY );
if ( CSTOP && SPCP ) {
SweepTo( 1500 );
Serial.print( "Center " );
CheckSerial();
delay ( DELAY ); } }
/*************************************************************/
void RetrieveDefaults() {
EPAD = 0;
ABYTE = int( EEPROM.read( EPAD++ ) );
MIN = ABYTE << 8;
ABYTE = int( EEPROM.read( EPAD++ ) );
MIN = MIN | ABYTE;
Serial.print( "\n\rMIN:" );
Serial.print( MIN, DEC );
if ( MIN < AMIN || MIN >= MAX ) MIN = AMIN;
ABYTE = int( EEPROM.read( EPAD++ ) );
MAX = ABYTE << 8;
ABYTE = int( EEPROM.read( EPAD++ ) );
MAX = MAX | ABYTE;
Serial.print( " MAX:" );
Serial.print( MAX, DEC );
if ( MAX <= MIN || MAX > AMAX ) MAX = AMAX;
SPCP = MIN < 1500 && MAX > 1500; //Sweep Passes Center Position
ABYTE = int( EEPROM.read( EPAD++ ) );
SPEED = ABYTE << 8;
ABYTE = int( EEPROM.read( EPAD++ ) );
SPEED = SPEED | ABYTE;
Serial.print( " SPEED:" );
Serial.print( SPEED / 100, DEC );
if ( SPEED < 100 || SPEED > 30000 ) SPEED = 2500;
ABYTE = int( EEPROM.read( EPAD++ ) );
DELAY = ABYTE << 8;
ABYTE = int( EEPROM.read( EPAD++ ) );
DELAY = DELAY | ABYTE;
Serial.print( " DELAY:" );
Serial.print( DELAY / 50, DEC );
if ( DELAY < 0 || DELAY > 5000 ) DELAY = 1000;
ABYTE = int( EEPROM.read( EPAD++ ) );
INCR = ABYTE << 8;
ABYTE = int( EEPROM.read( EPAD ) );
INCR = INCR | ABYTE;
Serial.print( " INCR:" );
Serial.print( INCR, DEC );
if ( INCR < 1 || INCR > 50 ) INCR = 50; }
/*************************************************************/
void StoreDefaults() {
EPAD = 0;
EEPROM.write( EPAD++, char( ( MIN >> 8 ) & 0x00FF ) );
EEPROM.write( EPAD++, char( MIN & 0x00FF ) );
EEPROM.write( EPAD++, char( ( MAX >> 8 ) & 0x00FF ) );
EEPROM.write( EPAD++, char( MAX & 0x00FF ) );
EEPROM.write( EPAD++, char( ( SPEED >> 8 ) & 0x00FF ) );
EEPROM.write( EPAD++, char( SPEED & 0x00FF ) );
EEPROM.write( EPAD++, char( ( DELAY >> 8 ) & 0x00FF ) );
EEPROM.write( EPAD++, char( DELAY & 0x00FF ) );
EEPROM.write( EPAD++, char( ( INCR >> 8 ) & 0x00FF ) );
EEPROM.write( EPAD, char( INCR & 0x00FF ) ); }
/*************************************************************/
void CheckSerial() {
char Key;
while ( Serial.available() ) {
if ( ( Key = Serial.read() ) == 's' || Key == 'S' || Key == '?' ) {
if ( Key == 's' && SPEED > 300 ) SPEED -= 300;
if ( Key == 'S' && SPEED < 30000 ) SPEED += 300;
if ( Key == '?' ) Serial.print( "\n\r" );
sprintf( String, " Speed:%d ", SPEED / 300 ); //0 - 100
Serial.print( String ); }
if ( Key == 'i' || Key == 'I' || Key == '?' ) {
if ( Key == 'i' && INCR > 1 ) INCR--;
if ( Key == 'I' && INCR < 50 ) INCR++;
sprintf( String, " Incr:%d ", INCR ); //0 - 50
Serial.print( String ); }
if ( Key == 'd' || Key == 'D' || Key == '?' ) {
if ( Key == 'd' && DELAY > 49 ) DELAY -= 50;
if ( Key == 'D' && DELAY < 5000 ) DELAY += 50;
sprintf( String, " Delay:%d ", DELAY / 50 ); //0 - 100
Serial.print( String ); }
if ( Key == 'n' || Key == 'N' || Key == '?' ) {
if ( Key == 'n' ) if ( ( MIN -= 20 ) < AMIN ) MIN = AMIN;
if ( Key == 'N' ) if ( ( MIN += 20 ) > MAX - 1 ) MIN = MAX - 1;
sprintf( String, " Min:%d ", MIN );
Serial.print( String );
SPCP = MIN < 1500 && MAX > 1500; } //Sweep Passes Center Position
if ( Key == 'm' || Key == 'M' || Key == '?' ) {
if ( Key == 'm' ) if ( ( MAX -= 20 ) < MIN + 1 ) MAX = MIN + 1;
if ( Key == 'M' ) if ( ( MAX += 20 ) > AMAX ) MAX = AMAX;
sprintf( String, " Max:%d ", MAX );
Serial.print( String );
SPCP = MIN < 1500 && MAX > 1500; }
if ( Key == 'c' || Key == 'C' ) {
CSTOP = !CSTOP;
sprintf( String, "\n\rStop @ Center = %s \n\r", CSTOP ? "Yes" : "No" );
Serial.print( String ); }
if ( Key == ' ' ) {
Serial.print( "Pause" );
while ( ! Serial.available() ); //Paused - Wait for Any Key
Serial.read();
Serial.print( "Run " ); }
if ( Key == 'x' || Key == 'X' ) { //Reset all variables
SPEED = 7500;
DELAY = 1000;
INCR = CSTOP = SPCP = 1;
MIN = AMIN;
MAX = AMAX; }
if ( Key == 'r' || Key == 'R' || Key == 'l' || Key == 'L' )
ManualRotation();
if ( Key == 'z' || Key == 'Z' ) StoreDefaults();
if ( Key =='p' || Key == 'P' ) {
while ( Serial.available() ) Serial.read(); //Clear Buffer
ManualEntry(); } } }
/*************************************************************/
void ManualEntry() {
Serial.print( "\n\n\rMANUAL ENTRY MODE" );
sprintf( String, " --- MIN:%d, MAX:%d", MIN, MAX );
Serial.print( String );
Serial.print( "\n\rNumeric Position<Enter>, <Space>=Exit \n\r" );
while ( 1 ) {
GetANumber();
if ( Number < 0 ) return;
sprintf( String, ":%d ", Number );
Serial.print( String );
if ( Number >= MIN && Number <= MAX ) {
CPOS = Number;
Servo1.writeMicroseconds( CPOS );
Serial.print( "\n\r" ); }
else {
Serial.print( "Is Not In Range \n\r" );
sprintf( String, "%d - %d<Enter> or <Space> to Exit...\n\r", MIN, MAX );
Serial.print( String ); } } }
/*************************************************************/
void GetANumber() {
char Key = 0;
Number = 0;
while ( Key != ' ' ) {
if ( Serial.available() ) {
if ( ( Key = Serial.read() ) == 13 ) return; //CR
if ( Key > 47 && Key < 58 ) { //ASCII '0' t h r u '9'
if ( Number ) Number *= 10; //No Leading Zeros!
Number += ( Key - 48 ); } } }
Number = -1;
return; }
/*************************************************************/
void ManualRotation() {
char Key;
Serial.print( "\n\n\rMANUAL ROTATION MODE" );
Serial.print( "\n\rl/L=Left, r/R=Right, <Space>=Exit \n\r" );
while (1) {
if ( Serial.available() ) {
if ( ( Key = Serial.peek() ) == 'p' || Key == 'P' ) return;
if ( ( Key = Serial.read() ) == ' ' ) {
Serial.print( "\n\r" );
return; }
if ( Key == 'r' || Key == 'R' || Key == 'l' || Key == 'L' ) {
if ( Key == 'r' && CPOS < AMAX ) CPOS++;
if ( Key == 'R' ) {
if ( CPOS < AMAX - 9 ) CPOS += 10;
else CPOS = AMAX; }
if ( Key == 'l' && CPOS > AMIN ) CPOS--;
if ( Key == 'L' ) {
if ( CPOS > AMIN + 9 ) CPOS -= 10;
else CPOS = AMIN; }
Servo1.writeMicroseconds( CPOS );
sprintf( String, ":%d ", CPOS );
Serial.print( String ); } } } }
/*************************************************************/
void SweepTo( int Position ) {
if ( CPOS < Position ) while ( ( CPOS += INCR ) <= Position ) {
Servo1.writeMicroseconds( CPOS );
for ( Ct = SPEED; Ct < 30000; Ct++ ); }
else while ( ( CPOS -= INCR ) >= Position ) {
Servo1.writeMicroseconds( CPOS );
for ( Ct = SPEED; Ct < 30000; Ct++ ); }
if ( CPOS != Position ) Servo1.writeMicroseconds( CPOS = Position ); }
/*************************************************************/
/******** END PROGRAM ********/
/*************************************************************/
This code has been extensively tested and I am not aware of any bugs!
/*************************************************************/
/*** S W E E P U S E C S -- Servo Drive Program V.110308 ***/
/*** Mark Theobald -- www.DisabledAdventurers.com ***/
/*************************************************************/
#include <Servo.h>
#include <EEPROM.h>
Servo Servo1; //Create a Servo Object
int CPOS = 1500; //Current Position (in uSecs)
int SPEED = 2500; //Servo Speed (1-100 * 300)
int DELAY = 1000; //Delay Between Sweeps (0-5000 mSecs)
int INCR = 1; //Servo Increment (1-50 uSecs)
int CSTOP = 1; //Stop at Center (Yes or No)
int AMIN = 675; //Absolute Min (Minimum = 675)
int AMAX = 2325; //Absolute Max (Maximum = 2325)
int MIN, MAX; //Current MIN & MAX Values
int SPCP; //Sweep Passes Center Position
int Ct, LoopCount; //Some Global Variables
char String[80]; //A String Buffer
int Number; //A Number Variable
int ABYTE, EPAD; //for reading & Writing EEPROM
void StoreDefaults( void );
void RetrieveDefaults( void );
void CheckSerial( void );
void SweepTo( int Position );
void ManualRotation( void );
void ManualEntry( void );
void GetANumber( void );
/*************************************************************/
void setup() {
Servo1.attach( 9, AMIN, AMAX ); //Attach the SERVO to Pin 9
Servo1.writeMicroseconds( 1500 ); //Position Servo to Center Position
Serial.begin( 115200 ); //Match this in HyperTerminal!
Serial.print( "\n\r" ); //Display the Operator Menu
Serial.print( "-------------------------------------------- \n\r" );
Serial.print( " S W E E P U S E C S -- Servo Drive Program \n\r" );
Serial.print( "-------------------------------------------- \n\r" );
Serial.print( " s/S=Speed, i/I=Move Increment, d/D=Delay \n\r" );
Serial.print( " n/N=Min, m/M=Max, <Space>=Pause, ?=Display \n\r" );
Serial.print( " C=Center Stop, L/R=Manual Mode, X=Reset \n\r" );
Serial.print( " Z=Store Defaults, P=Enter Postion Manually \n\r" );
Serial.print( "-------------------------------------------- " );
MIN = AMIN; //Initialize MIN and MAX Values to ABSOLUTE MIN & MAX Values
MAX = AMAX;
RetrieveDefaults(); //Read any values that may have been stored in EEPROM
while ( ! Serial.available() ); //Wait for Any Key to Begin
Serial.read(); }
/*************************************************************/
void loop() {
sprintf( String, "\n\r%06d: ", ++LoopCount ); //Keep track of Loops
Serial.print( String );
SweepTo( MAX );
Serial.print( "CW " ); //Indicate last movement
CheckSerial();
delay ( DELAY );
if ( CSTOP && SPCP ) { //We want to stop at center &&
SweepTo( 1500 ); //Sweep passes center position
Serial.print( "Center " );
CheckSerial();
delay ( DELAY ); }
SweepTo( MIN );
Serial.print( "CCW " );
CheckSerial();
delay ( DELAY );
if ( CSTOP && SPCP ) {
SweepTo( 1500 );
Serial.print( "Center " );
CheckSerial();
delay ( DELAY ); } }
/*************************************************************/
void RetrieveDefaults() {
EPAD = 0;
ABYTE = int( EEPROM.read( EPAD++ ) );
MIN = ABYTE << 8;
ABYTE = int( EEPROM.read( EPAD++ ) );
MIN = MIN | ABYTE;
Serial.print( "\n\rMIN:" );
Serial.print( MIN, DEC );
if ( MIN < AMIN || MIN >= MAX ) MIN = AMIN;
ABYTE = int( EEPROM.read( EPAD++ ) );
MAX = ABYTE << 8;
ABYTE = int( EEPROM.read( EPAD++ ) );
MAX = MAX | ABYTE;
Serial.print( " MAX:" );
Serial.print( MAX, DEC );
if ( MAX <= MIN || MAX > AMAX ) MAX = AMAX;
SPCP = MIN < 1500 && MAX > 1500; //Sweep Passes Center Position
ABYTE = int( EEPROM.read( EPAD++ ) );
SPEED = ABYTE << 8;
ABYTE = int( EEPROM.read( EPAD++ ) );
SPEED = SPEED | ABYTE;
Serial.print( " SPEED:" );
Serial.print( SPEED / 100, DEC );
if ( SPEED < 100 || SPEED > 30000 ) SPEED = 2500;
ABYTE = int( EEPROM.read( EPAD++ ) );
DELAY = ABYTE << 8;
ABYTE = int( EEPROM.read( EPAD++ ) );
DELAY = DELAY | ABYTE;
Serial.print( " DELAY:" );
Serial.print( DELAY / 50, DEC );
if ( DELAY < 0 || DELAY > 5000 ) DELAY = 1000;
ABYTE = int( EEPROM.read( EPAD++ ) );
INCR = ABYTE << 8;
ABYTE = int( EEPROM.read( EPAD ) );
INCR = INCR | ABYTE;
Serial.print( " INCR:" );
Serial.print( INCR, DEC );
if ( INCR < 1 || INCR > 50 ) INCR = 50; }
/*************************************************************/
void StoreDefaults() {
EPAD = 0;
EEPROM.write( EPAD++, char( ( MIN >> 8 ) & 0x00FF ) );
EEPROM.write( EPAD++, char( MIN & 0x00FF ) );
EEPROM.write( EPAD++, char( ( MAX >> 8 ) & 0x00FF ) );
EEPROM.write( EPAD++, char( MAX & 0x00FF ) );
EEPROM.write( EPAD++, char( ( SPEED >> 8 ) & 0x00FF ) );
EEPROM.write( EPAD++, char( SPEED & 0x00FF ) );
EEPROM.write( EPAD++, char( ( DELAY >> 8 ) & 0x00FF ) );
EEPROM.write( EPAD++, char( DELAY & 0x00FF ) );
EEPROM.write( EPAD++, char( ( INCR >> 8 ) & 0x00FF ) );
EEPROM.write( EPAD, char( INCR & 0x00FF ) ); }
/*************************************************************/
void CheckSerial() {
char Key;
while ( Serial.available() ) {
if ( ( Key = Serial.read() ) == 's' || Key == 'S' || Key == '?' ) {
if ( Key == 's' && SPEED > 300 ) SPEED -= 300;
if ( Key == 'S' && SPEED < 30000 ) SPEED += 300;
if ( Key == '?' ) Serial.print( "\n\r" );
sprintf( String, " Speed:%d ", SPEED / 300 ); //0 - 100
Serial.print( String ); }
if ( Key == 'i' || Key == 'I' || Key == '?' ) {
if ( Key == 'i' && INCR > 1 ) INCR--;
if ( Key == 'I' && INCR < 50 ) INCR++;
sprintf( String, " Incr:%d ", INCR ); //0 - 50
Serial.print( String ); }
if ( Key == 'd' || Key == 'D' || Key == '?' ) {
if ( Key == 'd' && DELAY > 49 ) DELAY -= 50;
if ( Key == 'D' && DELAY < 5000 ) DELAY += 50;
sprintf( String, " Delay:%d ", DELAY / 50 ); //0 - 100
Serial.print( String ); }
if ( Key == 'n' || Key == 'N' || Key == '?' ) {
if ( Key == 'n' ) if ( ( MIN -= 20 ) < AMIN ) MIN = AMIN;
if ( Key == 'N' ) if ( ( MIN += 20 ) > MAX - 1 ) MIN = MAX - 1;
sprintf( String, " Min:%d ", MIN );
Serial.print( String );
SPCP = MIN < 1500 && MAX > 1500; } //Sweep Passes Center Position
if ( Key == 'm' || Key == 'M' || Key == '?' ) {
if ( Key == 'm' ) if ( ( MAX -= 20 ) < MIN + 1 ) MAX = MIN + 1;
if ( Key == 'M' ) if ( ( MAX += 20 ) > AMAX ) MAX = AMAX;
sprintf( String, " Max:%d ", MAX );
Serial.print( String );
SPCP = MIN < 1500 && MAX > 1500; }
if ( Key == 'c' || Key == 'C' ) {
CSTOP = !CSTOP;
sprintf( String, "\n\rStop @ Center = %s \n\r", CSTOP ? "Yes" : "No" );
Serial.print( String ); }
if ( Key == ' ' ) {
Serial.print( "Pause" );
while ( ! Serial.available() ); //Paused - Wait for Any Key
Serial.read();
Serial.print( "Run " ); }
if ( Key == 'x' || Key == 'X' ) { //Reset all variables
SPEED = 7500;
DELAY = 1000;
INCR = CSTOP = SPCP = 1;
MIN = AMIN;
MAX = AMAX; }
if ( Key == 'r' || Key == 'R' || Key == 'l' || Key == 'L' )
ManualRotation();
if ( Key == 'z' || Key == 'Z' ) StoreDefaults();
if ( Key =='p' || Key == 'P' ) {
while ( Serial.available() ) Serial.read(); //Clear Buffer
ManualEntry(); } } }
/*************************************************************/
void ManualEntry() {
Serial.print( "\n\n\rMANUAL ENTRY MODE" );
sprintf( String, " --- MIN:%d, MAX:%d", MIN, MAX );
Serial.print( String );
Serial.print( "\n\rNumeric Position<Enter>, <Space>=Exit \n\r" );
while ( 1 ) {
GetANumber();
if ( Number < 0 ) return;
sprintf( String, ":%d ", Number );
Serial.print( String );
if ( Number >= MIN && Number <= MAX ) {
CPOS = Number;
Servo1.writeMicroseconds( CPOS );
Serial.print( "\n\r" ); }
else {
Serial.print( "Is Not In Range \n\r" );
sprintf( String, "%d - %d<Enter> or <Space> to Exit...\n\r", MIN, MAX );
Serial.print( String ); } } }
/*************************************************************/
void GetANumber() {
char Key = 0;
Number = 0;
while ( Key != ' ' ) {
if ( Serial.available() ) {
if ( ( Key = Serial.read() ) == 13 ) return; //CR
if ( Key > 47 && Key < 58 ) { //ASCII '0' t h r u '9'
if ( Number ) Number *= 10; //No Leading Zeros!
Number += ( Key - 48 ); } } }
Number = -1;
return; }
/*************************************************************/
void ManualRotation() {
char Key;
Serial.print( "\n\n\rMANUAL ROTATION MODE" );
Serial.print( "\n\rl/L=Left, r/R=Right, <Space>=Exit \n\r" );
while (1) {
if ( Serial.available() ) {
if ( ( Key = Serial.peek() ) == 'p' || Key == 'P' ) return;
if ( ( Key = Serial.read() ) == ' ' ) {
Serial.print( "\n\r" );
return; }
if ( Key == 'r' || Key == 'R' || Key == 'l' || Key == 'L' ) {
if ( Key == 'r' && CPOS < AMAX ) CPOS++;
if ( Key == 'R' ) {
if ( CPOS < AMAX - 9 ) CPOS += 10;
else CPOS = AMAX; }
if ( Key == 'l' && CPOS > AMIN ) CPOS--;
if ( Key == 'L' ) {
if ( CPOS > AMIN + 9 ) CPOS -= 10;
else CPOS = AMIN; }
Servo1.writeMicroseconds( CPOS );
sprintf( String, ":%d ", CPOS );
Serial.print( String ); } } } }
/*************************************************************/
void SweepTo( int Position ) {
if ( CPOS < Position ) while ( ( CPOS += INCR ) <= Position ) {
Servo1.writeMicroseconds( CPOS );
for ( Ct = SPEED; Ct < 30000; Ct++ ); }
else while ( ( CPOS -= INCR ) >= Position ) {
Servo1.writeMicroseconds( CPOS );
for ( Ct = SPEED; Ct < 30000; Ct++ ); }
if ( CPOS != Position ) Servo1.writeMicroseconds( CPOS = Position ); }
/*************************************************************/
/******** END PROGRAM ********/
/*************************************************************/