Tim Michals
Published © GPL3+

Mini But Mighty motor control via builtin accelerometer

Builds on the project Mini But Mightyu, the MiniZed and Vitis for Motor Control using the built in accelerometer to control the speed.

BeginnerWork in progress3 hours268
Mini But Mighty motor control via builtin accelerometer

Things used in this project

Hardware components

MiniZed
Avnet MiniZed
×1
Pmod HB3
Digilent Pmod HB3
×1

Story

Read more

Code

lis2ds12.c

C/C++
#include <stdbool.h>
#include <stdio.h>
#include <unistd.h>
#include "platform.h"
#include "xil_printf.h"

#include "xparameters.h"
#include "xiicps.h"
#include "xil_printf.h"

#include "lis2ds12.h"

/* The original code was
 * https://www.element14.com/community/thread/66802/l/i2csensor-app-source-code
 *
 */

/************************** Constant Definitions ******************************/

/*
 * The following constants map to the XPAR parameters created in the
 * xparameters.h file. They are defined here such that a user can easily
 * change all the needed parameters in one place.
 */


#define IIC_DEVICE_ID		XPAR_XIICPS_0_DEVICE_ID

/*
 * The slave address to send to and receive from.
 */
#define IIC_SLAVE_ADDR		0x55
#define IIC_SCLK_RATE		100000

XIicPs Iic;		/**< Instance of the IIC Device */

#define MINIZED_MOTION_SENSOR_ADDRESS_SA0_HI  0x1D /* 0011101b for LIS2DS12 on MiniZed when SA0 is pulled high*/

#define TEST_BUFFER_SIZE 32
/*
 * The following buffers are used in this example to send and receive data
 * with the IIC.
 */
u8 SendBuffer[TEST_BUFFER_SIZE];    /**< Buffer for Transmitting Data */
u8 RecvBuffer[TEST_BUFFER_SIZE];    /**< Buffer for Receiving Data */



int IicPsMasterPolledExample()
{
	int Status;
	XIicPs_Config *Config;
	int Index;

	/*
	 * Initialize the IIC driver so that it's ready to use
	 * Look up the configuration in the config table,
	 * then initialize it.
	 */
	Config = XIicPs_LookupConfig(IIC_DEVICE_ID);
	if (NULL == Config) {
		return XST_FAILURE;
	}

	Status = XIicPs_CfgInitialize(&Iic, Config, Config->BaseAddress);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	/*
	 * Perform a self-test to ensure that the hardware was built correctly.
	 */
	Status = XIicPs_SelfTest(&Iic);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	/*
	 * Set the IIC serial clock rate.
	 */
	XIicPs_SetSClk(&Iic, IIC_SCLK_RATE);

	return XST_SUCCESS;

}



u8 LIS2DS12_WriteReg(u8 Reg, u8 *Bufp, u16 len)
{
	SendBuffer[0] = Reg;
	int Status;
	for (int ByteCount = 1;ByteCount <= len; ByteCount++)
	{
		SendBuffer[ByteCount] = Bufp[ByteCount-1];
	}

	/*
	 * Send the buffer using the IIC and ignore the number of bytes sent
	 * as the return value since we are using it in interrupt mode.
	 */
	Status = XIicPs_MasterSendPolled(&Iic, SendBuffer,	 (len+1), MINIZED_MOTION_SENSOR_ADDRESS_SA0_HI);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	/*
	 * Wait until bus is idle to start another transfer.
	 */
	while (XIicPs_BusIsBusy(&Iic)) {
		/* NOP */
	}

	return XST_SUCCESS;
}

u8 LIS2DS12_ReadReg(uint8_t Reg, uint8_t *Bufp, uint16_t len)
{
	int Status;
	SendBuffer[0] = Reg;

	/*
	 * Send the buffer using the IIC and ignore the number of bytes sent
	 * as the return value since we are using it in interrupt mode.
	 */
	Status = XIicPs_MasterSendPolled(&Iic, SendBuffer,1, MINIZED_MOTION_SENSOR_ADDRESS_SA0_HI);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	/*
	 * Wait until bus is idle to start another transfer.
	 */
	while (XIicPs_BusIsBusy(&Iic)) {
		/* NOP */
	}

	Status = XIicPs_MasterRecvPolled(&Iic, Bufp, len, MINIZED_MOTION_SENSOR_ADDRESS_SA0_HI);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	while (XIicPs_BusIsBusy(&Iic)) {
		/* NOP */
	}

	return XST_SUCCESS;
}


bool isSensorConnected()
{
	uint8_t who_am_i = 0;
	uint8_t send_byte;

	LIS2DS12_ReadReg(LIS2DS12_ACC_WHO_AM_I_REG, &who_am_i, 1);
	printf("With I2C device address 0x%02x received WhoAmI = 0x%02x\r\n", MINIZED_MOTION_SENSOR_ADDRESS_SA0_HI, who_am_i);
	if (who_am_i != LIS2DS12_ACC_WHO_AM_I)
	{
		//maybe the address bit was changed, try the other one:
		LIS2DS12_ReadReg(LIS2DS12_ACC_WHO_AM_I_REG, &who_am_i, 1);
		printf("With I2C device address 0x%02x received WhoAmI = 0x%02x\r\n", MINIZED_MOTION_SENSOR_ADDRESS_SA0_HI, who_am_i);
	}
	send_byte = 0x00; //No auto increment
	LIS2DS12_WriteReg(LIS2DS12_ACC_CTRL2, &send_byte, 1);

	//Write 60h in CTRL1	// Turn on the accelerometer.  14-bit mode, ODR = 400 Hz, FS = 2g
	send_byte = 0x60;
	LIS2DS12_WriteReg(LIS2DS12_ACC_CTRL1, &send_byte, 1);
	printf("CTL1 = 0x60 written\r\n");

	//Enable interrupt
	send_byte = 0x01; //Acc data-ready interrupt on INT1
	LIS2DS12_WriteReg(LIS2DS12_ACC_CTRL4, &send_byte, 1);
	printf("CTL4 = 0x01 written\r\n");
	return true;

}
bool sensor_init()
{
	if (XST_SUCCESS != IicPsMasterPolledExample())
	{
		return false;
	}
	isSensorConnected();

	return true;
}


int u16_2s_complement_to_int(u16 word_to_convert)
{
	u16 result_16bit;
	int result_14bit;
	int sign;

	if (word_to_convert & 0x8000)
	{ //MSB is set, negative number
		//Invert and add 1
		sign = -1;
		result_16bit = (~word_to_convert) + 1;
	}
	else
	{ //Positive number
		//No change
		sign = 1;
		result_16bit = word_to_convert;
	}
	//We are using it in 14-bit mode
	//All data is left-aligned.  So convert 16-bit value to 14-but value
	result_14bit = sign * (int)(result_16bit >> 2);
	return(result_14bit);
} //u16_2s_complement_to_int()



bool pollForAccel(int *x, int *y )
{
		int iacceleration_X;
		int iacceleration_Y;
		int iacceleration_Z;
		u8 read_value_LSB;
		u8 read_value_MSB;
		u16 accel_X;
		u16 accel_Y;
		u16 accel_Z;
		u8 accel_status;
		u8 data_ready;

		data_ready = 0;

		 //wait for DRDY
		LIS2DS12_ReadReg(LIS2DS12_ACC_STATUS, &accel_status, 1);
		data_ready = accel_status & 0x01; //bit 0 = DRDY
		if (!data_ready)
			return false;

		//wait for DRDY


		//Read X:
		LIS2DS12_ReadReg(LIS2DS12_ACC_OUT_X_L, &read_value_LSB, 1);
		LIS2DS12_ReadReg(LIS2DS12_ACC_OUT_X_H, &read_value_MSB, 1);
		accel_X = (read_value_MSB << 8) + read_value_LSB;
		iacceleration_X = u16_2s_complement_to_int(accel_X);
		//Read Y:
		LIS2DS12_ReadReg(LIS2DS12_ACC_OUT_Y_L, &read_value_LSB, 1);
		LIS2DS12_ReadReg(LIS2DS12_ACC_OUT_Y_H, &read_value_MSB, 1);
		accel_Y = (read_value_MSB << 8) + read_value_LSB;
		iacceleration_Y = u16_2s_complement_to_int(accel_Y);
		//Read Z:
		LIS2DS12_ReadReg(LIS2DS12_ACC_OUT_Z_L, &read_value_LSB, 1);
		LIS2DS12_ReadReg(LIS2DS12_ACC_OUT_Z_H, &read_value_MSB, 1);
		accel_Z = (read_value_MSB << 8) + read_value_LSB;
		iacceleration_Z = u16_2s_complement_to_int(accel_Z);

		if (x)
		{
			*x = iacceleration_X;
		}

		if (y)
		{
			*y = iacceleration_Y;
		}
	//	printf("  Acceleration = X: %+5d, Y: %+5d, Z: %+5d\r\n",iacceleration_X, iacceleration_Y, iacceleration_Z);
//		printf("  Acceleration = X: %+5d, Y: %+5d, Z: %+5d\r",iacceleration_X, iacceleration_Y, iacceleration_Z);

		return true;
}

helloworld.c

C/C++
#include <stdio.h>
#include <stdlib.h>
#include "platform.h"
#include "xil_printf.h"
#include "xgpiops.h"
#include "sleep.h"
#include "xil_exception.h"
#include "xttcps.h"
#include "xscugic.h"
#include "lis2ds12.h"



#define ACCEL_SCALE_VAL 100/2000
#define ACCEL_OFF_VAL  200

#define GPIO_DEVICE_ID XPAR_XGPIOPS_0_DEVICE_ID
#define INTC_DEVICE_ID XPAR_SCUGIC_0_DEVICE_ID
#define TICK_TIMER_FREQ_HZ 100
#define TTC_TICK_DEVICE_ID XPAR_XTTCPS_0_DEVICE_ID
#define TTC_TICK_INTR_ID XPAR_XTTCPS_0_INTR
static void TickHandler(void *CallBackRef);

int SetupTicker(XTtcPs *TtcPsInst, u16 DeviceID, u16 TtcTickIntrID, XScuGic *InterruptController);
static int SetupInterruptSystem(u16 IntcDeviceID, XScuGic *IntcInstancePtr);
int SetupTimer(u16 DeviceID, XTtcPs *TtcPsInst);
void set_pwm(u32 cycle);
void display_menu();
typedef struct
{
    u32 OutputHz;       /* Output frequency */
    XInterval Interval; /* Interval value */
    u8 Prescaler;       /* Prescaler value */
    u16 Options;        /* Option settings */
} TmrCntrSetup;
XGpioPs Gpio;
XGpioPs_Config *ConfigPtr;
XTtcPs_Config *TtcConfig;
XTtcPs ttcTimer;
TmrCntrSetup *TimerSetup;
XScuGic InterruptController; /* Interrupt controller instance */
XTtcPs TtcPsInst;
u32 MatchValue;
static TmrCntrSetup SettingsTable = {TICK_TIMER_FREQ_HZ, 0, 0, 0};


int main()
{
    u8 DutyCycle;
    char key_input;
    init_platform();
    sensor_init();

    TmrCntrSetup SettingsTable = {TICK_TIMER_FREQ_HZ, 0, 0, 0};
    ConfigPtr = XGpioPs_LookupConfig(GPIO_DEVICE_ID);
    XGpioPs_CfgInitialize(&Gpio, ConfigPtr, ConfigPtr->BaseAddr);
    XGpioPs_SetDirectionPin(&Gpio, 54, 1);
    XGpioPs_SetOutputEnablePin(&Gpio, 54, 1);
    XGpioPs_WritePin(&Gpio, 54, 0x1);

    printf("www.adiuvoengineering.com\n\r");
    printf("DC Motor Control Example\n\r");
    SetupInterruptSystem(INTC_DEVICE_ID, &InterruptController);
    SetupTicker(&ttcTimer, TTC_TICK_DEVICE_ID, TTC_TICK_INTR_ID, &InterruptController);

    int x_axis_current, y_axis_current;
    int x_axis_inital, y_axis_inital;

    while( false == pollForAccel(&x_axis_inital, &y_axis_inital ));
    x_axis_inital = abs(x_axis_inital);

    while (1)
    {
        if ( pollForAccel( &x_axis_current, &y_axis_current))
        {
        	x_axis_current = abs(x_axis_current);

        	int diff_x = x_axis_current - x_axis_inital;
        	if (diff_x < ACCEL_OFF_VAL)
        	{
        		DutyCycle = 0;
        	}
        	else
        	{
        		DutyCycle = (u8)((float)ACCEL_SCALE_VAL * (float)diff_x);
        		if (DutyCycle > 100)
        		{
        			DutyCycle = 0;
        		}
        	}
        	set_pwm(DutyCycle);
        	printf("  Acceleration =  %+5d, X: diff =  %+5d,  DutyCycle = %+5d \r",x_axis_current, diff_x, DutyCycle);
        }


#ifdef USE_SERIAL
        display_menu();
        read(1, (char *)&key_input, 1);
        printf("Echo %c\n\r", key_input);
        switch (key_input)
        {
        //			case 0: // forward
        //
        //				set_pwm(0);
        //				usleep(1000000);
        //
        //				set_pwm(DutyCycle);
        //			break;
        //			case 1: //reverse
        //
        //				//set_pwm(0);
        //				//usleep(1000000);
        //				//XGpioPs_WritePin(&Gpio, 54, 0x1);
        //				//set_pwm(DutyCycle);
        //			break;
        case '1': //stop
            set_pwm(0);
            break;
        case '2': //25%
            printf("25%\n\r");
            DutyCycle = 25;
            set_pwm(DutyCycle);
            break;
        case '3': //33%
            DutyCycle = 33;
            set_pwm(DutyCycle);
            break;
        case '4': //50%
            DutyCycle = 50;
            set_pwm(DutyCycle);
            break;
        case '5': //66%
            DutyCycle = 66;
            set_pwm(DutyCycle);
            break;
        case '6': //75%
            DutyCycle = 75;
            set_pwm(DutyCycle);
            break;
        case '7': //100%
            DutyCycle = 100;
            set_pwm(DutyCycle);
            break;
        }
#endif
    }
    cleanup_platform();
    return 0;
}
void display_menu()
{
    //Clear the screen
    printf("\033[2J");
    //Display the main menu
    printf("*******************************************\n");
    printf("****      www.adiuvoengineering.com    ****\n");
    printf("****      Motor Control Example        ****\n");
    printf("*******************************************\n");
    printf("\n");
    printf("   MM10 Motor Control   \n");
    printf("------------------------------------------\n");
    printf("\n");
    printf("Select a Speed:\n");
    printf("  (1)   - Stop\n");
    printf("  (2)   - 25% \n");
    printf("  (3)   - 33% \n");
    printf("  (4)   - 50% \n");
    printf("  (5)   - 66% \n");
    printf("  (6)   - 75% \n");
    printf("  (7)   - 100% \n");
    printf("\n");
}
void set_pwm(u32 cycle)
{
    u32 MatchValue;
    MatchValue = (TimerSetup->Interval * cycle) / 100;
    XTtcPs_SetMatchValue(&ttcTimer, 0, MatchValue);
}
int SetupTicker(XTtcPs *TtcPsInst, u16 DeviceID, u16 TtcTickIntrID, XScuGic *InterruptController)
{
    int Status;
    TmrCntrSetup *TimerSetup;
    XTtcPs *TtcPsTick;
    TimerSetup = &SettingsTable;
    TimerSetup->Options |= (XTTCPS_OPTION_INTERVAL_MODE |
                            XTTCPS_OPTION_MATCH_MODE | XTTCPS_OPTION_WAVE_POLARITY);
    Status = SetupTimer(DeviceID, TtcPsInst);
    if (Status != XST_SUCCESS)
    {
        return Status;
    }
    TtcPsTick = TtcPsInst;
    Status = XScuGic_Connect(InterruptController, TtcTickIntrID,
                             (Xil_InterruptHandler)TickHandler, (void *)TtcPsTick);
    if (Status != XST_SUCCESS)
    {
        return XST_FAILURE;
    }
    XScuGic_Enable(InterruptController, TtcTickIntrID);
    XTtcPs_EnableInterrupts(TtcPsTick, XTTCPS_IXR_INTERVAL_MASK);
    XTtcPs_Start(TtcPsTick);
    return Status;
}
static int SetupInterruptSystem(u16 IntcDeviceID, XScuGic *IntcInstancePtr)
{
    int Status;
    XScuGic_Config *IntcConfig;
    IntcConfig = XScuGic_LookupConfig(IntcDeviceID);
    if (NULL == IntcConfig)
    {
        return XST_FAILURE;
    }
    Status = XScuGic_CfgInitialize(IntcInstancePtr, IntcConfig,
                                   IntcConfig->CpuBaseAddress);
    if (Status != XST_SUCCESS)
    {
        return XST_FAILURE;
    }
    Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_IRQ_INT,
                                 (Xil_ExceptionHandler)XScuGic_InterruptHandler,
                                 IntcInstancePtr);
    Xil_ExceptionEnable();
    return XST_SUCCESS;
}
int SetupTimer(u16 DeviceID, XTtcPs *TtcPsInst)
{
    int Status;
    XTtcPs_Config *Config;
    XTtcPs *Timer;
    TmrCntrSetup *TimerSetup;
    TimerSetup = &SettingsTable;
    Timer = TtcPsInst;
    Config = XTtcPs_LookupConfig(DeviceID);
    if (NULL == Config)
    {
        return XST_FAILURE;
    }
    Status = XTtcPs_CfgInitialize(Timer, Config, Config->BaseAddress);
    if (Status != XST_SUCCESS)
    {
        return XST_FAILURE;
    }
    XTtcPs_SetOptions(Timer, TimerSetup->Options);
    XTtcPs_CalcIntervalFromFreq(Timer, TimerSetup->OutputHz,
                                &(TimerSetup->Interval), &(TimerSetup->Prescaler));
    XTtcPs_SetInterval(Timer, TimerSetup->Interval);
    XTtcPs_SetPrescaler(Timer, TimerSetup->Prescaler);
    return XST_SUCCESS;
}
void TickHandler(void *CallBackRef)
{
    u32 StatusEvent;
    /*
* Read the interrupt status, then write it back to clear the interrupt.
*/
    StatusEvent = XTtcPs_GetInterruptStatus((XTtcPs *)CallBackRef);
    XTtcPs_ClearInterruptStatus((XTtcPs *)CallBackRef, StatusEvent);
    //printf("timer\n\r");
    /*update the flag if interrupt has been occurred*/
    //UpdateFlag = TRUE;
}


#ifdef OLD

#include <stdio.h>
#include "platform.h"
#include "xil_printf.h"


int main()
{
    init_platform();

    print("Hello World\n\r");

    cleanup_platform();
    return 0;
}

#endif

io.xdc

AsciiDoc
set_property PACKAGE_PIN M15 [get_ports {TTC0_WAVE0_OUT_0 }];
set_property PACKAGE_PIN L15 [get_ports {GPIO_O_0 }];
set_property IOSTANDARD LVCMOS33 [get_ports TTC0_WAVE0_OUT_0]
set_property IOSTANDARD LVCMOS33 [get_ports GPIO_O_0]

# ----------------------------------------------------------------------------
# I2C bus -- ST Micro LIS2DS12TR Accelerometer and temperature
#            Dialog DA9062
# ---------------------------------------------------------------------------- 
# Bank 35
set_property PACKAGE_PIN G15 [get_ports {IIC_0_0_scl_io        }];  # "G15.I2C_SCL"
set_property PACKAGE_PIN F15 [get_ports {IIC_0_0_sda_io         }];  # "F15.I2C_SDA"
# Set the bank voltage for IO Bank 35 to 3.3V
set_property IOSTANDARD LVCMOS33 [get_ports -of_objects [get_iobanks 35]];

lis2ds12.h

C/C++
#include <stdbool.h>
extern bool sensor_init(void);
extern bool pollForAccel(int *x, int *y );




/*
 * The following constant defines the address of the IIC device on the IIC bus.  Note that since
 * the address is only 7 bits, this  constant is the address divided by 2.
 */
#define MAGNETOMETER_ADDRESS  0x1E /* LIS3MDL on Arduino shield */
#define MINIZED_MOTION_SENSOR_ADDRESS_SA0_LO  0x1E /* 0011110b for LIS2DS12 on MiniZed when SA0 is pulled low*/
#define MINIZED_MOTION_SENSOR_ADDRESS_SA0_HI  0x1D /* 0011101b for LIS2DS12 on MiniZed when SA0 is pulled high*/

#define LIS2DS12_ACC_WHO_AM_I         0x43
/************** Device Register  *******************/
#define LIS2DS12_ACC_SENSORHUB_OUT1  	0X06
#define LIS2DS12_ACC_SENSORHUB_OUT2  	0X07
#define LIS2DS12_ACC_SENSORHUB_OUT3  	0X08
#define LIS2DS12_ACC_SENSORHUB_OUT4  	0X09
#define LIS2DS12_ACC_SENSORHUB_OUT5  	0X0A
#define LIS2DS12_ACC_SENSORHUB_OUT6  	0X0B
#define LIS2DS12_ACC_MODULE_8BIT  	0X0C
#define LIS2DS12_ACC_WHO_AM_I_REG  	0X0F
#define LIS2DS12_ACC_CTRL1  	0X20
#define LIS2DS12_ACC_CTRL2  	0X21
#define LIS2DS12_ACC_CTRL3  	0X22
#define LIS2DS12_ACC_CTRL4  	0X23
#define LIS2DS12_ACC_CTRL5  	0X24
#define LIS2DS12_ACC_FIFO_CTRL  	0X25
#define LIS2DS12_ACC_OUT_T  	0X26
#define LIS2DS12_ACC_STATUS  	0X27
#define LIS2DS12_ACC_OUT_X_L  	0X28
#define LIS2DS12_ACC_OUT_X_H  	0X29
#define LIS2DS12_ACC_OUT_Y_L  	0X2A
#define LIS2DS12_ACC_OUT_Y_H  	0X2B
#define LIS2DS12_ACC_OUT_Z_L  	0X2C
#define LIS2DS12_ACC_OUT_Z_H  	0X2D
#define LIS2DS12_ACC_FIFO_THS  	0X2E
#define LIS2DS12_ACC_FIFO_SRC  	0X2F
#define LIS2DS12_ACC_FIFO_SAMPLES  	0X30
#define LIS2DS12_ACC_TAP_6D_THS  	0X31
#define LIS2DS12_ACC_INT_DUR  	0X32
#define LIS2DS12_ACC_WAKE_UP_THS  	0X33
#define LIS2DS12_ACC_WAKE_UP_DUR  	0X34
#define LIS2DS12_ACC_FREE_FALL  	0X35
#define LIS2DS12_ACC_STATUS_DUP  	0X36
#define LIS2DS12_ACC_WAKE_UP_SRC  	0X37
#define LIS2DS12_ACC_TAP_SRC  	0X38
#define LIS2DS12_ACC_6D_SRC  	0X39
#define LIS2DS12_ACC_STEP_C_MINTHS  	0X3A
#define LIS2DS12_ACC_STEP_C_L  	0X3B
#define LIS2DS12_ACC_STEP_C_H  	0X3C
#define LIS2DS12_ACC_FUNC_CK_GATE  	0X3D
#define LIS2DS12_ACC_FUNC_SRC  	0X3E
#define LIS2DS12_ACC_FUNC_CTRL  	0X3F

Credits

Tim Michals

Tim Michals

1 project • 3 followers

Comments