#include "stc8h.h"       
#include "intrins.h"     

#define MAIN_Fosc       24000000L   

typedef unsigned char   u8;   
typedef unsigned int    u16;  
typedef unsigned long   u32;  

#define Timer0_Reload   (65536UL - (MAIN_Fosc / 1000))

#define PWM_PERIOD      7499    

#define SERVO_MIN_US    500     
#define SERVO_MAX_US    2500    
#define SERVO_MIN_ANGLE 0       
#define SERVO_MAX_ANGLE 180     

#define PWM1_1      0x00    
#define PWM1_2      0x01    
#define PWM1_3      0x02    

#define PWM2_1      0x00    
#define PWM2_2      0x04    
#define PWM2_3      0x08    

#define PWM3_1      0x00    
#define PWM3_2      0x10    
#define PWM3_3      0x20    

#define PWM4_1      0x00    
#define PWM4_2      0x40    
#define PWM4_3      0x80    
#define PWM4_4      0xC0    

#define ENO1P       0x01    
#define ENO1N       0x02    
#define ENO2P       0x04    
#define ENO2N       0x08    
#define ENO3P       0x10    
#define ENO3N       0x20    
#define ENO4P       0x40    
#define ENO4N       0x80    

bit B_1ms;          

u16 servo_angle[4] = {90, 90, 90, 90};  
u16 servo_target[4] = {0, 45, 90, 135}; 
u16 servo_step = 0;                     
u16 servo_speed = 10;                   

#define INITIAL_DUTY_90_DEGREES   ((u16)((1500UL * PWM_PERIOD) / 20000UL))
u16 PWM1_Duty = INITIAL_DUTY_90_DEGREES;  
u16 PWM2_Duty = INITIAL_DUTY_90_DEGREES;  
u16 PWM3_Duty = INITIAL_DUTY_90_DEGREES;  
u16 PWM4_Duty = INITIAL_DUTY_90_DEGREES;  

void UpdatePwm(void);                
void SetServoAngle(u8 servo_num, u16 angle);  
u16 AngleToDuty(u16 angle);          
void Delay2000ms(void);              

void Delay2000ms(void)
{
    unsigned char data i, j, k;
    
    i = 244;
    j = 130;
    k = 4;
    do
    {
        do
        {
            while (--k);
        } while (--j);
    } while (--i);
}

void main(void)
{
    P_SW2 |= 0x80;
    
    P1M1 = 0x00;   P1M0 = 0xFF;   
    P3M1 = 0x00;   P3M0 = 0xFF;   
    
    P0M1 = 0x00;   P0M0 = 0x00;   
    P2M1 = 0x00;   P2M0 = 0x00;   
    P4M1 = 0x00;   P4M0 = 0x00;   
    P5M1 = 0x00;   P5M0 = 0x00;   
    
    AUXR = 0x80;    
    TH0 = (u8)(Timer0_Reload / 256);    
    TL0 = (u8)(Timer0_Reload % 256);    
    ET0 = 1;        
    TR0 = 1;        
    
    PWMA_CCER1 = 0x00;  
    PWMA_CCER2 = 0x00;  
    
    PWMA_CCMR1 = 0x60;  
    PWMA_CCMR2 = 0x60;  
    PWMA_CCMR3 = 0x60;  
    PWMA_CCMR4 = 0x60;  
    
    PWMA_CCER1 = 0x55;  
    PWMA_CCER2 = 0x55;  
    
    PWMA_CCMR1 |= 0x08;  
    PWMA_CCMR2 |= 0x08;  
    PWMA_CCMR3 |= 0x08;  
    PWMA_CCMR4 |= 0x08;  
    
    PWMA_PSCRH = 0x00;   
    PWMA_PSCRL = 63;     
    
    PWMA_ARRH = (u8)(PWM_PERIOD >> 8);  
    PWMA_ARRL = (u8)PWM_PERIOD;         
    
    PWMA_ENO = 0x00;  
    PWMA_ENO |= ENO1P | ENO1N;  
    PWMA_ENO |= ENO2P | ENO2N;  
    PWMA_ENO |= ENO3P | ENO3N;  
    PWMA_ENO |= ENO4P | ENO4N;  
    
    PWMA_PS = 0x00;  
    
    PWMA_PS |= PWM1_1;  
    PWMA_PS |= PWM2_1;  
    PWMA_PS |= PWM3_1;  
    PWMA_PS |= PWM4_4;  
    
    PWMA_BKR = 0x80;    
    PWMA_CR1 |= 0x01;   
    
    EA = 1;     
    
    while (1)
    {
        SetServoAngle(0, 0);     
        Delay2000ms();           
        
        SetServoAngle(0, 180);   
        Delay2000ms();
        
        SetServoAngle(1, 0);     
        Delay2000ms();
        
        SetServoAngle(1, 180);   
        Delay2000ms();
        
        SetServoAngle(2, 0);     
        Delay2000ms();
        
        SetServoAngle(2, 180);   
        Delay2000ms();
        
        SetServoAngle(3, 0);     
        Delay2000ms();
        
        SetServoAngle(3, 180);   
        Delay2000ms();
        
    }
}

void timer0(void) interrupt 1
{
    u8 i;
    static u16 time_counter = 0;  
    
    time_counter++;  
    
    if(time_counter >= servo_speed)
    {
        time_counter = 0;  
        
        for(i = 0; i < 4; i++)
        {
            if(servo_angle[i] < servo_target[i])
            {
                servo_angle[i]++;  
                if(servo_angle[i] > SERVO_MAX_ANGLE) 
                    servo_angle[i] = SERVO_MAX_ANGLE;
            }
            else if(servo_angle[i] > servo_target[i])
            {
                servo_angle[i]--;  
                if(servo_angle[i] < SERVO_MIN_ANGLE) 
                    servo_angle[i] = SERVO_MIN_ANGLE;
            }
        }
        
    PWM1_Duty = AngleToDuty(servo_angle[0]);
    PWM2_Duty = AngleToDuty(servo_angle[1]);
    PWM3_Duty = AngleToDuty(servo_angle[2]);
    PWM4_Duty = AngleToDuty(servo_angle[3]);
       
       
        
        UpdatePwm();
    }
}

void UpdatePwm(void)
{
    PWMA_CCR1H = (u8)(PWM1_Duty >> 8);
    PWMA_CCR1L = (u8)PWM1_Duty;
    
    PWMA_CCR2H = (u8)(PWM2_Duty >> 8);
    PWMA_CCR2L = (u8)PWM2_Duty;
    
    PWMA_CCR3H = (u8)(PWM3_Duty >> 8);
    PWMA_CCR3L = (u8)PWM3_Duty;
    
    PWMA_CCR4H = (u8)(PWM4_Duty >> 8);
    PWMA_CCR4L = (u8)PWM4_Duty;
}

void SetServoAngle(u8 servo_num, u16 angle)
{
    if(servo_num < 4)
    {
        if(angle < SERVO_MIN_ANGLE) 
            angle = SERVO_MIN_ANGLE;
        if(angle > SERVO_MAX_ANGLE) 
            angle = SERVO_MAX_ANGLE;
        
        servo_target[servo_num] = angle;
    }
}

u16 AngleToDuty(u16 angle)
{
    u32 pulse_width;
    u32 duty_value;
    
    pulse_width = SERVO_MIN_US + ((u32)angle * (SERVO_MAX_US - SERVO_MIN_US) / SERVO_MAX_ANGLE);
    
    duty_value = (pulse_width * PWM_PERIOD) / 20000UL;
    
    if(duty_value > PWM_PERIOD) 
        duty_value = PWM_PERIOD;
    if(duty_value < 0) 
        duty_value = 0;
    
    return (u16)duty_value;
}