Scribd
Upload
24 views8 pages

For Vlad

The document is a C program that interfaces with hardware components to control motors based on light intensity readings from an ADC. It initializes various peripherals including PWM timers, GPIO for motor control, LEDs, and buttons, and continuously polls the ADC for intensity data to adjust motor movement accordingly. The program also includes functions for efficiency calculations and recalibration of intensity compensators based on button presses.

Uploaded by

koulachannel05
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
24 views8 pages

For Vlad

The document is a C program that interfaces with hardware components to control motors based on light intensity readings from an ADC. It initializes various peripherals including PWM timers, GPIO for motor control, LEDs, and buttons, and continuously polls the ADC for intensity data to adjust motor movement accordingly. The program also includes functions for efficiency calculations and recalibration of intensity compensators based on button presses.

Uploaded by

koulachannel05
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
You are on page 1/ 8

#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

You might also like