#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include "inc/hw_types.h"
#include "inc/hw_memmap.h"
#include "inc/hw_gpio.h"
#include "driverlib/sysctl.h"
#include "driverlib/pin_map.h"
#include "driverlib/gpio.h"
#include "driverlib/timer.h"
#include "driverlib/interrupt.h"
#include "driverlib/i2c.h"
#include "driverlib/pwm.h"
#include "driverlib/uart.h"
#include "inc/tm4c123gh6pm.h"
// In this system, when SW1 is pressed and held it will ping the rotational sensor 20 times a second to get deg/sec
// and it will ping the distance sensor 20 times a second to get range to target in cm
// the linear velocity is derived two ways
// method one:
// W * R = V, this is the rotational velocity converted to linear velocity
// this method is selected if there is a significant change in rotation rate W
//
// method two:
// (R - R_old) * 1/20 sec = V, this is change in distance multiplied by
// change in time this method is selected if there has not been enough
// of a rotation by the user
//
// Finally the average is found as SW1 is pressed and the final result is when
//the switch
is released.
//
// Also when SW2 is pressed it will toggle a laser pointer for better aiming
//
// the accelerometer has the B2 pin connected to SCL
// and B3 pin is connected to the SDA
// next steps
// better distance filtering
// derive target movement vector from W and change in distance
// create enclosure
// built in serial screen
// the pins connecting to the TOF sensor are PC4 -> TX on TOF green cable
// PC5 -> RX on ToF white cable
#define CLOCK_TIC 4000000 // runs system at 10Hz
#define MPU6050_ADDRESS 0x68 // 0x68 //_AD0_LOW
#define MPU6050_DATA_R 0x3B // XOUT_H,XL,YH,YL,ZH,ZL,TempH,TempL,GXH,GXL,GYH,GYL,GZH,GZL=0x48, 14 total
#define GYRO_Y_high 0x45 // bites 15 to 8 for y axis gyro
#define GYRO_Y_low 0x46 // bites 7 to 0 for y axis gyro
#define TOLERANCE 50 // value that the pitch and roll compare, and when to turn on the green light showing stable
#define LASER 0x80 // port A pin 7, tied to laser MOSFET
volatile uint16_t GYRO_Y_OLD; // global variable to save old rotational acceleration to determine if rotation is negative (CW) or not
volatile uint8_t pst_bt1 = 0x01; // 8bit variable to hold status of previous button
volatile float W_old = 0; // saved angular speed variable, this is used to determine if theres been a change in angle
volatile float W = 0; // angular speed variable, found with accelerometer
float W_TOLERANCE = 0.05; // angular velocity tolerance, determines which equation to use to find speed
volatile float R_old = 0; // old radius to target
volatile float R = 0; // Radius to target, found with distance sensor
volatile float V_sum = 0; // adds the found velocity of each timerA cycle, will be divided by sample count to find average
volatile uint16_t sampleCount = 0; // global variable to count the samples, used for averageing speed
volatile float V_avg = 0; // average Velocity found by system
volatile int out_avg = 0;
volatile bool lazOn = false;
volatile bool sw2Hold = false;
volatile bool sw1Hold = false;
uint8_t pitRED = 0;
uint8_t rolBLU = 0;
uint8_t lvlGRN = 0;
void PortInit(void)
{
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0);
SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM1);
SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA); // UART0 Pins
GPIOPinTypeGPIOOutput(GPIO_PORTA_BASE, LASER); // Pin A7 Output, tied to laser
SysCtlPeripheralEnable(SYSCTL_PERIPH_UART1); // UART1 Pins
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
SysCtlPeripheralEnable(SYSCTL_PERIPH_I2C0);
SysCtlPeripheralReset(SYSCTL_PERIPH_I2C0);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB); // I2C Pins
GPIOPinConfigure(GPIO_PA0_U0RX);
GPIOPinConfigure(GPIO_PA1_U0TX);
GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);
SysCtlDelay(5); // let clocks stablize
// PortF
HWREG(GPIO_PORTF_BASE + GPIO_O_LOCK) = GPIO_LOCK_KEY;
HWREG(GPIO_PORTF_BASE + GPIO_O_CR) = 0x1; // unlock Pin 0
GPIOPinTypeGPIOInput(GPIO_PORTF_BASE, 0x11); // Pin 0,4 Input
GPIO_PORTF_PUR_R |= 0x11; // pull up resistors needed
// Timer
TimerConfigure(TIMER0_BASE, TIMER_CFG_PERIODIC); // configure for 32-bit timer mode
TimerLoadSet(TIMER0_BASE, TIMER_A, CLOCK_TIC); // reload value, run at clock tick variable speed
IntPrioritySet(INT_TIMER0A, 0x00); // configure Timer0A interrupt priority as 0
IntEnable(INT_TIMER0A); // enable interrupt 19 in NVIC (Timer0A)
TimerIntEnable(TIMER0_BASE, TIMER_TIMA_TIMEOUT); // arm timeout interrupt
// UART0, this is used to Write to serial port
GPIOPinConfigure(GPIO_PA0_U0RX);
GPIOPinConfigure(GPIO_PA1_U0TX);
GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);
UARTConfigSetExpClk(UART0_BASE, SysCtlClockGet(), 115200, (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE));
UARTFIFOEnable(UART0_BASE);
// UART1, this connects to ToF sensor
GPIOPinConfigure(GPIO_PC4_U1RX);
GPIOPinConfigure(GPIO_PC5_U1TX); // may not need this since i am not talking to sensor, just reading
GPIOPinTypeUART(GPIO_PORTC_BASE, GPIO_PIN_4 | GPIO_PIN_5);
UARTConfigSetExpClk(UART1_BASE, SysCtlClockGet(), 115200, (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE));
UARTFIFOEnable(UART1_BASE);
GPIOPinConfigure(GPIO_PF1_M1PWM5); // red //PWM
GPIOPinTypePWM(GPIO_PORTF_BASE, GPIO_PIN_1);
GPIOPinConfigure(GPIO_PF2_M1PWM6); // blue
GPIOPinTypePWM(GPIO_PORTF_BASE, GPIO_PIN_2);
GPIOPinConfigure(GPIO_PF3_M1PWM7); // green
GPIOPinTypePWM(GPIO_PORTF_BASE, GPIO_PIN_3);
PWMGenConfigure(PWM1_BASE, PWM_GEN_2, PWM_GEN_MODE_DOWN | PWM_GEN_MODE_NO_SYNC); // red
PWMGenPeriodSet(PWM1_BASE, PWM_GEN_2, 512); // freq 1/512 of system, and range for widthSet
PWMGenConfigure(PWM1_BASE, PWM_GEN_3, PWM_GEN_MODE_DOWN | PWM_GEN_MODE_NO_SYNC); // blue and green
PWMGenPeriodSet(PWM1_BASE, PWM_GEN_3, 512);
PWMPulseWidthSet(PWM1_BASE, PWM_OUT_5, 1); // red (1 off 512 max), do not set as 0
PWMPulseWidthSet(PWM1_BASE, PWM_OUT_6, 1); // blue
PWMPulseWidthSet(PWM1_BASE, PWM_OUT_7, 1); // green
PWMGenEnable(PWM1_BASE, PWM_GEN_2); // once setup, enable generators
PWMGenEnable(PWM1_BASE, PWM_GEN_3);
PWMOutputState(PWM1_BASE, (PWM_OUT_5_BIT | PWM_OUT_6_BIT | PWM_OUT_7_BIT), true);
GPIOPinConfigure(GPIO_PB2_I2C0SCL); // I2C
GPIOPinConfigure(GPIO_PB3_I2C0SDA);
GPIOPinTypeI2CSCL(GPIO_PORTB_BASE, GPIO_PIN_2);
GPIOPinTypeI2C(GPIO_PORTB_BASE, GPIO_PIN_3);
GPIO_PORTB_PUR_R |= 0x0C; // I2C buss needs pull up, normally ~4k, these internal will work but are ~10k
I2CMasterInitExpClk(I2C0_BASE, SysCtlClockGet(), false); // false==100kbps,true==400kbps
I2CMasterTimeoutSet(I2C0_BASE, 0x7D); //~20ms, based on 100kHz i2c clock.
I2CMasterEnable(I2C0_BASE);
}
void uartSend(char *mess)
{ // this function was used from the Lab 8 UART example
for (uint8_t i = 0; mess[i] != '\0'; i++)
{
while (!UARTSpaceAvail(UART0_BASE))
{
}
UARTCharPutNonBlocking(UART0_BASE, mess[i]);
}
}
void uartSendInt(uint64_t value, bool positive)
{ // Needs input to know whether value is negative or not.
uint8_t dig = 0;
char ch[12]; // max 12 digit long
// Had isses with zeros outputing nothing
if (value < 1)
{
UARTCharPutNonBlocking(UART0_BASE, '0');
return;
}
if (!positive)
{
UARTCharPutNonBlocking(UART0_BASE, '-');
}
while (value != 0)
{
ch[dig++] = (value % 10) + '0'; // modulus the current ones places, change to ASCII
value /= 10; // shift decimal to next
}
while (dig != 0)
{ // reverse digit order
dig--;
while (!UARTSpaceAvail(UART0_BASE))
{
}
UARTCharPutNonBlocking(UART0_BASE, ch[dig]);
}
}
void writeI2C(uint8_t target_reg, uint8_t data)
{ // sends a single value to the target register
I2CMasterSlaveAddrSet(I2C0_BASE, MPU6050_ADDRESS, false); // setsup sub device to receive
I2CMasterDataPut(I2C0_BASE, target_reg); // Place first value (the sub reg) in the data buffer on the primary device
I2CMasterControl(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_START); // sends a signal to aux device to get ready to receive data at the target register
while (I2CMasterBusy(I2C0_BASE))
; // delay to wait for auxillery device to respond
I2CMasterDataPut(I2C0_BASE, data); // send data to auxillery device register
I2CMasterControl(I2C0_BASE, I2C_MASTER_CMD_BURST_SEND_FINISH); // sends signal to device that the transmission is done
while (I2CMasterBusy(I2C0_BASE))
; // delay to wait for auxillery device to respond
}
// this function was used from the I2C lab 10 example
void I2CBurstReceive(uint32_t base, uint32_t sub_addr, uint8_t sub_reg, uint8_t length, uint8_t *dataOut)
{
// Warning, can be long blocking function
// First sending requested reg
I2CMasterSlaveAddrSet(base, sub_addr, false); // First sending requested reg
I2CMasterDataPut(base, sub_reg);
I2CMasterControl(base, I2C_MASTER_CMD_BURST_SEND_START); // sends reg byte and leave frame open
while (I2CMasterBusy(base))
{
} /// BLOCKING FUNCTION,
SysCtlDelay(1024); // 512, 16M/s*32us//Assuming 16M TOF needs least 30us to measure
// Now switching to read first value
I2CMasterSlaveAddrSet(base, sub_addr, true); // I2C0->MSA = (subAddr << 1) + 1;
if (length < 2)
{ // 0 and 1 treated as signal value read
I2CMasterControl(base, I2C_MASTER_CMD_SINGLE_RECEIVE);
} // I2C0->MCS = 7;
else
{
I2CMasterControl(base, I2C_MASTER_CMD_BURST_RECEIVE_START);
} // I2C0->MCS = b;}
while (I2CMasterBusy(base))
{
} /// BLOCKING FUNCTION,Better to use ISR on i2c
dataOut[0] = I2CMasterDataGet(base);
if (length < 2)
{
return;
} // if only one value, leave function here
// Read mid values
for (char i = 1; i < (length - 1); i++)
{
I2CMasterControl(base, I2C_MASTER_CMD_BURST_RECEIVE_CONT); // 9
while (I2CMasterBusy(base))
{
} /// BLOCKING FUNCTION,Better to use ISR on i2c
dataOut[i] = I2CMasterDataGet(base);
}
// Read last
I2CMasterControl(base, I2C_MASTER_CMD_BURST_RECEIVE_FINISH); // 5
while (I2CMasterBusy(base))
{
} /// BLOCKING FUNCTION,Better to use ISR on i2c
dataOut[length - 1] = I2CMasterDataGet(base);
}
void GetAngularSpeed(void)
{ // method to return angular speed after pinging the accelerometer Also will change the PWM output to the LEDs
uint8_t dataHIGH[8]; // i = 2 to get Yaw rate, CCW = + most sig bits
uint8_t dataLOW[8]; // i = 2 to get yaw rate, CCW = + least sig bits
uint8_t data[8]; // used to store the xyz acceleration angles
I2CBurstReceive(I2C0_BASE, MPU6050_ADDRESS, GYRO_Y_high, 8, dataHIGH); // pings gyro
I2CBurstReceive(I2C0_BASE, MPU6050_ADDRESS, GYRO_Y_high, 8, dataLOW); // pings gyro again for rest of data
I2CBurstReceive(I2C0_BASE, MPU6050_ADDRESS, MPU6050_DATA_R, 8, data); // pings accelerometer
// whats happening here is that each for loop reads the values from each register
// then prints it. I should see if i can get this to work in serial and see if thats more readable
// print to UART
uint16_t roll_raw = (dataHIGH[0] << 8) | (dataLOW[0]);
uint16_t yaw_raw = (dataHIGH[2] << 8) | (dataLOW[2]); //[0] is the roll axis, [2] is the yaw axis
rolBLU = 0;
lvlGRN = 255;
int output_yaw = (yaw_raw * 250) / 65536; // returns degrees/sec
int output_roll = (roll_raw * 250) / 65536; // returns degrees/sec
if (data[0] > TOLERANCE + abs(output_yaw))
{ // constant is a tolerance, the output yaw takes into account rotational acceleration which would be mistaken for tilting
rolBLU = 255;
lvlGRN = 0;
}
if (yaw_raw & 0x8000)
{ // check if yaw is negative
output_yaw = abs(output_yaw - 250); // should give me negative value
}
// so final "output" is the degrees per second
PWMPulseWidthSet(PWM1_BASE, PWM_OUT_6, rolBLU + 1); // blue is roll, outputs if the device is rolled too much
PWMPulseWidthSet(PWM1_BASE, PWM_OUT_7, lvlGRN + 1); // green, displays if the orientation is good
PWMPulseWidthSet(PWM1_BASE, PWM_OUT_5, 1); // red, Turns on when the button isnt pressed
W_old = W;
W = output_yaw * (3.14 / 180); // returns the yaw output in radians/sec
}
void GetDistance(void)
{ // this function will get the distance from the ToF sensor using UART1
// RX pin is PC4, TX pin is PC5
// while pinging the UART I need to check if rx[7] = x59, else i need to re-ping
// if rx[0] = 59, then distance is rx[8]+rx[9]*256 = cm
int rx[10]; // stores values from ToF sensor
for (int i = 0; i < 10; i++)
{ // loop to read each byte from UART
rx[i] = UARTCharGetNonBlocking(UART1_BASE);
}
if (rx[7] == 0x59)
{ // check if value is good, HERE it sort of filters, this could be better
R = rx[8] + rx[9] * 256; // byte 8 and 9 are the laser range values in HEX
}
}
void SumSpeed()
{ // function to get the add the speeds of each timer trigger
// two cases, get speed from angular rotation and radius
// or get speed from only change in distance over time.
// this change in time is the frequency that the timer triggers, so 20Hz
if (W > (W_old + W_TOLERANCE) | W < (W_old - W_TOLERANCE))
{ // Case 1: speed from angular rotation
V_sum = V_sum + (R * W);
}
else
{ // case 2: speed from change in distance
V_sum = V_sum + ((R - R_old) * (SysCtlClockGet() / CLOCK_TIC)); // bit of math, here it finds change in distance times change in time
}
R_old = R;
}
// this function will get the horizontal rotation rate and tilt
// it will output a rotation rate, but it will turn a light blue if the roll is too much
// if the orientation is good the light shows green
void Timer0A_Handler(void)
{ // Set to Execute 64/s
TimerIntClear(TIMER0_BASE, TIMER_TIMA_TIMEOUT); // acknowledge flag for Timer0A timeout
uint8_t btn_SW1 = GPIOPinRead(GPIO_PORTF_BASE, GPIO_PIN_4); // the GPIO SW1 ACTIVATES ToF SENSOR AND ESTIMATES SPEED
uint8_t btn_SW2 = GPIOPinRead(GPIO_PORTF_BASE, GPIO_PIN_0); // the GPIO SW2 TOGGLES RED LASER POINTER
// polls sw1 to see if it is pressed
if (btn_SW1 == 0x00)
{ // if SW1 is pressed
if (!sw1Hold)
{ // here it resets the variables on rising edge
V_sum = 0;
sampleCount = 0;
R = 0;
W = 0;
}
sw1Hold = true;
sampleCount++;
GetAngularSpeed(); // pings the accelerometer to get angular speed
GetDistance(); // pings ToF sensor to get distance
SumSpeed(); // does calculations to determine speed
}
else
{ // SW1 button Not Pressed
sw1Hold = false;
// Turn on Red LED to show button not pressed
PWMPulseWidthSet(PWM1_BASE, PWM_OUT_6, 1); // blue is roll, outputs if the device is rolled too much
PWMPulseWidthSet(PWM1_BASE, PWM_OUT_7, 1); // green, displays if the orientation is good
PWMPulseWidthSet(PWM1_BASE, PWM_OUT_5, 256); // red, Turns on when the button isnt pressed
V_avg = V_sum / sampleCount;
out_avg = V_avg; // converts float to int
if (out_avg < 0)
{ // checks to make sure not negative
out_avg = abs(out_avg);
}
// here it prints to the serial port
uartSend("target average velocity: ");
uartSendInt(out_avg, true);
uartSend(" cm/sec\r\n");
}
if (btn_SW2 == 0x00)
{ // SW2 is pressed
if (!sw2Hold & !lazOn)
{
GPIOPinWrite(GPIO_PORTA_BASE, LASER, 0xff); // turns on Pin 7 on port A, which should turn on the laser pointer
sw2Hold = true;
}
if (lazOn)
{
GPIOPinWrite(GPIO_PORTA_BASE, LASER, 0x00); // turns off Pin 7 on port A, which should turn off the laser pointer
sw2Hold = false;
}
}
else
{ // sw2 is not pressedf
if (sw2Hold & !lazOn)
{ // should trigger on release of button
lazOn = true;
}
if (!sw2Hold & lazOn)
{
lazOn = false;
}
}
}
int main(void)
{
PortInit();
SysCtlDelay(100); // short pause to get the clocks started
writeI2C(0x6B, 0x00); // wakes up the accelerometer/gyro
uartSend("System Starting!\r\n");
TimerEnable(TIMER0_BASE, TIMER_A); // starts timer
while (1)
{
}
}