/* Scheduler includes. */  
#include "FreeRTOS.h"  
#include "task.h"  
  
/* Demo includes. */  
#include "partest.h"  
  
/* Xilinx includes. */  
#include "xgpiops.h"  
#include "xgpio.h"  
#include "xuartps.h"  
  
#include <stdlib.h>  
  
#define partstNUM_LEDS          ( 1 )  
#define partstDIRECTION_INPUT   ( 1 )  
#define partstDIRECTION_OUTPUT  ( 0 )  
#define partstOUTPUT_ENABLED    ( 1 )  
#define partst_RX               ( 9 )//MIO 9  
#define partst_TX               ( 10 )//MIO 10  
#define partstLED_OUTPUT        ( 1 )  
  
/*-----------------------------------------------------------*/  
  
static XGpioPs xGpio;  
  
static XUartPs xUart;  
unsigned int addr;  
unsigned int num = 1;  
  
  
static XGpio xServo;  
BaseType_t xLEDState;  
static double count;  
double buff;  
int change = 0;  
  
/*-----------------------------------------------------------*/  
  
void vParTestInitialise( void )  
{  
  
BaseType_t xxxStatus;  
XGpioPs_Config *pxConfigPtr;  
BaseType_t xStatus;  
BaseType_t xxStatus;  
XUartPs_Config *uart;  
  
    /* Initialise the GPIO driver. */  
  
    xxxStatus = XGpio_Initialize(&xServo, XPAR_AXI_GPIO_0_DEVICE_ID);  
    configASSERT( xxxStatus == XST_SUCCESS );  
    ( void ) xxxStatus; /* Remove compiler warning if configASSERT() is not defined. */  
  
    /* Enable outputs and set low. */  
    XGpio_SetDataDirection(&xServo, partstLED_OUTPUT, partstDIRECTION_OUTPUT);  
    XGpio_DiscreteWrite(&xServo, partstLED_OUTPUT, 0);  
  
    /* Initialise the GPIO driver. */  
    pxConfigPtr = XGpioPs_LookupConfig( XPAR_XGPIOPS_0_DEVICE_ID );  
    xStatus = XGpioPs_CfgInitialize( &xGpio, pxConfigPtr, pxConfigPtr->BaseAddr );  
    configASSERT( xStatus == XST_SUCCESS );  
    ( void ) xStatus; /* Remove compiler warning if configASSERT() is not defined. */  
  
//  /* Enable outputs and set low. */  
    XGpioPs_SetDirectionPin( &xGpio, partst_RX, partstDIRECTION_INPUT );  
    XGpioPs_SetDirectionPin( &xGpio, partst_TX, partstDIRECTION_OUTPUT );  
    XGpioPs_SetOutputEnablePin( &xGpio, partst_TX, partstOUTPUT_ENABLED );  
    XGpioPs_WritePin( &xGpio, partstLED_OUTPUT, 0x0 );  
  
    uart = XUartPs_LookupConfig(XPAR_PS7_UART_0_DEVICE_ID);  
    xxStatus = XUartPs_CfgInitialize(&xUart, uart, uart->BaseAddress);  
    configASSERT( xxStatus == XST_SUCCESS );  
    ( void ) xxStatus;  
  
    XUartPs_SetBaudRate(&xUart, 57600);  
  
    //Read from UART and setup Servo control Index
    XUartPs_Recv(&xUart, &buff, sizeof(double));  
    count = 1.5;  
}  
/*-----------------------------------------------------------*/  
  
void vParTestSetLED( UBaseType_t uxLED, BaseType_t xValue )  
{  
    ( void ) uxLED;  
    XGpio_DiscreteWrite(&xServo, partstLED_OUTPUT, xValue );  
    XGpioPs_WritePin( &xGpio, partstLED_OUTPUT, xValue );  
}  
/*-----------------------------------------------------------*/  
  
void vParTestToggleLED( unsigned portBASE_TYPE uxLED )  
{  
  
    ( void ) uxLED;  
    TickType_t xNextWakeTime;  
    xNextWakeTime = xTaskGetTickCount();  
  
    //Read from UART, contains data from BLE
    XUartPs_Recv(&xUart, &buff, sizeof(double));  
  
    //Increment the count index for Servo Control
    count = count + buff;  
  
    if (count > 2.25)  
        count = 2.25;  
  
    if (count < 1) {  
        count = 1;  
    }  
  
    //Output and write to Servo to move Camera
    XGpio_DiscreteWrite(&xServo, partstLED_OUTPUT, 1);  
    vTaskDelayUntil( &xNextWakeTime,  count / portTICK_PERIOD_MS  );  
    XGpio_DiscreteWrite(&xServo, partstLED_OUTPUT, 0);  
    vTaskDelayUntil( &xNextWakeTime,  20 / portTICK_PERIOD_MS  );  
  
  
  
  
}