#include <stdio.

h>
#include <stdbool.h>
#include "sleep.h"
#include "config.h"
#include "timer_f.h"
#include "platform.h"
#include "SH1106_Screen.h"
#include "xgpio.h"
#include "xadcps.h"
#include "xiicps.h"
#include "xtime_l.h"
#include "xtmrctr.h"
#include "xscugic.h"
#include "xscutimer.h"
#include "xil_printf.h"
#include "xparameters.h"
#include "xil_exception.h"

// Polling
#define Time_Divisor 4 // will be used to divide
the number of counts per second
float time_record;

// ADC
static XAdcPs XAdcInst; // XADC driver instance
#define XADC_DEVICE_ID XPAR_XADCPS_0_DEVICE_ID // XADC definitions
const float CHANNEL[4] = {1, 9, 6, 15}; // XADC channels
float averagedIntensity[4]; // (top, bottom, left, right)

// Normalisation factors for LDR readings

float INTENSITY_COMP[4] = {1, 1, 1, 1};

// PWM Configuration
XTmrCtr TimerCounterInst0; // Xtimer for X motor
XTmrCtr TimerCounterInst1; // Xtimer for Y motor
#define DEADZONE 500 // To prevent motor from constantly moving
#define PWM_PERIOD 5000000 // 5 ms period
#define MIN_IDEAL_EFFICIENCY 97.0f // Threshold efficiency to prevent constant
movement
u32 HIGHTIME0 = 0; // Duty cycle for X-axis motor
u32 HIGHTIME1 = 0; // Duty cycle for Y-axis motor

// GPIO for motor turning with H-bridge

XGpio Gpio; //Gpio instance
// pin 4 , pin 6 , pin 8 , pin 10
int in1 = 0, in2 = 0, in3 = 0, in4 = 0; // Initialise all pins as LOW
#define GPIO_DEVICE_ID XPAR_GPIO_1_DEVICE_ID //non interrupt Arduino
pins
#define GPIO_CHANNEL 1

//LED
XGpio leds; // LED instance
#define LEDSDEVICE_ID XPAR_USER_LEDS_GPIO_DEVICE_ID //user controlled LEDs
#define LED_CHANNEL 1 // GPIO channel for LED

// Buttons
XGpio buttons;
#define BUTTON_DEVIE_ID XPAR_USER_BTNS_GPIO_DEVICE_ID
#define BUTTON_CHANNEL 1

float intensityData[4]; // Array for XADC values (A0, A1, A2, A3)
char buffer1[20], buffer2[20], buffer3[20], buffer4[20];

int PERCENT = 40; // Duty cycle as a percentage

float efficiency_x, efficiency_y; // Efficiencies for X and Y directions

// Function Prototypes
int configXadc(u16 xadc_id);
float XAdcPs_RawToVoltage_own(u16 adcData);
float calculateEfficiency(float val1, float val2);

void printGPIOoutputs();
void axisAverage(float intensity[]);
void recalibrateComp(int number_of_channels);
void SetPinOutputs(int pin0, int pin1, int pin2, int pin3);
void controlMovement(float intensityData[], char *buffer3, char *buffer4);

int main() {
init_platform();
initDisplay();

// Initialise PWM Timers

if (PwmInit(&TimerCounterInst0, TMR0_DEVICE_ID) != XST_SUCCESS ||
PwmInit(&TimerCounterInst1, TMR1_DEVICE_ID) != XST_SUCCESS) {
xil_printf("PWM Initialisation Failed.\n\r");
return XST_FAILURE;
}

xil_printf("PWM Timers Initialised Successfully.\n\r");

PwmConfig(&TimerCounterInst0, PWM_PERIOD, 0); // Start with 0% duty cycle
PwmConfig(&TimerCounterInst1, PWM_PERIOD, 0); // Start with 0% duty cycle

// Initialise GPIO driver

int Status = XGpio_Initialize(&Gpio, GPIO_DEVICE_ID);
if (Status != XST_SUCCESS) {
xil_printf("GPIO Configuration Failed.\n\r");
return XST_FAILURE;
}

xil_printf("XGPIO Initialised Successfully.\n\r");

XGpio_SetDataDirection(&Gpio, GPIO_CHANNEL, 0x0); // 0x0 = all pins as output

// Initialise and configure LEDs

Status = XGpio_Initialize(&leds, LEDSDEVICE_ID);
if (Status != XST_SUCCESS) {
xil_printf("LED Configuration Failed.\n\r");
return XST_FAILURE;
}

XGpio_SetDataDirection(&leds, LED_CHANNEL, 0x00);

 // Initialise and configure buttons
Status = XGpio_Initialize(&buttons, BUTTON_DEVIE_ID);
if (Status != XST_SUCCESS) {
xil_printf("Button Configuration Failed.\n\r");
return XST_FAILURE;
}
XGpio_SetDataDirection(&buttons, BUTTON_CHANNEL, 0x0F); //all buttons are
inputs
char defaultButton = XGpio_DiscreteRead(&buttons, BUTTON_CHANNEL);

// Configure XADC
if (configXadc(XADC_DEVICE_ID) != XST_SUCCESS) {
xil_printf("XADC Configuration Failed.\n\r");
return XST_FAILURE;
}else{xil_printf("XADC Initialised Successfully.\n\r");}

// Initialising polling
XTime current_time;
XTime last_time = 0;
char PrevButtonState;

int count = 0;

while (1) {

XTime_GetTime(&current_time);

// 325,000,000 / Time_Divisor
if ( (current_time - last_time) > (COUNTS_PER_SECOND / Time_Divisor) ){

//Polling reset
last_time = current_time;

if (count == 1) {xil_printf("\r program has polled %d time\r\n",

count++);}
else {xil_printf("\r program has polled %d times\r\n",
count++);}

// Re-calibrate intensity compensator

char ButtonState = XGpio_DiscreteRead(&buttons, BUTTON_CHANNEL);

if (ButtonState != defaultButton && ButtonState != PrevButtonState){

recalibrateComp(4);
}

PrevButtonState = ButtonState;

// Read ADC data and normalise

intensityData[0] = XAdcPs_GetAdcData(&XAdcInst, XADCPS_CH_AUX_MIN + 1)
* INTENSITY_COMP[0];
 intensityData[1] = XAdcPs_GetAdcData(&XAdcInst, XADCPS_CH_AUX_MIN + 9)
* INTENSITY_COMP[1];
intensityData[2] = XAdcPs_GetAdcData(&XAdcInst, XADCPS_CH_AUX_MIN + 6)
* INTENSITY_COMP[2];
intensityData[3] = XAdcPs_GetAdcData(&XAdcInst, XADCPS_CH_AUX_MIN + 15)
* INTENSITY_COMP[3];

// Display ADC samples

xil_printf("\n\r ----- XADC POLLING -----\n");

printf(" TL--------------TR \n");

printf(" | |\n");
printf(" BL--------------BR \n");

printf(" TL (A1) = %.1f | TR (A0) = %.1f\n", intensityData[1] ,

intensityData[0]);

printf(" BL (A2) = %.1f | TR (A3) = %.1f\n", intensityData[2],

intensityData[3]);

// averaging the LDR pairs

axisAverage(intensityData);

// 0---T[0]---0
// | |
// L[2] R[3]
// | |
// 0---B[1]---0

// Calculate efficiencies not with averaged intensities

efficiency_y = calculateEfficiency(intensityData[1], intensityData[2]);
efficiency_x = calculateEfficiency(intensityData[0], intensityData[1]);
//efficiency_y = calculateEfficiency(averagedIntensity[0],
averagedIntensity[1]);
//efficiency_x = calculateEfficiency(averagedIntensity[2],
averagedIntensity[3]);

// Control motor movement

controlMovement(intensityData, buffer3, buffer4);
//controlMovement(averagedIntensity, buffer3, buffer4);

// Update display
sprintf(buffer1, " X-Axis = %.1f%% ", efficiency_x);
sprintf(buffer2, " Y-Axis = %.1f%% ", efficiency_y);
printCentreX(1, " Efficiency: ");
printCentreX(17, buffer1);
printCentreX(29, buffer2);
printDisplay(2, 46, buffer3);
printDisplay(98, 46, buffer4);

}
}
cleanup_platform();
return 0;
}

// Configure XADC
int configXadc(u16 xadc_id) {
XAdcPs_Config *ConfigPtr;
ConfigPtr = XAdcPs_LookupConfig(xadc_id);
if (!ConfigPtr) return XST_FAILURE;

if (XAdcPs_CfgInitialize(&XAdcInst, ConfigPtr, ConfigPtr->BaseAddress) !=

XST_SUCCESS ||
XAdcPs_SelfTest(&XAdcInst) != XST_SUCCESS) {
return XST_FAILURE;
}

XAdcPs_SetSequencerMode(&XAdcInst, XADCPS_SEQ_MODE_CONTINPASS); // uses

continuous mode
xil_printf("XADC Configured Successfully.\n\r");
return XST_SUCCESS;
}

// Calculate efficiency
float calculateEfficiency(float val1, float val2) {
//always get a percentage < 100
if (val1 > val2){return ((val2 * 100) / val1);}
else {return ((val1 * 100) / val2);}
}

// Control motor movement

void controlMovement(float intensityData[], char *buffer3, char *buffer4) {

// X-axis
if ((intensityData[1] - DEADZONE) > intensityData[0] && efficiency_x <
MIN_IDEAL_EFFICIENCY) {
sprintf(buffer3, "Rotating: LEFT ");
HIGHTIME0 = (u32)((PWM_PERIOD * PERCENT) / (efficiency_x));

in1 = 1; // Set IN1 HIGH

in2 = 0; // Set IN2 LOW

} else if ((intensityData[0] - DEADZONE) > intensityData[1] && efficiency_x <

MIN_IDEAL_EFFICIENCY) {
sprintf(buffer3, "Rotating: RIGHT");
HIGHTIME0 = (u32)((PWM_PERIOD * PERCENT * 3) / (efficiency_x * 7));

in1 = 0; // Set IN1 LOW

in2 = 1; // Set IN2 HIGH

} else {
sprintf(buffer3, "Rotating: _____");
HIGHTIME0 = 0; // 0% duty cycle

in1 = 0; // Set IN1 LOW

in2 = 0; // Set IN2 LOW
}

PwmConfig(&TimerCounterInst0, PWM_PERIOD, HIGHTIME0); // Update PWM for X motor

// Y-axis
if ((intensityData[1] - DEADZONE) > intensityData[2] && efficiency_y <
MIN_IDEAL_EFFICIENCY) {
sprintf(buffer4, "UP ");
HIGHTIME1 = (u32)((PWM_PERIOD * PERCENT) / (efficiency_y * 2)); // Update
PWM for Y motor

in3 = 1; // Set IN3 HIGH

in4 = 0; // Set IN4 LOW

} else if ((intensityData[2] - DEADZONE) > intensityData[1] && efficiency_y <

MIN_IDEAL_EFFICIENCY) {
sprintf(buffer4, "DOWN");
HIGHTIME1 = (u32)((PWM_PERIOD * PERCENT) / (efficiency_y * 2));

in3 = 0; // Set IN3 LOW

in4 = 1; // Set IN4 HIGH

} else {
sprintf(buffer4, "____");
HIGHTIME1 = 0; // 0% duty cycle

in3 = 0; // Set IN3 LOW

in4 = 0; // Set IN4 LOW
}

PwmConfig(&TimerCounterInst1, PWM_PERIOD, HIGHTIME1); // Update PWM for Y motor

// setting the H-bridge directional pins

SetPinOutputs(in1, in2, in3, in4);

xil_printf("\n ----- MOTOR DATA -----\n");

xil_printf(" PWM1 (ENA) = %u | PWM2 (ENB) = %u | out of 5000000 \n",

HIGHTIME0, HIGHTIME1);

printGPIOoutputs();
}

void SetPinOutputs(int pin0, int pin1, int pin2, int pin3) {

u8 outputValue = 0;

// Set bits based on the corresponding integers

if (pin0) outputValue |= (1 << 0); // Pin 4 (bit 0)
if (pin1) outputValue |= (1 << 1); // Pin 6 (bit 1)
if (pin2) outputValue |= (1 << 3); // Pin 8 (bit 3)
if (pin3) outputValue |= (1 << 4); // Pin 10 (bit 10)

// Write to GPIO outputs

XGpio_DiscreteWrite(&Gpio, GPIO_CHANNEL, outputValue);
}

void printGPIOoutputs(){

//reads from pins to make sure pins as actually sending data

u8 gpio_output_value = XGpio_DiscreteRead(&Gpio, GPIO_CHANNEL);

u8 outputLEDValue = 0; //used to assign LEDS

 // Extract individual pin states by bit-masking
int pin0_state = (gpio_output_value >> 0) & 0x1; // Pin 4 (bit 0)
int pin1_state = (gpio_output_value >> 1) & 0x1; // Pin 6 (bit 1)
int pin2_state = (gpio_output_value >> 3) & 0x1; // Pin 8 (bit 3)
int pin3_state = (gpio_output_value >> 4) & 0x1; // Pin 10 (bit 4)

// Indicator LED code

if (pin0_state) outputLEDValue |= (1 << 3); // LED 1 (bit 0)
if (pin1_state) outputLEDValue |= (1 << 2); // LED 2 (bit 1)
if (pin2_state) outputLEDValue |= (1 << 1); // LED 3 (bit 2)
if (pin3_state) outputLEDValue |= (1 << 0); // LED 4 (bit 3)

XGpio_DiscreteWrite(&leds, LED_CHANNEL, outputLEDValue);

// Print the pin states in the same format as the PWM output
xil_printf(" Pin4 (LEFT) (IN1) = %d | Pin6 (RIGHT) (IN2) = %d | Pin8 (UP)
(IN3) = %d | Pin10 (DOWN) (IN4) = %d \n",
pin0_state, pin1_state, pin2_state, pin3_state);
}

//compensator re-calibration
void recalibrateComp(int number_of_channels){
float highestReading = 0.0f;
float currentReading;
float dataArray[4];

for (int i = 0; i < (number_of_channels - 1); i++){

currentReading = XAdcPs_GetAdcData(&XAdcInst, XADCPS_CH_AUX_MIN +
CHANNEL[i]);

dataArray[i] = XAdcPs_GetAdcData(&XAdcInst, XADCPS_CH_AUX_MIN +

CHANNEL[i]);

if (currentReading > highestReading){highestReading = currentReading;}

}

for (int j = 0; j < (number_of_channels - 1); j++){

if (highestReading > 0){INTENSITY_COMP[j] = highestReading /

dataArray[j];}

}
}

//for 4 LDRs
void axisAverage(float intensity[]){

// [0]--------[1]
// | |
// [2]--------[3]

//x axis values

averagedIntensity[0] = (intensity[0] + intensity[1])/2; //top
averagedIntensity[1] = (intensity[2] + intensity[3])/2; //bottom

// y axis values
averagedIntensity[2] = (intensity[0] + intensity[2])/2; //left
 averagedIntensity[3] = (intensity[1] + intensity[3])/2; //right