Servo Controller – 18 channel control using software
#include<Servo.h>
#include <EEPROM.h>
/******************************************************************************************************************************
#Global Variables
*******************************************************************************************************************************/
int
Value
=
0
,
Angle
=
0
,
/*Command = ‘A’,*/
i
,
Eeprom_Index
=
0
;
int
Seq
[
20
]
,
Seq_Index
=
0
,
Loop_Count
=
0
,
Enabled_Servos
=
0
;
static
boolean
Set_Servo_number
=
0
,
Servo_Enable_Disable
=
0
,
Servo_Reset_Angle
=
0
,
Servo_Reset
=
0
,
Sequence
=
0
,
Sequence_Finish
=
0
;
static
boolean
Get_Servo_Position
=
0
,
Set_Servo_Direction
=
0
,
Set_Servo_Speed
=
0
,
Servo_Default
=
0
,
Command_Finish
=
0
,
Save
=
0
;
char
Command
=
‘A’
;
/******************************************************************************************************************************
#Array of servo objects
*******************************************************************************************************************************/
Servo
servo
[
19
]
;
/******************************************************************************************************************************
#Structure to store parameters
*******************************************************************************************************************************/
struct
Servo_Controller
{
int
Reset_Angle
;
int
Current_Angle
;
int
Prev_Angle
;
int
Speed
;
}
Servo_No
[
19
]
;
/* Array of 18 objects store the parameters of 18 servos */
/******************************************************************************************************************************
#Setup
*******************************************************************************************************************************/
void
setup
(
)
{
Serial
.
begin
(
9600
)
;
for
(
i
=
1
;
i
&
lt
;
19
;
i
++
)
{
if
(
EEPROM
.
read
(
Eeprom_Index
)
==
255
)
{
Servo_No
[
i
]
.
Reset_Angle
=
90
;
Servo_No
[
i
]
.
Current_Angle
=
90
;
Servo_No
[
i
]
.
Prev_Angle
=
90
;
}
else
{
Servo_No
[
i
]
.
Reset_Angle
=
EEPROM
.
read
(
Eeprom_Index
)
;
Servo_No
[
i
]
.
Current_Angle
=
Servo_No
[
i
]
.
Reset_Angle
;
Servo_No
[
i
]
.
Prev_Angle
=
Servo_No
[
i
]
.
Reset_Angle
;
}
if
(
EEPROM
.
read
(
Eeprom_Index
+
1
)
==
255
)
{
Servo_No
[
i
]
.
Speed
=
50
;
}
else
Servo_No
[
i
]
.
Speed
=
EEPROM
.
read
(
Eeprom_Index
+
1
)
;
Serial
.
(
“Servo”
)
;
Serial
.
(
i
)
;
Serial
.
(
” >Rst:”
)
;
Serial
.
(
EEPROM
.
read
(
Eeprom_Index
)
)
;
Serial
.
(
” >Speed:”
)
;
Serial
.
println
(
EEPROM
.
read
(
Eeprom_Index
+
1
)
)
;
Eeprom_Index
+
=
2
;
}
pinMode
(
13
,
OUTPUT
)
;
Serial
.
(
F
(
“Servo Controller”
)
)
;
}
/******************************************************************************************************************************
#Loop
*******************************************************************************************************************************/
void
loop
(
)
{
if
(
Read_command
(
)
)
/* True if new command / sequence received */
{
Check_Command
(
)
;
/* Check the command / sequence received */
Command
=
‘A’
;
/* Reset the command variable */
Value
=
0
;
Angle
=
0
;
}
if
(
Sequence_Finish
)
/* True if sequence is received */
{
Set_Sequence
(
)
;
/* Assign the received angles to corresponding servos */
Rotate_Sequence
(
)
;
/* Turn the servos to new positions */
Seq_Index
=
0
;
Sequence
=
0
;
Seq
[
Seq_Index
]
=
”
;
Sequence_Finish
=
0
;
Loop_Count
=
0
;
/* Reset the variables to receive next sequence */
}
}
/******************************************************************************************************************************
#Function 1 – Read_command()
Extracts all the necessary parameters from the received sequence
*******************************************************************************************************************************/
boolean
Read_command
(
)
{
if
(
Serial
.
available
(
)
&
gt
;
0
)
{
char
Rec_data
=
Serial
.
read
(
)
;
Serial
.
(
Rec_data
)
;
if
(
Set_Servo_number
&
amp
;
&
amp
;
(
Rec_data
!=
‘\r’
&
amp
;
&
amp
;
Rec_data
!=
‘\n’
)
)
{
if
(
isdigit
(
Rec_data
)
)
{
Value
=
(
Value*
10
)
+
(
Rec_data
–
‘0’
)
;
/* Get servo number */
Angle
=
0
;
}
if
(
Sequence
==
1
&
amp
;
&
amp
;
Rec_data
==
‘ ‘
)
{
Seq
[
Seq_Index
]
=
Value
;
/* Extract the angles of 18 servos from the seq. */
Seq_Index
++
;
Value
=
0
;
/* & store in array */
}
}
if
(
Rec_data
==
‘0’
&
amp
;
&
amp
;
Servo_Enable_Disable
)
{
Servo_Enable_Disable
=
0
;
}
if
(
Rec_data
==
‘0’
&
amp
;
&
amp
;
Set_Servo_Direction
)
{
Set_Servo_Direction
=
0
;
}
if
(
(
Servo_Reset_Angle
==
1
||
Get_Servo_Position
==
1
||
Set_Servo_Speed
==
1
)
&
amp
;
&
amp
;
(
Rec_data
!=
‘\r’
&
amp
;
&
amp
;
Rec_data
!=
‘\n’
)
)
{
if
(
isdigit
(
Rec_data
)
)
{
Angle
=
(
Angle*
10
)
+
(
Rec_data
–
‘0’
)
;
/* Get angle received as parameter with command */
}
}
if
(
Set_Servo_number
==
1
&
amp
;
&
amp
;
Rec_data
==
‘:’
)
{
Sequence
=
1
;
/* Seq. received in the format S:<sequence> */
}
if
(
Rec_data
==
‘\n’
)
/* Command ends with \r\n */
{
if
(
Sequence
==
1
)
/* If seq. is received, terminate the array & set flag */
{
Seq
[
Seq_Index
]
=
”
;
Sequence_Finish
=
1
;
}
Command_Finish
=
1
;
Set_Servo_number
=
0
;
return
true
;
}
if
(
Rec_data
==
‘S’
)
{
Set_Servo_number
=
1
;
}
if
(
Rec_data
==
‘E’
)
{
Servo_Enable_Disable
=
1
;
Command
=
‘E’
;
Set_Servo_number
=
0
;
}
if
(
Rec_data
==
‘Z’
)
{
Servo_Reset
=
1
;
Command
=
‘Z’
;
Set_Servo_number
=
0
;
}
if
(
Rec_data
==
‘R’
)
{
Servo_Reset_Angle
=
1
;
Command
=
‘R’
;
Set_Servo_number
=
0
;
}
if
(
Rec_data
==
‘P’
)
{
Get_Servo_Position
=
1
;
Command
=
‘P’
;
Set_Servo_number
=
0
;
Angle
=
0
;
}
if
(
Rec_data
==
‘D’
)
{
Set_Servo_Direction
=
1
;
Command
=
‘D’
;
Set_Servo_number
=
0
;
}
if
(
Rec_data
==
‘s’
)
{
Set_Servo_Speed
=
1
;
Command
=
‘s’
;
Set_Servo_number
=
0
;
}
if
(
Rec_data
==
‘F’
)
{
Servo_Default
=
1
;
Command
=
‘F’
;
Set_Servo_number
=
0
;
}
if
(
Rec_data
==
‘W’
)
{
Save
=
1
;
Command
=
‘W’
;
Set_Servo_number
=
0
;
}
}
return
false
;
}
/******************************************************************************************************************************
#Function 2 – Check_Command()
Perform action based on the command received
*******************************************************************************************************************************/
void
Check_Command
(
)
{
if
(
Command_Finish
)
{
Command_Finish
=
0
;
Serial
.
println
(
Command
)
;
switch
(
Command
)
{
case
‘E’
:
Enable_Disable
(
)
;
break
;
case
‘Z’
:
Reset_Servo
(
)
;
break
;
case
‘R’
:
Set_Reset_Angle
(
)
;
break
;
case
‘P’
:
if
(
(
(
Angle
–
Servo_No
[
Value
]
.
Prev_Angle
)
&
gt
;
3
)
||
(
(
Servo_No
[
Value
]
.
Prev_Angle
–
Angle
)
&
gt
;
3
)
)
Set_Servo_Position
(
)
;
break
;
case
‘D’
:
//Set_Direction();
break
;
case
‘s’
:
Set_Speed
(
)
;
break
;
case
‘F’
:
Factory_Reset
(
)
;
break
;
case
‘W’
:
Save_Parameters
(
)
;
break
;
default
:
break
;
}
}
}
/******************************************************************************************************************************
#Function 3 – Enable_Disable()
Enable / Disable the servo as per the command received
*******************************************************************************************************************************/
void
Enable_Disable
(
)
{
if
(
Servo_Enable_Disable
)
{
Servo_Enable_Disable
=
0
;
Enabled_Servos
++
;
Serial
.
(
“Servo”
)
;
Serial
.
(
Value
)
;
Serial
.
(
“Enabled , “
)
;
Serial
.
(
Enabled_Servos
)
;
servo
[
Value
]
.
attach
(
Value
+
1
)
;
servo
[
Value
]
.
write
(
Servo_No
[
Value
]
.
Reset_Angle
)
;
Servo_No
[
Value
]
.
Prev_Angle
=
Servo_No
[
Value
]
.
Reset_Angle
;
}
else
{
Serial
.
(
“Servo”
)
;
Serial
.
(
Value
)
;
Serial
.
(
“Disabled, “
)
;
servo
[
Value
]
.
detach
(
)
;
Enabled_Servos
—
;
Serial
.
(
Enabled_Servos
)
;
}
Value
=
0
;
Angle
=
0
;
}
/******************************************************************************************************************************
#Function 4 – Reset_Servo()
Resets the servo
*******************************************************************************************************************************/
void
Reset_Servo
(
)
{
Serial
.
(
“Reset “
)
;
Serial
.
(
“Servo”
)
;
Serial
.
(
Value
)
;
if
(
servo
[
Value
]
.
attached
(
)
)
{
servo
[
Value
]
.
write
(
Servo_No
[
Value
]
.
Reset_Angle
)
;
Servo_No
[
Value
]
.
Prev_Angle
=
Servo_No
[
Value
]
.
Reset_Angle
;
Value
=
0
;
Angle
=
0
;
Servo_Reset
=
0
;
}
}
/******************************************************************************************************************************
#Function 5 – Set_Reset_Angle()
Sets reset angle of the servo
*******************************************************************************************************************************/
void
Set_Reset_Angle
(
)
{
Serial
.
(
“Servo”
)
;
Serial
.
(
Value
)
;
Serial
.
(
” Reset Angle : “
)
;
Serial
.
(
Angle
)
;
Servo_No
[
Value
]
.
Reset_Angle
=
Angle
;
Value
=
0
;
Angle
=
0
;
Servo_Reset_Angle
=
0
;
}
/******************************************************************************************************************************
#Function 6 – Set_Servo_Position()
Moves servo to the position received
*******************************************************************************************************************************/
void
Set_Servo_Position
(
)
{
Serial
.
(
“Servo”
)
;
Serial
.
(
Value
)
;
Serial
.
(
” Position : “
)
;
Serial
.
println
(
Angle
)
;
if
(
servo
[
Value
]
.
attached
(
)
)
{
while
(
Angle
!=
Servo_No
[
Value
]
.
Prev_Angle
)
{
Serial
.
(
Servo_No
[
Value
]
.
Prev_Angle
)
;
if
(
Angle
&
lt
;
Servo_No
[
Value
]
.
Prev_Angle
)
{
servo
[
Value
]
.
write
(
Servo_No
[
Value
]
.
Prev_Angle
)
;
Servo_No
[
Value
]
.
Prev_Angle
—
;
}
else
{
servo
[
Value
]
.
write
(
Servo_No
[
Value
]
.
Prev_Angle
)
;
Servo_No
[
Value
]
.
Prev_Angle
++
;
}
delay
(
Servo_No
[
Value
]
.
Speed
)
;
}
}
Value
=
0
;
Angle
=
0
;
Get_Servo_Position
=
0
;
}
/******************************************************************************************************************************
#Function 7 – Set_Speed()
Set the angles as per the sequence received
*******************************************************************************************************************************/
void
Set_Speed
(
)
{
switch
(
Angle
)
{
case
1
:
Servo_No
[
Value
]
.
Speed
=
100
;
break
;
case
2
:
Servo_No
[
Value
]
.
Speed
=
50
;
break
;
case
3
:
Servo_No
[
Value
]
.
Speed
=
20
;
break
;
case
4
:
Servo_No
[
Value
]
.
Speed
=
10
;
break
;
case
5
:
Servo_No
[
Value
]
.
Speed
=
5
;
break
;
default
:
break
;
}
Serial
.
(
“Servo”
)
;
Serial
.
(
Value
)
;
Serial
.
(
” Speed : “
)
;
Serial
.
println
(
Servo_No
[
Value
]
.
Speed
)
;
Value
=
0
;
Angle
=
0
;
Set_Servo_Speed
=
0
;
}
/******************************************************************************************************************************
#Function 8 – Factory Reset()
Initialize the objects with default values
*******************************************************************************************************************************/
void
Factory_Reset
(
)
{
for
(
i
=
1
;
i
&
lt
;
19
;
i
++
)
{
Servo_No
[
i
]
=
{
90
,
90
,
90
,
50
}
;
/* Initialize the objects with default values */
}
Serial
.
(
“Factory Reset”
)
;
Value
=
0
;
Angle
=
0
;
Servo_Default
=
0
;
}
/******************************************************************************************************************************
#Function 9 – Set_Sequence()
Set the angles as per the sequence received
*******************************************************************************************************************************/
void
Set_Sequence
(
)
{
for
(
i
=
0
;
i
&
lt
;
18
;
i
++
)
{
Servo_No
[
i
+
1
]
.
Current_Angle
=
Seq
[
i
]
;
Serial
.
(
i
+
1
)
;
Serial
.
(
‘ ‘
)
;
Serial
.
println
(
Servo_No
[
i
+
1
]
.
Current_Angle
)
;
}
if
(
Servo_No
[
i
+
1
]
.
Current_Angle
==
Servo_No
[
i
+
1
]
.
Prev_Angle
)
Enabled_Servos
—
;
Seq_Index
=
0
;
Sequence
=
0
;
Seq
[
Seq_Index
]
=
”
;
}
while
(
Loop_Count
!=
Enabled_Servos
)
{
for
(
i
=
1
;
i
&
lt
;
19
;
i
++
)
{
if
(
servo
[
i
]
.
attached
(
)
)
{
//Serial.print(“Servo”); Serial.print(i);Serial.print(” “);
if
(
Servo_No
[
i
]
.
Current_Angle
&
lt
;
Servo_No
[
i
]
.
Prev_Angle
)
{
//Serial.print(Servo_No[i].Prev_Angle);Serial.print(“, “);
servo
[
i
]
.
write
(
Servo_No
[
i
]
.
Prev_Angle
)
;
Servo_No
[
i
]
.
Prev_Angle
—
;
if
(
Servo_No
[
i
]
.
Current_Angle
==
Servo_No
[
i
]
.
Prev_Angle
)
{
Loop_Count
++
;
}
}
else
if
(
Servo_No
[
i
]
.
Current_Angle
&
gt
;
Servo_No
[
i
]
.
Prev_Angle
)
{
//Serial.print(Servo_No[i].Prev_Angle);Serial.print(“, “);
servo
[
i
]
.
write
(
Servo_No
[
i
]
.
Prev_Angle
)
;
Servo_No
[
i
]
.
Prev_Angle
++
;
if
(
Servo_No
[
i
]
.
Current_Angle
==
Servo_No
[
i
]
.
Prev_Angle
)
{
Loop_Count
++
;
}
}
}
}
//Serial.print(Loop_Count);Serial.print(“, “);Serial.println(Enabled_Servos);
}
//Serial.println(“Loop Finished”);
}
/******************************************************************************************************************************
#Function 11 – Rotate_Sequence()
Saves the parameters permanently
*******************************************************************************************************************************/
void
Save_Parameters
(
)
{
Eeprom_Index
=
0
;
for
(
i
=
1
;
i
&
lt
;
19
;
i
++
)
{
EEPROM
.
write
(
Eeprom_Index
,
Servo_No
[
i
]
.
Reset_Angle
)
;
EEPROM
.
write
(
Eeprom_Index
+
1
,
Servo_No
[
i
]
.
Speed
)
;
Eeprom_Index
+
=
2
;
}
}
/******************************************************************************************************************************
>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> END <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
*******************************************************************************************************************************/
