D:\education\MY PSM\program files\test servo1.

#include <p18f452.h>

#pragma config OSC=HS, OSCS=OFF, BOR=OFF, WDT=OFF, PWRT=OFF, CCP2MUX=ON

#pragma config STVR=OFF, LVP=OFF, DEBUG=OFF

#define servo1 LATBbits.LATB0

#define servo2 LATBbits.LATB1
#define servo3 LATBbits.LATB2

#define stepper1_1 LATDbits.LATD0

#define stepper1_2 LATDbits.LATD1
#define stepper1_3 LATDbits.LATD2
#define stepper1_4 LATDbits.LATD3

#define stepper2_1 LATDbits.LATD4

#define stepper2_2 LATDbits.LATD5
#define stepper2_3 LATDbits.LATD6
#define stepper2_4 LATDbits.LATD7

#define stepper3_1 LATCbits.LATC2

#define stepper3_2 LATCbits.LATC5
#define stepper3_3 LATCbits.LATC6
#define stepper3_4 LATCbits.LATC7

#define DC_motor_a LATAbits.LATA2

#define DC_motor_b LATAbits.LATA5

//unsigned char speed;

void ISRHigh(void);
void ISRLow(void);
void delay(unsigned int data);
void delay_int(unsigned int data);
void initial(void);
void Delay_ms(unsigned int);
void Sequence_left(unsigned long step);
void Sequence_right(unsigned long step);
void Sequence_up(unsigned long step);
void Sequence_down(unsigned long step);
void Sequence_22degR_down(unsigned long step);
void Sequence_22degR_up(unsigned long step);
void Steer_horizon(void);
void Steer_vertical(void);
void Steer_22degR(void);
void Steer_22degL(void);
void pen_up_down(int x);
void Letter_A(void);
void Letter_E(void);
void Letter_F(void);
void Letter_H(void);
void Letter_I(void);
void Letter_L(void);
//========================= Declare variable ==========================//
unsigned int servo=0;
unsigned int servo1_pulse =60; //home position
unsigned int servo2_pulse =45; //home position
unsigned int servo3_pulse =230; //home position
//----------------------------------------------------------------------------

void main (void)

{
initial();
servo1=1;
servo2=1;
servo3=1;
DC_motor_a=0;
DC_motor_b=0;
pen_up_down(0);
Delay_ms(6000);

Letter_A();
Letter_E();
Letter_F();

1
 D:\education\MY PSM\program files\test servo1.c

Letter_H();
Letter_I();
Letter_L();

while(1);
}

//========================= Function Initial ==========================//

void initial(void)
{
TRISA = 0b00000000; // PORTB is output
TRISB = 0b00000000; // Initialize PORT direction
TRISD = 0b00000000;
TRISC = 0b00000000; // PORT A,D,C is output

T0CON = 0b10000111; // timer 0 is selected //0b11000110 16 bit high to low prescale

1:256
TMR0H=0; // write // must clear high first
TMR0L=0;

T1CON = 0b00010111; // Configure TIMER 1 as counter

TMR1H = 0; // must clear high first
TMR1L = 0;

TMR3H = 0xCF;
TMR3L = 0x2B; //initial value in TMR3

T3CON = 0b00110001; // prescale 1:8

RCONbits.IPEN = 1; // enable the priority the interrupt // the interrupt priority feature
enable
// can be write like this : INTCONbits.GIE = 1;
// INTCONbits.PEIE = 1;

INTCON = 0x80; // 0b1000 0000 enable the high priority interrupt

// when we disable the low priority interrupt
PIR2bits.TMR3IF = 0;
PIE2bits.TMR3IE = 1;
IPR2bits.TMR3IP = 1; // Configure TIMER 3 as interrupt on overflow

//========================= Function delay ==========================//

void delay(unsigned int data)

{
unsigned char i;
for( ;data>0;data--)
{
for(i=150;i>0;i--);
}
}

void delay_int(unsigned int data)

{
unsigned char i;
for( ;data>0;data--);
}

void Delay_ms(unsigned int data)

{
unsigned y;

for(;data>0;data--)
{
for(y = 255;y>0;y--) //asal Y=255
{}

}
}

2
 D:\education\MY PSM\program files\test servo1.c

//========================= Function Interrupt vector ==========================//

#pragma code InterruptVectorHigh = 0x08//adress

void InterruptVectorHigh(void)
{
_asm
goto ISRHigh //jump to interrupt routine
_endasm
}
#pragma code
#pragma code InterruptVectorLow = 0x18
void InterruptVectorLow(void)
{
_asm
goto ISRLow //jump to interrupt routine
_endasm
}
#pragma code

//----------------------------------------------------------------------------
// Interupt Service Routine

#pragma interrupt ISRHigh

void ISRHigh(void)
{
if(PIR2bits.TMR3IF == 1) //Timer 3 interrupt on overflow
{
PIR2bits.TMR3IF = 0; // must clear this bit

TMR3H = 0xCF; //asal 9E

TMR3L = 0x2B; //asal 57

servo1=1;
servo2=1;
servo3=1;
delay_int(145); // 0.642m 220 // 0 degree 168

while(servo < 285)

{
if(servo1_pulse==servo)servo1=0;
if(servo2_pulse==servo)servo2=0;
if(servo3_pulse==servo)servo3=0;
servo+=1;

}
servo=0;

}
}

#pragma interrupt ISRLow

void ISRLow(void)
{

//====================== Function of sequence on stepper motor =======================//

void Sequence_left(unsigned long step)

{int i;
for(i=0;i < step;i++)
{
stepper1_1 = 1; stepper1_2 = 0; stepper1_3 = 0; stepper1_4 = 0;
stepper2_1 = 0; stepper2_2 = 0; stepper2_3 = 0; stepper2_4 = 1;
stepper3_1 = 0; stepper3_2 = 0; stepper3_3 = 0; stepper3_4 = 1;
Delay_ms(30);
stepper1_1 = 0; stepper1_2 = 1; stepper1_3 = 0; stepper1_4 = 0;
stepper2_1 = 0; stepper2_2 = 0; stepper2_3 = 1; stepper2_4 = 0;
stepper3_1 = 0; stepper3_2 = 0; stepper3_3 = 1; stepper3_4 = 0;
Delay_ms(30);
stepper1_1 = 0; stepper1_2 = 0; stepper1_3 = 1; stepper1_4 = 0;
stepper2_1 = 0; stepper2_2 = 1; stepper2_3 = 0; stepper2_4 = 0;
stepper3_1 = 0; stepper3_2 = 1; stepper3_3 = 0; stepper3_4 = 0;
Delay_ms(30);
stepper1_1 = 0; stepper1_2 = 0; stepper1_3 = 0; stepper1_4 = 1;

3
 D:\education\MY PSM\program files\test servo1.c

stepper2_1 = 1; stepper2_2 = 0; stepper2_3 = 0; stepper2_4 = 0;

stepper3_1 = 1; stepper3_2 = 0; stepper3_3 = 0; stepper3_4 = 0;
Delay_ms(30);
}
}

void Sequence_right(unsigned long step)

{int i;
for(i=0;i < step;i++)
{
stepper1_1 = 0; stepper1_2 = 0; stepper1_3 = 0; stepper1_4 = 1;
stepper2_1 = 1; stepper2_2 = 0; stepper2_3 = 0; stepper2_4 = 0;
stepper3_1 = 1; stepper3_2 = 0; stepper3_3 = 0; stepper3_4 = 0;
Delay_ms(30);
stepper1_1 = 0; stepper1_2 = 0; stepper1_3 = 1; stepper1_4 = 0;
stepper2_1 = 0; stepper2_2 = 1; stepper2_3 = 0; stepper2_4 = 0;
stepper3_1 = 0; stepper3_2 = 1; stepper3_3 = 0; stepper3_4 = 0;
Delay_ms(30);
stepper1_1 = 0; stepper1_2 = 1; stepper1_3 = 0; stepper1_4 = 0;
stepper2_1 = 0; stepper2_2 = 0; stepper2_3 = 1; stepper2_4 = 0;
stepper3_1 = 0; stepper3_2 = 0; stepper3_3 = 1; stepper3_4 = 0;
Delay_ms(30);
stepper1_1 = 1; stepper1_2 = 0; stepper1_3 = 0; stepper1_4 = 0;
stepper2_1 = 0; stepper2_2 = 0; stepper2_3 = 0; stepper2_4 = 1;
stepper3_1 = 0; stepper3_2 = 0; stepper3_3 = 0; stepper3_4 = 1;
Delay_ms(30);
}
}

void Sequence_up(unsigned long step)

{int i;
for(i=0;i < step;i++)
{
stepper1_1 = 0; stepper1_2 = 0; stepper1_3 = 0; stepper1_4 = 1;
stepper2_1 = 0; stepper2_2 = 0; stepper2_3 = 0; stepper2_4 = 1;
stepper3_1 = 0; stepper3_2 = 0; stepper3_3 = 0; stepper3_4 = 1;
Delay_ms(30);
stepper1_1 = 0; stepper1_2 = 0; stepper1_3 = 1; stepper1_4 = 0;
stepper2_1 = 0; stepper2_2 = 0; stepper2_3 = 1; stepper2_4 = 0;
stepper3_1 = 0; stepper3_2 = 0; stepper3_3 = 1; stepper3_4 = 0;
Delay_ms(30);
stepper1_1 = 0; stepper1_2 = 1; stepper1_3 = 0; stepper1_4 = 0;
stepper2_1 = 0; stepper2_2 = 1; stepper2_3 = 0; stepper2_4 = 0;
stepper3_1 = 0; stepper3_2 = 1; stepper3_3 = 0; stepper3_4 = 0;
Delay_ms(30);
stepper1_1 = 1; stepper1_2 = 0; stepper1_3 = 0; stepper1_4 = 0;
stepper2_1 = 1; stepper2_2 = 0; stepper2_3 = 0; stepper2_4 = 0;
stepper3_1 = 1; stepper3_2 = 0; stepper3_3 = 0; stepper3_4 = 0;
Delay_ms(30);
}
}

void Sequence_down(unsigned long step)

{int i;
for(i=0;i < step;i++)
{
stepper1_1 = 1; stepper1_2 = 0; stepper1_3 = 0; stepper1_4 = 0;
stepper2_1 = 1; stepper2_2 = 0; stepper2_3 = 0; stepper2_4 = 0;
stepper3_1 = 1; stepper3_2 = 0; stepper3_3 = 0; stepper3_4 = 0;
Delay_ms(30);
stepper1_1 = 0; stepper1_2 = 1; stepper1_3 = 0; stepper1_4 = 0;
stepper2_1 = 0; stepper2_2 = 1; stepper2_3 = 0; stepper2_4 = 0;
stepper3_1 = 0; stepper3_2 = 1; stepper3_3 = 0; stepper3_4 = 0;
Delay_ms(30);
stepper1_1 = 0; stepper1_2 = 0; stepper1_3 = 1; stepper1_4 = 0;
stepper2_1 = 0; stepper2_2 = 0; stepper2_3 = 1; stepper2_4 = 0;
stepper3_1 = 0; stepper3_2 = 0; stepper3_3 = 1; stepper3_4 = 0;
Delay_ms(30);
stepper1_1 = 0; stepper1_2 = 0; stepper1_3 = 0; stepper1_4 = 1;
stepper2_1 = 0; stepper2_2 = 0; stepper2_3 = 0; stepper2_4 = 1;
stepper3_1 = 0; stepper3_2 = 0; stepper3_3 = 0; stepper3_4 = 1;
Delay_ms(30);
}
}

void Sequence_22degR_up(unsigned long step)

4
 D:\education\MY PSM\program files\test servo1.c

{int i;
for(i=0;i < step;i++)
{
stepper1_1 = 0; stepper1_2 = 0; stepper1_3 = 0; stepper1_4 = 1;
stepper2_1 = 0; stepper2_2 = 0; stepper2_3 = 0; stepper2_4 = 1;
stepper3_1 = 1; stepper3_2 = 0; stepper3_3 = 0; stepper3_4 = 0;
Delay_ms(30);
stepper1_1 = 0; stepper1_2 = 0; stepper1_3 = 1; stepper1_4 = 0;
stepper2_1 = 0; stepper2_2 = 0; stepper2_3 = 1; stepper2_4 = 0;
stepper3_1 = 0; stepper3_2 = 1; stepper3_3 = 0; stepper3_4 = 0;
Delay_ms(30);
stepper1_1 = 0; stepper1_2 = 1; stepper1_3 = 0; stepper1_4 = 0;
stepper2_1 = 0; stepper2_2 = 1; stepper2_3 = 0; stepper2_4 = 0;
stepper3_1 = 0; stepper3_2 = 0; stepper3_3 = 1; stepper3_4 = 0;
Delay_ms(30);
stepper1_1 = 1; stepper1_2 = 0; stepper1_3 = 0; stepper1_4 = 0;
stepper2_1 = 1; stepper2_2 = 0; stepper2_3 = 0; stepper2_4 = 0;
stepper3_1 = 0; stepper3_2 = 0; stepper3_3 = 0; stepper3_4 = 1;
Delay_ms(30);
}
}

void Sequence_22degR_down(unsigned long step)

{int i;
for(i=0;i < step;i++)
{
stepper1_1 = 1; stepper1_2 = 0; stepper1_3 = 0; stepper1_4 = 0;
stepper2_1 = 1; stepper2_2 = 0; stepper2_3 = 0; stepper2_4 = 0;
stepper3_1 = 0; stepper3_2 = 0; stepper3_3 = 0; stepper3_4 = 1;
Delay_ms(30);
stepper1_1 = 0; stepper1_2 = 1; stepper1_3 = 0; stepper1_4 = 0;
stepper2_1 = 0; stepper2_2 = 1; stepper2_3 = 0; stepper2_4 = 0;
stepper3_1 = 0; stepper3_2 = 0; stepper3_3 = 1; stepper3_4 = 0;
Delay_ms(30);
stepper1_1 = 0; stepper1_2 = 0; stepper1_3 = 1; stepper1_4 = 0;
stepper2_1 = 0; stepper2_2 = 0; stepper2_3 = 1; stepper2_4 = 0;
stepper3_1 = 0; stepper3_2 = 1; stepper3_3 = 0; stepper3_4 = 0;
Delay_ms(30);
stepper1_1 = 0; stepper1_2 = 0; stepper1_3 = 0; stepper1_4 = 1;
stepper2_1 = 0; stepper2_2 = 0; stepper2_3 = 0; stepper2_4 = 1;
stepper3_1 = 1; stepper3_2 = 0; stepper3_3 = 0; stepper3_4 = 0;
Delay_ms(30);
}
}

//========================= Function of DC motor ==========================//

void pen_up_down(int x)
{
if(x==0)
{ DC_motor_a=0;
DC_motor_b=1; }

if(x==1)
{ DC_motor_a=1;
DC_motor_b=0; }
}

//========================= Function of steering servo motor ==========================//

void Steer_horizon(void)
{
if(servo1_pulse < 157)
{ while(servo1_pulse < 157)
{ servo1_pulse++;
Delay_ms(15); }}

if(servo1_pulse > 157)

{ while(servo1_pulse > 157)
{ servo1_pulse--;
Delay_ms(15); }}

if(servo2_pulse < 43)

{ while(servo2_pulse < 43)
{ servo2_pulse++;
Delay_ms(15);}}

5
 D:\education\MY PSM\program files\test servo1.c

if(servo2_pulse > 43)

{ while(servo2_pulse > 43)
{ servo2_pulse--;
Delay_ms(15);}}

if(servo3_pulse > 131)

{ while(servo3_pulse > 131)
{ servo3_pulse--;
Delay_ms(15);}}

else if(servo3_pulse < 131)

{ while(servo3_pulse < 131)
{servo3_pulse++;
Delay_ms(15);}}

void Steer_vertical(void)
{
if(servo1_pulse < 14)
{ while(servo1_pulse < 14)
{ servo1_pulse++;
Delay_ms(15); }}

if(servo1_pulse > 14)

{ while(servo1_pulse > 14)
{ servo1_pulse--;
Delay_ms(15); }}

if(servo2_pulse < 203)

{ while(servo2_pulse < 203)
{ servo2_pulse++;
Delay_ms(15);}}

if(servo2_pulse > 203)

{ while(servo2_pulse > 203)
{ servo2_pulse--;
Delay_ms(15);}}

if(servo3_pulse > 280)

{ while(servo3_pulse > 280)
{ servo3_pulse--;
Delay_ms(15);}}

else if(servo3_pulse < 280)

{ while(servo3_pulse < 280)
{servo3_pulse++;
Delay_ms(15);}}
}

void Steer_22degR(void)
{
if(servo1_pulse < 50)
{ while(servo1_pulse < 50)
{ servo1_pulse++;
Delay_ms(15); }}

if(servo1_pulse > 50)

{ while(servo1_pulse > 50)
{ servo1_pulse--;
Delay_ms(15); }}

if(servo2_pulse < 248)

{ while(servo2_pulse < 248)
{ servo2_pulse++;
Delay_ms(15);}}

if(servo2_pulse > 248)

{ while(servo2_pulse > 248)
{ servo2_pulse--;
Delay_ms(15);}}

if(servo3_pulse > 11)

{ while(servo3_pulse > 11)
{ servo3_pulse--;

6
 D:\education\MY PSM\program files\test servo1.c

Delay_ms(15);}}

else if(servo3_pulse < 11)

{ while(servo3_pulse < 11)
{servo3_pulse++;
Delay_ms(15);}}
}

void Steer_22degL(void)
{
if(servo1_pulse < 263)
{ while(servo1_pulse < 263)
{ servo1_pulse++;
Delay_ms(15); }}

if(servo1_pulse > 263)

{ while(servo1_pulse > 263)
{ servo1_pulse--;
Delay_ms(15); }}

if(servo2_pulse < 167)

{ while(servo2_pulse < 167)
{ servo2_pulse++;
Delay_ms(15);}}

if(servo2_pulse > 167)

{ while(servo2_pulse > 167)
{ servo2_pulse--;
Delay_ms(15);}}

if(servo3_pulse > 237)

{ while(servo3_pulse > 237)
{ servo3_pulse--;
Delay_ms(15);}}

else if(servo3_pulse < 237)

{ while(servo3_pulse < 237)
{servo3_pulse++;
Delay_ms(15);}}
}

//=============================== Letter function ================================//

void Letter_A(void)
{
Steer_vertical();
Sequence_down(22);
Delay_ms(300);
Steer_22degR();
pen_up_down(1); // 1 = pen down , 0 = pen up
Delay_ms(300);
Sequence_22degR_up(24);
Steer_22degL();
Delay_ms(300);
Sequence_right(25);
pen_up_down(0);
Delay_ms(300);
Sequence_left(11);
pen_up_down(1);
Delay_ms(100);
Steer_horizon();
Delay_ms(100);
Sequence_left(9);
Delay_ms(100);
pen_up_down(0);
Steer_vertical();
Sequence_up(11);
Steer_horizon();
Sequence_right(18);
}

void Letter_E(void)
{
Steer_horizon();
Sequence_right(13);
pen_up_down(1);

7
 D:\education\MY PSM\program files\test servo1.c

Delay_ms(300);
Sequence_left(13);
Steer_vertical();
Sequence_down(22);
Steer_horizon();
Sequence_right(13);
pen_up_down(0);
Steer_vertical();
Sequence_up(11);
Steer_horizon();
pen_up_down(1);
Delay_ms(300);
Sequence_left(13);
pen_up_down(0);
Steer_vertical();
Sequence_up(11);
Steer_horizon();
Sequence_right(19);
}

void Letter_F(void)
{
Steer_horizon();
Sequence_right(13);
pen_up_down(1);
Delay_ms(300);
Sequence_left(13);
Steer_vertical();
Sequence_down(22);
pen_up_down(0);
Steer_vertical();
Sequence_up(11);
Steer_horizon();
pen_up_down(1);
Delay_ms(300);
Sequence_right(10);
Delay_ms(300);
pen_up_down(0);
Steer_vertical();
Sequence_up(11);
Steer_horizon();
Sequence_right(8);
}

void Letter_H(void)
{
Steer_vertical();
pen_up_down(1);
Delay_ms(300);
Sequence_down(22);
Delay_ms(300);
pen_up_down(0);
Sequence_up(11);
Steer_horizon();
pen_up_down(1);
Delay_ms(300);
Sequence_right(10);
Steer_vertical();
pen_up_down(0);
Delay_ms(300);
Sequence_down(11);
Delay_ms(100);
pen_up_down(1);
Delay_ms(300);
Sequence_up(22);
Delay_ms(300);
pen_up_down(0);
Steer_horizon();
Sequence_right(5);
}

void Letter_I(void)
{
Steer_horizon();
pen_up_down(1);
Delay_ms(300);

8
 D:\education\MY PSM\program files\test servo1.c

Sequence_right(12);
Delay_ms(300);
pen_up_down(0);
Delay_ms(100);
Sequence_left(6);
pen_up_down(1);
Steer_vertical();
Sequence_down(22);
pen_up_down(0);
Steer_horizon();
Sequence_left(6);
pen_up_down(1);
Delay_ms(100);
Sequence_right(12);
pen_up_down(0);
Steer_vertical();
Sequence_up(22);
Steer_horizon();
Sequence_right(5);
}

void Letter_L(void)
{
Steer_vertical();
pen_up_down(1);
Delay_ms(300);
Sequence_down(22);
Steer_horizon();
Sequence_right(12);
pen_up_down(0);
Steer_vertical();
Sequence_up(22);
Steer_horizon();
Sequence_right(5);
}