int rt_hw_cpu_init(void)
{
MAP_IntMasterDisable();
IntRegister(FAULT_HARD, HardFault_Handler);
IntRegister(FAULT_PENDSV, PendSV_Handler);
IntRegister(FAULT_SYSTICK, SysTick_Handler);
// Enable lazy stacking for interrupt handlers. This allows floating-point
// instructions to be used within interrupt handlers, but at the expense of
// extra stack usage.
MAP_FPULazyStackingEnable();
// Set the clocking to run directly from the external crystal/oscillator.
// TODO: The SYSCTL_XTAL_ value must be changed to match the value of the
// crystal on your board.
SysClock = MAP_SysCtlClockFreqSet(
(SYSCTL_XTAL_25MHZ | SYSCTL_OSC_MAIN | SYSCTL_USE_PLL | SYSCTL_CFG_VCO_480),
SYS_CLOCK_DEFAULT);
MAP_SysTickDisable();
MAP_SysTickPeriodSet(SysClock/ RT_TICK_PER_SECOND - 1);
MAP_SysTickIntEnable();
MAP_SysTickEnable();
return 0;
}
int main(void)
{
// TivaC application specific code
MAP_FPUEnable();
MAP_FPULazyStackingEnable();
// TivaC system clock configuration. Set to 80MHz.
MAP_SysCtlClockSet(SYSCTL_SYSDIV_2_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN);
uint8_t button_debounced_delta;
uint8_t button_raw_state;
ButtonsInit();
// ROS nodehandle initialization and topic registration
nh.initNode();
nh.advertise(button_publisher);
while (1)
{
uint8_t button_debounced_state = ButtonsPoll(&button_debounced_delta, &button_raw_state);
// Publish message to be transmitted.
button_msg.sw1.data = button_debounced_state & LEFT_BUTTON;
button_msg.sw2.data = button_debounced_state & RIGHT_BUTTON;
button_publisher.publish(&button_msg);
// Handle all communications and callbacks.
nh.spinOnce();
// Delay for a bit.
nh.getHardware()->delay(100);
}
}
StellarisIO()
: magic(gMagic)
{
MAP_FPULazyStackingEnable();
/* Set clock to PLL at 50MHz */
MAP_SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_16MHZ);
MAP_SysTickPeriodSet(MAP_SysCtlClockGet() / (1000*10)); // 1us
MAP_SysTickIntEnable();
MAP_SysTickEnable();
}
int32_t main(void)
{
g_ui32SysClock = ROM_SysCtlClockFreqSet(SYSCTL_USE_PLL | SYSCTL_XTAL_25MHZ | SYSCTL_OSC_MAIN | SYSCTL_CFG_VCO_480, 120000000);
// Reset index for FFT data
inputIndex = 0;
MAP_FPULazyStackingEnable();
MAP_FPUEnable();
// Set up ADC sampling and interrupt
configureADC();
while(1); /* main function does not return */
}
void platform_init(void) {
/* Enable lazy stacking for interrupt handlers. This allows floating-point instructions to be used within interrupt
* handlers, but at the expense of extra stack usage. */
MAP_FPULazyStackingEnable();
/* Set clock to PLL at 50MHz */
MAP_SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN |
SYSCTL_XTAL_16MHZ);
/* Enable the GPIO pins for the LED (PF2 & PF3). */
MAP_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
MAP_GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_3|GPIO_PIN_2);
/* Enable the system tick. FIXME do we need this? */
MAP_SysTickPeriodSet(MAP_SysCtlClockGet() / SYSTICKS_PER_SECOND);
MAP_SysTickIntEnable();
MAP_SysTickEnable();
}
/**
* This function will initial LPC40xx board.
*/
void rt_hw_board_init()
{
MAP_IntMasterDisable();
IntRegister(FAULT_HARD, HardFault_Handler);
IntRegister(FAULT_PENDSV, PendSV_Handler);
IntRegister(FAULT_SYSTICK, SysTick_Handler);
//
// Enable lazy stacking for interrupt handlers. This allows floating-point
// instructions to be used within interrupt handlers, but at the expense of
// extra stack usage.
//
MAP_FPULazyStackingEnable();
//
// Set the clocking to run directly from the external crystal/oscillator.
// TODO: The SYSCTL_XTAL_ value must be changed to match the value of the
// crystal on your board.
//
SysClock = MAP_SysCtlClockFreqSet(
(SYSCTL_XTAL_25MHZ | SYSCTL_OSC_MAIN | SYSCTL_USE_PLL | SYSCTL_CFG_VCO_480),
SYS_CLOCK_DEFAULT);
MAP_SysTickDisable();
MAP_SysTickPeriodSet(SysClock/ RT_TICK_PER_SECOND - 1);
MAP_SysTickIntEnable();
MAP_SysTickEnable();
/* set pend exception priority */
//IntPrioritySet(FAULT_PENDSV, (1 << 5) - 1);
/*init uart device*/
rt_hw_uart_init();
//redirect RTT stdio to CONSOLE device
rt_console_set_device(RT_CONSOLE_DEVICE_NAME);
//
// Enable interrupts to the processor.
//
MAP_IntMasterEnable();
}
//*****************************************************************************
//
// Initialize and operate the data logger.
//
//*****************************************************************************
int
main(void)
{
tContext sDisplayContext, sBufferContext;
uint32_t ui32HibIntStatus, ui32SysClock, ui32LastTickCount;
bool bSkipSplash;
uint8_t ui8ButtonState, ui8ButtonChanged;
uint_fast8_t ui8X, ui8Y;
//
// Enable lazy stacking for interrupt handlers. This allows floating-point
// instructions to be used within interrupt handlers, but at the expense of
// extra stack usage.
//
MAP_FPULazyStackingEnable();
//
// Set the clocking to run at 50 MHz.
//
MAP_SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ |
SYSCTL_OSC_MAIN);
ui32SysClock = MAP_SysCtlClockGet();
//
// Initialize locals.
//
bSkipSplash = false;
ui32LastTickCount = 0;
//
// Initialize the data acquisition module. This initializes the ADC
// hardware.
//
AcquireInit();
//
// Enable access to the hibernate peripheral. If the hibernate peripheral
// was already running then this will have no effect.
//
MAP_SysCtlPeripheralEnable(SYSCTL_PERIPH_HIBERNATE);
//
// Check to see if the hiberate module is already active and if so then
// read the saved configuration state. If both are okay, then proceed
// to check and see if we are logging data using sleep mode.
//
if(HibernateIsActive() && !GetSavedState(&g_sConfigState))
{
//
// Read the status of the hibernate module.
//
ui32HibIntStatus = HibernateIntStatus(1);
//
// If this is a pin wake, that means the user pressed the select
// button and we should terminate the sleep logging. In this case
// we will fall out of this conditional section, and go through the
// normal startup below, but skipping the splash screen so the user
// gets immediate response.
//
if(ui32HibIntStatus & HIBERNATE_INT_PIN_WAKE)
{
//
// Clear the interrupt flag so it is not seen again until another
// wake.
//
HibernateIntClear(HIBERNATE_INT_PIN_WAKE);
bSkipSplash = true;
}
//
// Otherwise if we are waking from hibernate and it was not a pin
// wake, then it must be from RTC match. Check to see if we are
// sleep logging and if so then go through an abbreviated startup
// in order to collect the data and go back to sleep.
//
else if(g_sConfigState.ui32SleepLogging &&
(ui32HibIntStatus & HIBERNATE_INT_RTC_MATCH_0))
{
//
// Start logger and pass the configuration. The logger should
// configure itself to take one sample.
//
AcquireStart(&g_sConfigState);
g_iLoggerState = eSTATE_LOGGING;
//
// Enter a forever loop to run the acquisition. This will run
// until a new sample has been taken and stored.
//
while(!AcquireRun())
{
}
//
// Getting here means that a data acquisition was performed and we
// can now go back to sleep. Save the configuration and then
// activate the hibernate.
//
SetSavedState(&g_sConfigState);
//
// Set wake condition on pin-wake or RTC match. Then put the
// processor in hibernation.
//
HibernateWakeSet(HIBERNATE_WAKE_PIN | HIBERNATE_WAKE_RTC);
HibernateRequest();
//
// Hibernating takes a finite amount of time to occur, so wait
// here forever until hibernate activates and the processor
// power is removed.
//
for(;;)
{
}
}
//
// Otherwise, this was not a pin wake, and we were not sleep logging,
// so just fall out of this conditional and go through the normal
// startup below.
//
}
else
{
//
// In this case, either the hibernate module was not already active, or
// the saved configuration was not valid. Initialize the configuration
// to the default state and then go through the normal startup below.
//
GetDefaultState(&g_sConfigState);
}
//
// Enable the Hibernate module to run.
//
HibernateEnableExpClk(SysCtlClockGet());
//
// The hibernate peripheral trim register must be set per silicon
// erratum 2.1
//
HibernateRTCTrimSet(0x7FFF);
//
// Start the RTC running. If it was already running then this will have
// no effect.
//
HibernateRTCEnable();
//
// In case we were sleep logging and are now finished (due to user
// pressing select button), then disable sleep logging so it doesnt
// try to start up again.
//
g_sConfigState.ui32SleepLogging = 0;
SetSavedState(&g_sConfigState);
//
// Initialize the display driver.
//
CFAL96x64x16Init();
//
// Initialize the buttons driver.
//
ButtonsInit();
//
// Pass the restored state to the menu system.
//
MenuSetState(&g_sConfigState);
//
// Enable the USB peripheral
//
MAP_SysCtlPeripheralEnable(SYSCTL_PERIPH_USB0);
//
// Configure the required pins for USB operation.
//
MAP_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
MAP_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOG);
MAP_GPIOPinConfigure(GPIO_PG4_USB0EPEN);
MAP_GPIOPinTypeUSBDigital(GPIO_PORTG_BASE, GPIO_PIN_4);
MAP_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOL);
MAP_GPIOPinTypeUSBAnalog(GPIO_PORTL_BASE, GPIO_PIN_6 | GPIO_PIN_7);
MAP_GPIOPinTypeUSBAnalog(GPIO_PORTB_BASE, GPIO_PIN_0 | GPIO_PIN_1);
//
// Erratum workaround for silicon revision A1. VBUS must have pull-down.
//
if(CLASS_IS_BLIZZARD && REVISION_IS_A1)
{
HWREG(GPIO_PORTB_BASE + GPIO_O_PDR) |= GPIO_PIN_1;
}
//
// Initialize the USB stack mode and pass in a mode callback.
//
USBStackModeSet(0, eUSBModeOTG, ModeCallback);
//
// Initialize the stack to be used with USB stick.
//
USBStickInit();
//
// Initialize the stack to be used as a serial device.
//
USBSerialInit();
//
// Initialize the USB controller for dual mode operation with a 2ms polling
// rate.
//
USBOTGModeInit(0, 2000, g_pui8HCDPool, HCD_MEMORY_SIZE);
//
// Initialize the menus module. This module will control the user
// interface menuing system.
//
MenuInit(WidgetActivated);
//
// Configure SysTick to periodically interrupt.
//
g_ui32TickCount = 0;
MAP_SysTickPeriodSet(ui32SysClock / CLOCK_RATE);
MAP_SysTickIntEnable();
MAP_SysTickEnable();
//
// Initialize the display context and another context that is used
// as an offscreen drawing buffer for display animation effect
//
GrContextInit(&sDisplayContext, &g_sCFAL96x64x16);
GrContextInit(&sBufferContext, &g_sOffscreenDisplayA);
//
// Show the splash screen if we are not skipping it. The only reason to
// skip it is if the application was in sleep-logging mode and the user
// just waked it up with the select button.
//
if(!bSkipSplash)
{
const uint8_t *pui8SplashLogo = g_pui8Image_TI_Black;
//
// Draw the TI logo on the display. Use an animation effect where the
// logo will "slide" onto the screen. Allow select button to break
// out of animation.
//
for(ui8X = 0; ui8X < 96; ui8X++)
{
if(ButtonsPoll(0, 0) & SELECT_BUTTON)
{
break;
}
GrImageDraw(&sDisplayContext, pui8SplashLogo, 95 - ui8X, 0);
}
//
// Leave the logo on the screen for a long duration. Monitor the
// buttons so that if the user presses the select button, the logo
// display is terminated and the application starts immediately.
//
while(g_ui32TickCount < 400)
{
if(ButtonsPoll(0, 0) & SELECT_BUTTON)
{
break;
}
}
//
// Extended splash sequence
//
if(ButtonsPoll(0, 0) & UP_BUTTON)
{
for(ui8X = 0; ui8X < 96; ui8X += 4)
{
GrImageDraw(&sDisplayContext,
g_ppui8Image_Splash[(ui8X / 4) & 3],
(int32_t)ui8X - 96L, 0);
GrImageDraw(&sDisplayContext, pui8SplashLogo, ui8X, 0);
MAP_SysCtlDelay(ui32SysClock / 12);
}
MAP_SysCtlDelay(ui32SysClock / 3);
pui8SplashLogo = g_ppui8Image_Splash[4];
GrImageDraw(&sDisplayContext, pui8SplashLogo, 0, 0);
MAP_SysCtlDelay(ui32SysClock / 12);
}
//
// Draw the initial menu into the offscreen buffer.
//
SlideMenuDraw(&g_sMenuWidget, &sBufferContext, 0);
//
// Now, draw both the TI logo splash screen (from above) and the initial
// menu on the screen at the same time, moving the coordinates so that
// the logo "slides" off the display and the menu "slides" onto the
// display.
//
for(ui8Y = 0; ui8Y < 64; ui8Y++)
{
GrImageDraw(&sDisplayContext, pui8SplashLogo, 0, -ui8Y);
GrImageDraw(&sDisplayContext, g_pui8OffscreenBufA, 0, 63 - ui8Y);
}
}
//
// Add the menu widget to the widget tree and send an initial paint
// request.
//
WidgetAdd(WIDGET_ROOT, (tWidget *)&g_sMenuWidget);
WidgetPaint(WIDGET_ROOT);
//
// Set the focus handle to the menu widget. Any button events will be
// sent to this widget
//
g_ui32KeyFocusWidgetHandle = (uint32_t)&g_sMenuWidget;
//
// Forever loop to run the application
//
while(1)
{
//
// Each time the timer tick occurs, process any button events.
//
if(g_ui32TickCount != ui32LastTickCount)
{
//
// Remember last tick count
//
ui32LastTickCount = g_ui32TickCount;
//
// Read the debounced state of the buttons.
//
ui8ButtonState = ButtonsPoll(&ui8ButtonChanged, 0);
//
// Pass any button presses through to the widget message
// processing mechanism. The widget that has the button event
// focus (probably the menu widget) will catch these button events.
//
if(BUTTON_PRESSED(SELECT_BUTTON, ui8ButtonState, ui8ButtonChanged))
{
SendWidgetKeyMessage(WIDGET_MSG_KEY_SELECT);
}
if(BUTTON_PRESSED(UP_BUTTON, ui8ButtonState, ui8ButtonChanged))
{
SendWidgetKeyMessage(WIDGET_MSG_KEY_UP);
}
if(BUTTON_PRESSED(DOWN_BUTTON, ui8ButtonState, ui8ButtonChanged))
{
SendWidgetKeyMessage(WIDGET_MSG_KEY_DOWN);
}
if(BUTTON_PRESSED(LEFT_BUTTON, ui8ButtonState, ui8ButtonChanged))
{
SendWidgetKeyMessage(WIDGET_MSG_KEY_LEFT);
}
if(BUTTON_PRESSED(RIGHT_BUTTON, ui8ButtonState, ui8ButtonChanged))
{
SendWidgetKeyMessage(WIDGET_MSG_KEY_RIGHT);
}
}
//
// Tell the OTG library code how much time has passed in milliseconds
// since the last call.
//
USBOTGMain(GetTickms());
//
// Call functions as needed to keep the host or device mode running.
//
if(g_iCurrentUSBMode == eUSBModeDevice)
{
USBSerialRun();
}
else if(g_iCurrentUSBMode == eUSBModeHost)
{
USBStickRun();
}
//
// If in the logging state, then call the logger run function. This
// keeps the data acquisition running.
//
if((g_iLoggerState == eSTATE_LOGGING) ||
(g_iLoggerState == eSTATE_VIEWING))
{
if(AcquireRun() && g_sConfigState.ui32SleepLogging)
{
//
// If sleep logging is enabled, then at this point we have
// stored the first data item, now save the state and start
// hibernation. Wait for the power to be cut.
//
SetSavedState(&g_sConfigState);
HibernateWakeSet(HIBERNATE_WAKE_PIN | HIBERNATE_WAKE_RTC);
HibernateRequest();
for(;;)
{
}
}
//
// If viewing instead of logging then request a repaint to keep
// the viewing window updated.
//
if(g_iLoggerState == eSTATE_VIEWING)
{
WidgetPaint(WIDGET_ROOT);
}
}
//
// If in the saving state, then save data from flash storage to
// USB stick.
//
if(g_iLoggerState == eSTATE_SAVING)
{
//
// Save data from flash to USB
//
FlashStoreSave();
//
// Return to idle state
//
g_iLoggerState = eSTATE_IDLE;
}
//
// If in the erasing state, then erase the data stored in flash.
//
if(g_iLoggerState == eSTATE_ERASING)
{
//
// Save data from flash to USB
//
FlashStoreErase();
//
// Return to idle state
//
g_iLoggerState = eSTATE_IDLE;
}
//
// If in the flash reporting state, then show the report of the amount
// of used and free flash memory.
//
if(g_iLoggerState == eSTATE_FREEFLASH)
{
//
// Report free flash space
//
FlashStoreReport();
//
// Return to idle state
//
g_iLoggerState = eSTATE_IDLE;
}
//
// If we are exiting the clock setting widget, that means that control
// needs to be given back to the menu system.
//
if(g_iLoggerState == eSTATE_CLOCKEXIT)
{
//
// Give the button event focus back to the menu system
//
g_ui32KeyFocusWidgetHandle = (uint32_t)&g_sMenuWidget;
//
// Send a button event to the menu widget that means the left
// key was pressed. This signals the menu widget to deactivate
// the current child widget (which was the clock setting wigdet).
// This will cause the menu widget to slide the clock set widget
// off the screen and resume control of the display.
//
SendWidgetKeyMessage(WIDGET_MSG_KEY_LEFT);
g_iLoggerState = eSTATE_IDLE;
}
//
// Process any new messages that are in the widget queue. This keeps
// the user interface running.
//
WidgetMessageQueueProcess();
}
}
int main()
{
//
// Enable lazy stacking for interrupt handlers. This allows floating-point
// instructions to be used within interrupt handlers, but at the expense of
// extra stack usage.
//
MAP_FPUEnable();
MAP_FPULazyStackingEnable();
//
// Set the clocking to run from the PLL at 50MHZ, change to SYSCTL_SYSDIV_2_5 for 80MHz if neeeded
//
MAP_SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_16MHZ);
MAP_IntMasterDisable();
//
// Initialize the SW2 button
//
MAP_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
//
// Unlock PF0 so we can change it to a GPIO input
// Once we have enabled (unlocked) the commit register then re-lock it
// to prevent further changes. PF0 is muxed with NMI thus a special case.
//
HWREG(GPIO_PORTF_BASE + GPIO_O_LOCK) = GPIO_LOCK_KEY;
HWREG(GPIO_PORTF_BASE + GPIO_O_CR) |= 0x01;
HWREG(GPIO_PORTF_BASE + GPIO_O_LOCK) = 0;
MAP_GPIOPinTypeGPIOInput(GPIO_PORTF_BASE, GPIO_PIN_0);
MAP_GPIOPadConfigSet(GPIO_PORTF_BASE, GPIO_PIN_0, GPIO_STRENGTH_2MA , GPIO_PIN_TYPE_STD_WPU);
//
// Initialize the communication layer with a default UART setting
//
AbstractComm * comm = NULL;
AbstractComm * uart_comm = new UARTComm();
purpinsComm communication(uart_comm);
//
// Initialize the robot object
//
purpinsRobot purpins;
//
// Initialize the EEPROM
//
purpinsSettings settings;
//
// Initialize the MPU6050-based IMU
//
mpudata_t mpu;
//unsigned long sample_rate = 10 ;
//mpu9150_init(0, sample_rate, 0);
memset(&mpu, 0, sizeof(mpudata_t));
//
// Get the current processor clock frequency.
//
ulClockMS = MAP_SysCtlClockGet() / (3 * 1000);
//unsigned long loop_delay = (1000 / sample_rate) - 2;
//
// Configure SysTick to occur 1000 times per second
//
MAP_SysTickPeriodSet(MAP_SysCtlClockGet() / SYSTICKS_PER_SECOND);
MAP_SysTickIntEnable();
MAP_SysTickEnable();
MAP_IntMasterEnable();
//
// Load the settings from EEPROM
//
// Load the robot's ID
settings.get(PP_SETTING_ID, &id, sizeof(uint32_t));
// Load the motors' PID gains
settings.get(PP_SETTING_LEFT_PID, purpins.leftPIDGains(), sizeof(PIDGains));
settings.get(PP_SETTING_RIGHT_PID, purpins.rightPIDGains(), sizeof(PIDGains));
// Load the communication type
uint32_t comm_type;
settings.get(PP_SETTING_COMM_TYPE, &comm_type, sizeof(uint32_t));
// Holding SW2 at startup forces USB Comm
if(comm_type != PP_COMM_TYPE_USB && MAP_GPIOPinRead(GPIO_PORTF_BASE, GPIO_PIN_0) == 1)
{
if(comm_type == PP_COMM_TYPE_CC3000)
{
comm = new WiFiComm();
// Load network and server settings
settings.get(PP_SETTING_NETWORK_DATA, (static_cast<WiFiComm*>(comm))->network(), sizeof(Network));
settings.get(PP_SETTING_SERVER_DATA, (static_cast<WiFiComm*>(comm))->server(), sizeof(Server));
// Set WiFiComm as the underlying communication layer
communication.setCommLayer(comm);
}
else if(comm_type == PP_COMM_TYPE_ESP8266)
{
// TODO
}
else if(comm_type == PP_COMM_TYPE_XBEE)
{
// TODO
}
}
//
// Communication variables and stuff
//
uint8_t sensors_pack[SENSORS_PACK_SIZE];
bool send_pack = false;
unsigned long sensor_streaming_millis = 0;
unsigned long last_millis = millis();
uint8_t data[SERIAL_BUFFER_SIZE];
size_t data_size;
uint8_t action;
//
// Main loop, just managing communications
//
while(1)
{
//
// Check for a new action
//
action = communication.getMsg(data, data_size);
// If we got an action...
if(action > 0)
{
// Process it!!!
if(action == PP_ACTION_GET_VERSION)
{
uint8_t version = VERSION;
communication.sendMsg(PP_ACTION_GET_VERSION, (void*)(&version), 1);
}
else if(action == PP_ACTION_DRIVE)
{
RobotSpeed speed;
memcpy(&speed, data, sizeof(speed));
purpins.setSpeed(&speed);
communication.sendAck(PP_ACTION_DRIVE);
}
else if(action == PP_ACTION_DRIVE_MOTORS)
{
MotorSpeeds motor_speeds;
memcpy(&motor_speeds, data, sizeof(motor_speeds));
purpins.setMotorSpeeds(&motor_speeds);
communication.sendAck(PP_ACTION_DRIVE_MOTORS);
}
else if(action == PP_ACTION_DRIVE_PWM)
{
MotorPWMs motor_pwms;
memcpy(&motor_pwms, data, sizeof(motor_pwms));
purpins.setPWM(&motor_pwms);
communication.sendAck(PP_ACTION_DRIVE_PWM);
}
else if(action == PP_ACTION_GET_ODOMETRY)
{
Pose odometry;
purpins.getOdometry(&odometry);
communication.sendMsg(PP_ACTION_GET_ODOMETRY, (void*)(&odometry), sizeof(odometry));
}
else if(action == PP_ACTION_GET_MOTOR_SPEEDS)
{
MotorSpeeds speed;
purpins.getMotorSpeeds(&speed);
communication.sendMsg(PP_ACTION_GET_MOTOR_SPEEDS, (void*)(&speed), sizeof(speed));
}
else if(action == PP_ACTION_GET_ENCODER_PULSES)
{
EncoderPulses encoder_pulses;
purpins.getEncoderTicks(&encoder_pulses);
communication.sendMsg(PP_ACTION_GET_ENCODER_PULSES, (void*)(&encoder_pulses), sizeof(encoder_pulses));
}
else if(action == PP_ACTION_GET_IMU)
{
IMU imu_data;
imu_data.orientation_x = mpu.fusedQuat[0];
imu_data.orientation_y = mpu.fusedQuat[1];
imu_data.orientation_z = mpu.fusedQuat[2];
imu_data.orientation_w = mpu.fusedQuat[4];
imu_data.linear_acceleration_x = mpu.calibratedAccel[0];
imu_data.linear_acceleration_y = mpu.calibratedAccel[1];
imu_data.linear_acceleration_z = mpu.calibratedAccel[2];
imu_data.angular_velocity_x = mpu.calibratedMag[0];
imu_data.angular_velocity_y = mpu.calibratedMag[1];
imu_data.angular_velocity_z = mpu.calibratedMag[2];
communication.sendMsg(PP_ACTION_GET_IMU, (void*)(&imu_data), sizeof(imu_data));
}
else if(action == PP_ACTION_GET_IR_SENSORS)
{
// TODO: Add the IR sensors
}
else if(action == PP_ACTION_GET_GAS_SENSOR)
{
// TODO: Add the gas sensor
}
else if(action == PP_ACTION_SET_SENSORS_PACK)
{
bool ok = true;
memset(sensors_pack, 0, SENSORS_PACK_SIZE);
if(data_size > SENSORS_PACK_SIZE)
{
communication.error(PP_ERROR_BUFFER_SIZE);
ok = false;
break;
}
for(unsigned int i=0 ; i<data_size ; i++)
{
if(data[i] < PP_ACTION_GET_ODOMETRY || data[i] > PP_ACTION_GET_GAS_SENSOR)
{
communication.error(PP_ERROR_SENSOR_UNKNOWN);
ok = false;
break;
}
}
if(ok)
{
memcpy(sensors_pack, data, data_size);
communication.sendAck(PP_ACTION_SET_SENSORS_PACK);
}
}
else if(action == PP_ACTION_GET_SENSORS_PACK)
{
send_pack = true;
}
else if(action == PP_ACTION_SET_SENSOR_STREAMING)
{
float sensor_streaming_frequency;
memcpy(&sensor_streaming_frequency, data, sizeof(sensor_streaming_frequency));
sensor_streaming_millis = 1/sensor_streaming_frequency * 1000;
communication.sendAck(PP_ACTION_SET_SENSOR_STREAMING);
}
else if(action == PP_ACTION_SET_GLOBAL_POSE)
{
Pose pose;
memcpy(&pose, data, sizeof(pose));
purpins.setGlobalPose(&pose);
communication.sendAck(PP_ACTION_SET_GLOBAL_POSE);
}
else if(action == PP_ACTION_SET_NEIGHBORS_POSES)
{
// TODO: Finish this
}
else if(action == PP_ACTION_SET_ID)
{
memcpy(&id, data, sizeof(id));
settings.save(PP_SETTING_ID, data, sizeof(uint32_t));
communication.sendAck(PP_ACTION_SET_ID);
}
else if(action == PP_ACTION_GET_ID)
{
communication.sendMsg(PP_ACTION_GET_ID, (void*)(&id), sizeof(id));
}
else if(action == PP_ACTION_SET_PID_GAINS)
{
memcpy(purpins.leftPIDGains(), data, sizeof(PIDGains));
memcpy(purpins.rightPIDGains(), data+sizeof(PIDGains), sizeof(PIDGains));
settings.save(PP_SETTING_LEFT_PID, data, sizeof(PIDGains));
settings.save(PP_SETTING_RIGHT_PID, data+sizeof(PIDGains), sizeof(PIDGains));
communication.sendAck(PP_ACTION_SET_PID_GAINS);
}
else if(action == PP_ACTION_GET_PID_GAINS)
{
memcpy(data, purpins.leftPIDGains(), sizeof(PIDGains));
memcpy(data+sizeof(PIDGains), purpins.rightPIDGains(), sizeof(PIDGains));
communication.sendMsg(PP_ACTION_GET_ID, (void*)(data), 2*sizeof(PIDGains));
}
else if(action == PP_ACTION_SET_SERVER_DATA)
{
settings.save(PP_SETTING_SERVER_DATA, data, sizeof(Server));
communication.sendAck(PP_ACTION_SET_SERVER_DATA);
}
else if(action == PP_ACTION_GET_SERVER_DATA)
{
settings.get(PP_SETTING_SERVER_DATA, data, sizeof(Server));
communication.sendMsg(PP_ACTION_GET_SERVER_DATA, (void*)(data), sizeof(Server));
}
else if(action == PP_ACTION_SET_NETWORK_DATA)
{
settings.save(PP_SETTING_NETWORK_DATA, data, sizeof(Network));
communication.sendAck(PP_ACTION_SET_NETWORK_DATA);
}
else if(action == PP_ACTION_GET_NETWORK_DATA)
{
settings.get(PP_SETTING_NETWORK_DATA, data, sizeof(Network));
communication.sendMsg(PP_ACTION_GET_NETWORK_DATA, (void*)(data), sizeof(Network));
}
else if(action == PP_ACTION_SET_COMM_TYPE)
{
memcpy(&comm_type, data, sizeof(comm_type));
if(comm_type != communication.type())
{
if(comm_type == PP_COMM_TYPE_USB)
{
communication.setCommLayer(uart_comm);
if(comm != NULL) delete comm;
}
if(comm_type == PP_COMM_TYPE_CC3000)
{
if(comm != NULL) delete comm;
comm = new WiFiComm();
// Load network and server settings
settings.get(PP_SETTING_NETWORK_DATA, (static_cast<WiFiComm*>(comm))->network(), sizeof(Network));
settings.get(PP_SETTING_SERVER_DATA, (static_cast<WiFiComm*>(comm))->server(), sizeof(Server));
// Set WiFiComm as the underlying communication layer
communication.setCommLayer(comm);
}
else if(comm_type == PP_COMM_TYPE_ESP8266)
{
// TODO
}
else if(comm_type == PP_COMM_TYPE_XBEE)
{
// TODO
}
else
{
communication.error(PP_ERROR_COMM_UNKNOWN);
}
communication.sendAck(PP_ACTION_SET_COMM_TYPE);
}
}
} // if(action > 0)
//
// Manage sensor streaming
//
unsigned long current_millis = millis();
if(send_pack || (sensor_streaming_millis > 0 && (current_millis - last_millis) >= sensor_streaming_millis))
{
size_t size = 0;
for(int i=0 ; i<SENSORS_PACK_SIZE ; i++)
{
if(sensors_pack[i] == 0) break;
if(sensors_pack[i] == PP_ACTION_GET_ODOMETRY)
{
Pose odometry;
purpins.getOdometry(&odometry);
memcpy(data+size, &odometry, sizeof(odometry));
size += sizeof(odometry);
}
else if(sensors_pack[i] == PP_ACTION_GET_MOTOR_SPEEDS)
{
MotorSpeeds speed;
purpins.getMotorSpeeds(&speed);
memcpy(data+size, &speed, sizeof(speed));
size += sizeof(speed);
}
else if(sensors_pack[i] == PP_ACTION_GET_ENCODER_PULSES)
{
EncoderPulses encoder_pulses;
purpins.getEncoderTicks(&encoder_pulses);
memcpy(data+size, &encoder_pulses, sizeof(encoder_pulses));
size += sizeof(encoder_pulses);
}
else if(sensors_pack[i] == PP_ACTION_GET_IMU)
{
IMU imu_data;
imu_data.orientation_x = mpu.fusedQuat[0];
imu_data.orientation_y = mpu.fusedQuat[1];
imu_data.orientation_z = mpu.fusedQuat[2];
imu_data.orientation_w = mpu.fusedQuat[4];
imu_data.linear_acceleration_x = mpu.calibratedAccel[0];
imu_data.linear_acceleration_y = mpu.calibratedAccel[1];
imu_data.linear_acceleration_z = mpu.calibratedAccel[2];
imu_data.angular_velocity_x = mpu.calibratedMag[0];
imu_data.angular_velocity_y = mpu.calibratedMag[1];
imu_data.angular_velocity_z = mpu.calibratedMag[2];
memcpy(data+size, &imu_data, sizeof(imu_data));
size += sizeof(imu_data);
}
else if(sensors_pack[i] == PP_ACTION_GET_IR_SENSORS)
{
// TODO: Add the IR sensors
}
else if(sensors_pack[i] == PP_ACTION_GET_GAS_SENSOR)
{
// TODO: Add the gas sensor
}
}
communication.sendMsg(PP_ACTION_GET_SENSORS_PACK, (void*)(data), size);
last_millis = current_millis;
send_pack = false;
} // if(send_pack ...
} // while(1)
return 0;
}
//*****************************************************************************
//
// The program main function. It performs initialization, then runs a command
// processing loop to read commands from the console.
//
//*****************************************************************************
int
main(void)
{
// int nStatus;
FRESULT iFResult;
// tRectangle sRect;
//
// Enable the GPIO port that is used for the on-board LED.
//
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPION);
//
// Check if the peripheral access is enabled.
//
while(!SysCtlPeripheralReady(SYSCTL_PERIPH_GPION))
{
}
//
// Enable the GPIO pin for the LED (PN0). Set the direction as output, and
// enable the GPIO pin for digital function.
//
GPIOPinTypeGPIOOutput(GPIO_PORTN_BASE, GPIO_PIN_0);
volatile uint32_t ui32Loop;
int counter = 2;
//
// Enable lazy stacking for interrupt handlers. This allows floating-point
// instructions to be used within interrupt handlers, but at the expense of
// extra stack usage.
//
MAP_FPULazyStackingEnable();
//
// Set the system clock to run at 50MHz from the PLL.
//
g_ui32SysClock = SysCtlClockFreqSet (SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN |
SYSCTL_XTAL_16MHZ,50000000);
//
// Enable the peripherals used by this example.
//
MAP_SysCtlPeripheralEnable(SYSCTL_PERIPH_SSI3);
//
// Configure SysTick for a 100Hz interrupt. The FatFs driver wants a 10 ms
// tick.
//
MAP_SysTickPeriodSet(g_ui32SysClock / 100);
MAP_SysTickEnable();
MAP_SysTickIntEnable();
//
// Enable Interrupts
//
MAP_IntMasterEnable();
// Mount the file system, using logical disk 0.
//
iFResult = f_mount(0, &g_sFatFs);
iFResult = f_open(&g_sFileObject, "testFile.txt", (FA_OPEN_ALWAYS | FA_WRITE));
while(counter > 0){
GPIOPinWrite(GPIO_PORTN_BASE, GPIO_PIN_0, GPIO_PIN_0);
for(ui32Loop = 0; ui32Loop < 200000; ui32Loop++)
{
}
//
// Turn off the LED.
//
GPIOPinWrite(GPIO_PORTN_BASE, GPIO_PIN_0, 0x0);
//
// Delay for a bit.
//
for(ui32Loop = 0; ui32Loop < 200000; ui32Loop++)
{
}
counter--;
}
if(iFResult != FR_OK)
{
return(1);
}
}
int main(){
unsigned long ulClockMS=0;
//
// Enable lazy stacking for interrupt handlers. This allows floating-point
// instructions to be used within interrupt handlers, but at the expense of
// extra stack usage.
//
MAP_FPUEnable();
MAP_FPULazyStackingEnable();
//
// Set the clocking to run directly from the crystal.
//
MAP_SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN |
SYSCTL_XTAL_16MHZ);
MAP_IntMasterDisable();
configureQEI();
configurePWM();
/*leftmotor_pid.setMaxOutput(255);
leftmotor_pid.setMinOutput(-255);
leftmotor_pid.setGains(12.0,5.0,0.0);
leftmotor_pid.setSampleTime(1.0/QEILOOPFREQUENCY);*/
leftmotor_pid.setInputLimits(-100,100);
leftmotor_pid.setOutputLimits(-255,255);
leftmotor_pid.setBias(0.0);
leftmotor_pid.setMode(AUTO_MODE);
configureUART();
MAP_IntMasterEnable();
// Get the current processor clock frequency.
ulClockMS = MAP_SysCtlClockGet() / (3 * 1000);
int char_counter=0;
char inputbuffer[10];
while(1){
inputbuffer[char_counter]=UARTgetc();
char_counter++;
if (char_counter==3){
inputbuffer[3]=0;
goal_vel=atoi(inputbuffer);
char_counter=0;
}
//uint32_t pos = MAP_QEIPositionGet(QEI1_BASE);
/*uint32_t vel = MAP_QEIVelocityGet(QEI1_BASE);
//UARTprintf("pos : %u \n",pos);QEIDirectionGet(QEI1_BASE)*/
/*UARTprintf("vel : %d pwm %d\n",vel,pwm_value);
MAP_SysCtlDelay(ulClockMS*50);*/
}
return 0;
}
// Initialize FreeRTOS and start the initial set of tasks.
int main(void)
{
// TivaC application specific code
MAP_FPUEnable();
MAP_FPULazyStackingEnable();
// Run from the PLL at 120 MHz.
MAP_SysCtlClockFreqSet((SYSCTL_XTAL_25MHZ | SYSCTL_OSC_MAIN |
SYSCTL_USE_PLL | SYSCTL_CFG_VCO_480), TM4C129FREQ);
// Make sure the main oscillator is enabled because this is required by
// the PHY. The system must have a 25MHz crystal attached to the OSC
// pins. The SYSCTL_MOSC_HIGHFREQ parameter is used when the crystal
// frequency is 10MHz or higher.
MAP_SysCtlMOSCConfigSet(SYSCTL_MOSC_HIGHFREQ);
MAP_IntEnable(FAULT_NMI);
MAP_IntEnable(FAULT_MPU);
MAP_IntEnable(FAULT_BUS);
MAP_IntEnable(FAULT_USAGE);
ConfigUART(115200);
UARTprintf("\n\nWelcome to the EK-TM4C129XL FreeRTOS Demo!\n");
// ROS nodehandle initialization and topic registration
nh.initNode("192.168.1.135");
// Start ROS spin task, responsible for handling callbacks and communications
if (spinInitTask(&nh))
{
UARTprintf("Couldn't create ROS spin task.\n");
while (1);
}
else
{
UARTprintf("Created ROS spin task.\n");
}
// Register and init subscribe task
if (subscribeInitTask(&nh))
{
UARTprintf("Couldn't create subscribe task.\n");
while (1);
}
else
{
UARTprintf("Created subscribe task.\n");
}
// Register and init publish task
if (publishInitTask(&nh))
{
UARTprintf("Couldn't create publish task.\n");
while (1);
}
else
{
UARTprintf("Created publish task.\n");
}
// Start the scheduler. This should not return.
UARTprintf("Starting scheduller.\n");
vTaskStartScheduler();
// In case the scheduler returns for some reason, print an error and loop forever.
while (1)
{
UARTprintf("Scheduler returned!\n");
}
}
void SetupFPU(void)
{
MAP_FPUEnable();
MAP_FPULazyStackingEnable();
}
