SERVOS CODE
/* The Servo functions are
programmed to perform it's actions over a number of
function calls.
Important Functions
ServoInit() Initializes
servo
'Emotion' Servo() Servo control function
Reset() To be used on a emotion transition to reset
counters
*/
#include <delay.h>
//Servo direction
#define Right 1
#define Left 0
#define Up 1
#define Down 0
//Servo task period
#define t5 100
//Variable declaration
char init1=94, init2=55;
char State1,State2;
char direction1, direction2;
char
servocounter1,servocounter2;
char tempcount1,tempcount2;
int servotimer;
void ServoInit(void); // Initialize Servo
void HappyServo(void); // Runs servos through happy
sequence
void ExcitedServo(void); // Runs servos through excited
sequence
void NaughtyServo(void); // Runs servos through naughty
sequence
void SadServo(void); // Runs servos through sad
sequence
void AngryServo(void); // Runs servos through angry
sequence
void BoredServo(void); // Runs servos through bored sequence
void ResetServo(void); // Reset counters
//Initializes servos
void ServoInit(){
TCCR1A=0xA2; //OCR1A, OCR1B set to PWM mode
TCCR1B=0x0C; // Clock set to 1/256
State1=init1; // Servo1 State
State2=init2; // Servo2 State
OCR1A = State1; // Center Servo
OCR1B = State2;
direction1=Right; // Servo1 Initial Direction
direction2=Up; // Servo2 Initial Direction
servocounter1=0; // Number of repetitions
servocounter2=0;
tempcount1=0;
tempcount2=0;
delay_ms(200);
OCR1A = 100; // Center Servo
OCR1B = 61;
delay_ms(200);
OCR1A = State1; // Center Servo
OCR1B = State2;
servotimer=t5;
}
// Resets servo counters to
0 and resets servos to initial positions
void ResetServo(void)
{
servocounter1=0;
servocounter2=0;
tempcount1=0;
tempcount2=0;
State1=init1;
State2=init2;
}
// Following Functions moves
servos controlling the head and arms according to EVAs
current emotion
void HappyServo(){
//Head motion
if(servocounter1<5){
if(direction1==Right){
State1+=2;
OCR1A=State1;
if(State1>120){
direction1=Left;
servocounter1+=1;
}
}
else {
State1-=2;
OCR1A=State1;
if(State1<64){
direction1=Right;
servocounter1+=1;
}
}
}//end if(counter)
else
{
if (direction1==Right && State1<init1){
State1+=2;
OCR1A=State1;
}
else if (direction1==Left &&
State1>init1){
State1-=2;
OCR1A=State1;
}
else
State1=init1;
OCR1A=State1;
} //end Servo 1
// Arms motion
if(servocounter2<3){
if(direction2==Up){
State2+=2;
OCR1B=State2;
if(State2>125){
direction2=Left;
servocounter2+=1;
}
}
else {
State2-=2;
OCR1B=State2;
if(State2<50){
direction2=Right;
servocounter2+=1;
}
}
}//end if(counter)
else
{
if (direction2==Right && State2<init2){
State2+=2;
OCR1B=State2;
}
else if (direction2==Left &&
State2>init2){
State2-=2;
OCR1B=State2;
}
else
State2=init2;
OCR1B=State2;
} // end Servo2
} // end Happy Servo
void ExcitedServo(){
if(servocounter1<3){
if(direction1==Right){
State1+=4;
OCR1A=State1;
if(State1>120){
direction1=Left;
servocounter1+=1;
}
}
else {
State1-=4;
OCR1A=State1;
if(State1<64){
direction1=Right;
servocounter1+=1;
}
}
}//end if(counter)
else
{
if (direction1==Right && State1<94){
State1+=4;
OCR1A=State1;
}
else if (direction1==Left &&
State1>94){
State1-=4;
OCR1A=State1;
}
else
State1=init1;
OCR1A=State1;
} //end Servo 1
if(servocounter2<3){
if(direction2==Right){
State2+=4;
OCR1B=State2;
if(State2>138){
direction2=Left;
servocounter2+=1;
}
}
else {
State2-=4;
OCR1B=State2;
if(State2<50){
direction2=Right;
servocounter2+=1;
}
}
}//end if(counter)
else
{
if (direction2==Right && State2<init2){
State2+=4;
OCR1B=State2;
}
else if (direction2==Left &&
State2>init2){
State2-=4;
OCR1B=State2;
}
else
State2=init2;
OCR1B=State2;
} // end Servo2
} // end Excited Servo
void NaughtyServo(){
if(servocounter1<4){
if(direction1==Right){
State1+=2;
OCR1A=State1;
if(State1>120){
direction1=Left;
servocounter1+=1;
}
}
else {
State1-=2;
OCR1A=State1;
if(State1<64){
direction1=Right;
servocounter1+=1;
}
}
}//end if(counter)
else
{
if (direction1==Right && State1<94){
State1+=2;
OCR1A=State1;
}
else if (direction1==Left && State1>94){
State1-=2;
OCR1A=State1;
}
else
State1=init1;
OCR1A=State1;
} //end Servo 1
if(servocounter2<3){
if(direction2==Right){
State2+=2;
OCR1B=State2;
if(State2>138){
direction2=Left;
servocounter2+=1;
}
}
else {
State2-=2;
OCR1B=State2;
if(State2<50){
direction2=Right;
servocounter2+=1;
}
}
}//end if(counter)
else
{
if (direction2==Right && State2<init2){
State2+=2;
OCR1B=State2;
}
else if (direction2==Left &&
State2>init2){
State2-=2;
OCR1B=State2;
}
else
State2=init2;
OCR1B=State2;
} // end Servo2
} // end Naughty Servo
void SadServo(){
if(servocounter1<4
){
if(direction1==Right){
State1+=1;
OCR1A=State1;
if(State1>120){
direction1=Left;
servocounter1+=1;
}
}
else {
State1-=1;
OCR1A=State1;
if(State1<66){
direction1=Right;
servocounter1+=1;
}
}
}//end if(counter)
else
{
if (direction1==Right && State1<94){
State1+=1;
OCR1A=State1;
}
else if (direction1==Left &&
State1>94){
State1-=1;
OCR1A=State1;
}
else
State1=init1;
OCR1A=State1;
} //end Servo 1
if(servocounter2<3){
if(direction2==Right){
State2+=1;
OCR1B=State2;
if(State2>138){
direction2=Left;
servocounter2+=1;
}
}
else {
State2-=1;
OCR1B=State2;
if(State2<50){
direction2=Right;
servocounter2+=1;
}
}
}//end if(counter)
else
{
if (direction2==Right && State2<init2){
State2+=1;
OCR1B=State2;
}
else if (direction2==Left &&
State2>init2){
State2-=1;
OCR1B=State2;
}
else
State2=init2;
OCR1B=State2;
} // end Servo2
} // end Sad Servo
void AngryServo(){
if(tempcount1<8){
State1=120;
OCR1A=State1;
tempcount1+=1;
}
else
if(tempcount1>7 && tempcount1<20){
State1=60;
OCR1A=State1;
tempcount1+=1;
}
else {
State1=94;
OCR1A=State1;
}
if(tempcount2<8){
State2=120;
OCR1B=State1;
tempcount1+=1;
}
else
if(tempcount2>7 && tempcount2<17){
State2=60;
OCR1B=State2;
tempcount1+=1;
}
else {
State2=init2;
OCR1B=State2;
}
} // end Angry Servo
void BoredServo(){
if(servocounter1<5){
if(direction1==Right){
State1+=2;
OCR1A=State1;
if(State1>120){
direction1=Left;
servocounter1+=1;
}
}
else {
State1-=2;
OCR1A=State1;
if(State1<64){
direction1=Right;
servocounter1+=1;
}
}
}//end if(counter)
else
{
if (direction1==Right && State1<94){
State1+=2;
OCR1A=State1;
}
else if (direction1==Left &&
State1>94){
State1-=2;
OCR1A=State1;
}
else
State1=init1;
OCR1A=State1;
} //end Servo 1
if(servocounter2<3){
if(direction2==Right){
State2+=2;
OCR1B=State2;
if(State2>138){
direction2=Left;
servocounter2+=1;
}
}
else {
State2-=2;
OCR1B=State2;
if(State2<50){
direction2=Right;
servocounter2+=1;
}
}
}//end if(counter)
else
{
if (direction2==Right && State2<init2){
State2+=2;
OCR1B=State2;
}
else if (direction2==Left &&
State2>init2){
State2-=2;
OCR1B=State2;
}
else
State2=init2;
OCR1B=State2;
} // end Servo2
} // end Bored Servo