
/*
 * helloworld.c: simple test application
 *
 *
 * PS7 UART (Zynq) is not initialized by this application, since
 * bootrom/bsp configures it to baud rate 115200
 *
 * ------------------------------------------------
 * | UART TYPE   BAUD RATE                        |
 * ------------------------------------------------
 *   uartns550   9600
 *   uartlite    Configurable only in HW design
 *   ps7_uart    115200 (configured by bootrom/bsp)
 */

#include <stdio.h>

/*Xilinx includes.*/
#include "platform.h"
#include "xil_printf.h"
#include "xil_io.h"
#include "xparameters.h"
#include "xtmrctr.h" // Xilinx Timer Control Library
#include "xuartps.h" // Xilina Uart PS library


#define PWM_X_BASE  XPAR_AXI_TIMER_0_BASEADDR // Base Address
#define PWM_Z_BASE  XPAR_AXI_TIMER_1_BASEADDR // Base Address

#define CLK_PERIOD 1e-8   // AXI Clock Period : (1 / 100Mhz)
#define SERVO_FREQ 20e-3  // Servo PWM frequency 20ms



float X_Deg;
float Z_Deg;

float Duty_val( float Degree);      //Convert the degree value into Duty Cycle for PWM register

void PWM_Freq_Duty(u32 PWM_ADDRESS, float CLOCK_PERIOD, float Freq, float Duty); //This works
void PWM_START( u32 PWM_ADDRESS);  // This works
void PWM_STOP(u32 PWM_ADDRESS); //This works



int main()
{
	init_platform ();


	/* Set the period and duty cycle of the PWM signal.
	* The first call sets TLR0 to have a period of 20ms.
	* The second call sets TLR0 to have a duty cycle of 50 % ( 10ms period ).
	* The formula to calculate the value to use is
	* ( period / clk_period ) - 2
	* The values are in nanoseconds
	* The clk_period is the period of the AXI Timer aclk input,
	* which is 100MHz ( 10ns period ) ( the default from FCLK ).
	*/

//	 Xil_Out32 ( PWM_X_BASE + 0x04 , ( 20e-3 / 1e-8 ) - 2 );   // 20ms
//	 Xil_Out32 ( PWM_X_BASE + 0x14 , (10e-3 / 1e-8)   - 2 );   // Duty 50%
//	 Xil_Out32 ( PWM_X_BASE + 0x14 , ( 0.5e-3 / 1e-8 ) - 2 );  // Duty = 0.5ms/20ms, the right side
//   Xil_Out32 ( PWM_X_BASE + 0x14 , ( 2.5e-3 / 1e-8 ) - 2);   // Duty = 2.5ms/20ms, the left side

	  // Xil_Out32 ( PWM_X_BASE + 0x14 , ( 2.5*10e5 / 10 ) - 2 );

	/* Write our control values to the TCSRx registers.
	* In both , the PWMAx bit is set to enable PWM and
	* UDTx is set to put the timer in down count mode.
	* The GENTx bit is also set as required for PWM mode.
	* In the second write , we write also enable both timers
	*/
//	   Xil_Out32 ( PWM_X_BASE , 0x206 );
//	   Xil_Out32 ( PWM_X_BASE + 0x10 , 0x606 );


	/****************************/
    X_Deg = 90.0;
    Z_Deg = 90.0;
/*
	//PWM LoadRegister Value Set
//	XTmrCtr_SetLoadReg(PWM_X_BASE,0,(20e-3 / CLK_PERIOD) - 2); //PWM Frequency
//  XTmrCtr_SetLoadReg(PWM_X_BASE,1,(Duty_val(X_Deg) / CLK_PERIOD) - 2); //PWM Duty

    //PWM_START
//	XTmrCtr_SetControlStatusReg(PWM_X_BASE,0,XTC_CSR_ENABLE_ALL_MASK| XTC_CSR_ENABLE_PWM_MASK | XTC_CSR_EXT_GENERATE_MASK | XTC_CSR_DOWN_COUNT_MASK);
// 	XTmrCtr_SetControlStatusReg(PWM_X_BASE,1,XTC_CSR_ENABLE_ALL_MASK| XTC_CSR_ENABLE_PWM_MASK | XTC_CSR_EXT_GENERATE_MASK| XTC_CSR_DOWN_COUNT_MASK);


 	//PWM LoadRegister Value Set
// 	XTmrCtr_SetLoadReg(PWM_Z_BASE,0,(20e-3 / CLK_PERIOD) - 2); //PWM Frequency
// 	XTmrCtr_SetLoadReg(PWM_Z_BASE,1,(Duty_val(Z_Deg) / CLK_PERIOD) - 2); //PWM Duty

 	//PWM_START
//    XTmrCtr_SetControlStatusReg(PWM_Z_BASE,0,XTC_CSR_ENABLE_ALL_MASK| XTC_CSR_ENABLE_PWM_MASK | XTC_CSR_EXT_GENERATE_MASK | XTC_CSR_DOWN_COUNT_MASK);
// 	XTmrCtr_SetControlStatusReg(PWM_Z_BASE,1,XTC_CSR_ENABLE_ALL_MASK| XTC_CSR_ENABLE_PWM_MASK | XTC_CSR_EXT_GENERATE_MASK| XTC_CSR_DOWN_COUNT_MASK);
*/


	PWM_Freq_Duty(PWM_X_BASE,CLK_PERIOD,SERVO_FREQ,  Duty_val(X_Deg));
	PWM_START(PWM_X_BASE);


	PWM_Freq_Duty(PWM_Z_BASE,CLK_PERIOD,SERVO_FREQ, Duty_val(Z_Deg));
	PWM_START(PWM_Z_BASE);

	printf ( "Initialization done...\n" );



    while(1){
    	/* Do Nothing */
    	for ( float X = 0; X <= 120 ; X++) {
    		PWM_Freq_Duty(PWM_X_BASE,CLK_PERIOD,SERVO_FREQ,  Duty_val(X));
    	    PWM_START(PWM_X_BASE);

    	    PWM_Freq_Duty(PWM_Z_BASE,CLK_PERIOD,SERVO_FREQ, Duty_val(X));
    	    PWM_START(PWM_Z_BASE);




    	}
    }

    cleanup_platform();
    return 0;
}



    float Duty_val( float Degree)
 	{

 		float Duty_val = ( (Degree / 90.0) + 0.5 ) /1000 ;
 		return Duty_val;

 	}


    void PWM_Freq_Duty( u32 PWM_ADDRESS, float CLOCK_PERIOD, float Freq, float Duty)
    {
    	XTmrCtr_SetLoadReg(PWM_ADDRESS,0,(Freq / CLOCK_PERIOD) - 2); //PWM Frequency
        XTmrCtr_SetLoadReg(PWM_ADDRESS,1,(Duty / CLOCK_PERIOD) - 2); //PWM Duty
    }


    void PWM_START( u32 PWM_ADDRESS)
    {
    	XTmrCtr_SetControlStatusReg(PWM_ADDRESS,0,XTC_CSR_ENABLE_ALL_MASK| XTC_CSR_ENABLE_PWM_MASK | XTC_CSR_EXT_GENERATE_MASK | XTC_CSR_DOWN_COUNT_MASK);
    	XTmrCtr_SetControlStatusReg(PWM_ADDRESS,1,XTC_CSR_ENABLE_ALL_MASK| XTC_CSR_ENABLE_PWM_MASK | XTC_CSR_EXT_GENERATE_MASK | XTC_CSR_DOWN_COUNT_MASK);

    }

    void PWM_STOP(u32 PWM_ADDRESS)
    {
    	XTmrCtr_SetControlStatusReg(PWM_ADDRESS,0,0);
    	XTmrCtr_SetControlStatusReg(PWM_ADDRESS,1,0);
    }



