void sleepSeconds(uint32_t seconds)
{
unsigned long i;
i = milliseconds;
i += seconds * 1000;
stay_asleep = true;
HWREG(NVIC_SYS_CTRL) |= NVIC_SYS_CTRL_SLEEPDEEP;
while ( stay_asleep && (milliseconds < i) ) {
MAP_IntMasterDisable(); // Set PRIMASK so CPU wakes on IRQ but ISRs don't execute until PRIMASK is cleared
SysTickMode_DeepSleepCoarse();
CPUwfi_safe();
// Handle low-power SysTick triggers without using the default SysTickIntHandler
if (HWREG(NVIC_INT_CTRL) & NVIC_INT_CTRL_PENDSTSET) {
milliseconds += 1000;
HWREG(NVIC_INT_CTRL) |= NVIC_INT_CTRL_PENDSTCLR;
} else {
milliseconds += (DEEPSLEEP_CPU - HWREG(NVIC_ST_CURRENT)) / (DEEPSLEEP_CPU / 1000);
}
// Restore SysTick to normal parameters in preparation for full-speed ISR execution
SysTickMode_Run();
MAP_IntMasterEnable(); // Clearing PRIMASK allows pending ISRs to run
}
HWREG(NVIC_SYS_CTRL) &= ~(NVIC_SYS_CTRL_SLEEPDEEP);
stay_asleep = false;
}
//*****************************************************************************
//
// Change the value of a variable atomically.
//
// \param pulVal points to the index whose value is to be modified.
// \param ulDelta is the number of bytes to increment the index by.
// \param ulSize is the size of the buffer the index refers to.
//
// This function is used to increment a read or write buffer index that may be
// written in various different contexts. It ensures that the read/modify/write
// sequence is not interrupted and, hence, guards against corruption of the
// variable. The new value is adjusted for buffer wrap.
//
// \return None.
//
//*****************************************************************************
static void
UpdateIndexAtomic(volatile unsigned long *pulVal, unsigned long ulDelta,
unsigned long ulSize)
{
tBoolean bIntsOff;
//
// Turn interrupts off temporarily.
//
bIntsOff = MAP_IntMasterDisable();
//
// Update the variable value.
//
*pulVal += ulDelta;
//
// Correct for wrap. We use a loop here since we don't want to use a
// modulus operation with interrupts off but we don't want to fail in
// case ulDelta is greater than ulSize (which is extremely unlikely but...)
//
while(*pulVal >= ulSize)
{
*pulVal -= ulSize;
}
//
// Restore the interrupt state
//
if(!bIntsOff)
{
MAP_IntMasterEnable();
}
}
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;
}
void HardwareSerial::end()
{
unsigned long ulInt = MAP_IntMasterDisable();
flushAll();
/* If interrupts were enabled when we turned them off,
* turn them back on again. */
if(!ulInt) {
MAP_IntMasterEnable();
}
MAP_UARTIntDisable(UART_BASE, UART_INT_RT | UART_INT_TX);
MAP_UARTIntUnregister(UART_BASE);
}
void suspend(void)
{
stay_asleep = true;
HWREG(NVIC_SYS_CTRL) |= NVIC_SYS_CTRL_SLEEPDEEP;
while(stay_asleep) {
MAP_IntMasterDisable(); // Set PRIMASK so CPU wakes on IRQ but ISRs don't execute until PRIMASK is cleared
MAP_SysTickDisable(); // Halt SysTick during suspend mode - millis will no longer increment
CPUwfi_safe();
MAP_SysTickEnable(); // Re-enable SysTick before ISRs start (in case ISR uses millis/micros)
MAP_IntMasterEnable(); // Clearing PRIMASK allows pending ISRs to run
}
HWREG(NVIC_SYS_CTRL) &= ~(NVIC_SYS_CTRL_SLEEPDEEP);
}
//*****************************************************************************
//
//! Empties the ring buffer.
//!
//! \param ptRingBuf is the ring buffer object to empty.
//!
//! Discards all data from the ring buffer.
//!
//! \return None.
//
//*****************************************************************************
void
RingBufFlush(tRingBufObject *ptRingBuf)
{
tBoolean bIntsOff;
//
// Check the arguments.
//
ASSERT(ptRingBuf != NULL);
//
// Set the Read/Write pointers to be the same. Do this with interrupts
// disabled to prevent the possibility of corruption of the read index.
//
bIntsOff = MAP_IntMasterDisable();
ptRingBuf->ulReadIndex = ptRingBuf->ulWriteIndex;
if(!bIntsOff)
{
MAP_IntMasterEnable();
}
}
/**
* 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();
}
/*
* @brief Initialize the minimal amount of hardware for the bootloader to function
* @returns void
*/
void systemInit(void) {
uint32 i, j;
MAP_IntMasterDisable();
// increase LDO voltage so that PLL operates properly
MAP_SysCtlLDOSet(SYSCTL_LDO_2_75V);
#ifdef PART_LM3S8962
MAP_SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_8MHZ);
#endif
systemIDInit();
blinkyLedInit();
buttonsInit();
srand(roneID);
serialInit();
#if defined(RONE_V9) || defined(RONE_V12)
SPIInit();
radioInit();
#endif // defined(RONE_V9) || defined(RONE_V12)
// Triple blink blinky, startup signal
for (i = 0; i < 3; i++) {
blinkyLedSet(1);
for (j = 0; j < 150000;) {
j++;
}
blinkyLedSet(0);
for (j = 0; j < 250000;) {
j++;
}
}
// Initialize 24-bit Systick
SysTickPeriodSet(0xffffff);
SysTickEnable();
}
/**
* This function is used to lock access to critical sections when lwipopt.h
* defines SYS_LIGHTWEIGHT_PROT. It disables interrupts and returns a value
* indicating the interrupt enable state when the function entered. This
* value must be passed back on the matching call to sys_arch_unprotect().
*
* @return the interrupt level when the function was entered.
*/
sys_prot_t
sys_arch_protect(void)
{
return((sys_prot_t)MAP_IntMasterDisable());
}
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;
}
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;
}
//*****************************************************************************
//
//! Add bytes to the ring buffer by advancing the write index.
//!
//! \param ptRingBuf points to the ring buffer to which bytes have been added.
//! \param ulNumBytes is the number of bytes added to the buffer.
//!
//! This function should be used by clients who wish to add data to the buffer
//! directly rather than via calls to RingBufWrite() or RingBufWriteOne(). It
//! advances the write index by a given number of bytes. If the \e ulNumBytes
//! parameter is larger than the amount of free space in the buffer, the
//! read pointer will be advanced to cater for the addition. Note that this
//! will result in some of the oldest data in the buffer being discarded.
//!
//! \return None.
//
//*****************************************************************************
void
RingBufAdvanceWrite(tRingBufObject *ptRingBuf,
unsigned long ulNumBytes)
{
unsigned long ulCount;
tBoolean bIntsOff;
//
// Check the arguments.
//
ASSERT(ptRingBuf != NULL);
//
// Make sure we were not asked to add a silly number of bytes.
//
ASSERT(ulNumBytes <= ptRingBuf->ulSize);
//
// Determine how much free space we currently think the buffer has.
//
ulCount = RingBufFree(ptRingBuf);
//
// Advance the buffer write index by the required number of bytes and
// check that we have not run past the read index. Note that we must do
// this within a critical section (interrupts disabled) to prevent
// race conditions that could corrupt one or other of the indices.
//
bIntsOff = MAP_IntMasterDisable();
//
// Update the write pointer.
//
ptRingBuf->ulWriteIndex += ulNumBytes;
//
// Check and correct for wrap.
//
if(ptRingBuf->ulWriteIndex >= ptRingBuf->ulSize)
{
ptRingBuf->ulWriteIndex -= ptRingBuf->ulSize;
}
//
// Did the client add more bytes than the buffer had free space for?
//
if(ulCount < ulNumBytes)
{
//
// Yes - we need to advance the read pointer to ahead of the write
// pointer to discard some of the oldest data.
//
ptRingBuf->ulReadIndex = ptRingBuf->ulWriteIndex + 1;
//
// Correct for buffer wrap if necessary.
//
if(ptRingBuf->ulReadIndex >= ptRingBuf->ulSize)
{
ptRingBuf->ulReadIndex -= ptRingBuf->ulSize;
}
}
//
// Restore interrupts if we turned them off earlier.
//
if(!bIntsOff)
{
MAP_IntMasterEnable();
}
}
int main(void) {
MAP_IntVTableBaseSet((unsigned long) &int_vectors[0]);
MAP_IntMasterEnable();
PRCMCC3200MCUInit();
/* Console UART init. */
#ifndef NO_DEBUG
MAP_PRCMPeripheralClkEnable(DEBUG_UART_PERIPH, PRCM_RUN_MODE_CLK);
#if MIOT_DEBUG_UART == 0
MAP_PinTypeUART(PIN_55, PIN_MODE_3); /* UART0_TX */
MAP_PinTypeUART(PIN_57, PIN_MODE_3); /* UART0_RX */
#else
MAP_PinTypeUART(PIN_07, PIN_MODE_5); /* UART1_TX */
MAP_PinTypeUART(PIN_08, PIN_MODE_5); /* UART1_RX */
#endif
MAP_UARTConfigSetExpClk(
DEBUG_UART_BASE, MAP_PRCMPeripheralClockGet(DEBUG_UART_PERIPH),
MIOT_DEBUG_UART_BAUD_RATE,
(UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE));
MAP_UARTFIFOLevelSet(DEBUG_UART_BASE, UART_FIFO_TX1_8, UART_FIFO_RX4_8);
MAP_UARTFIFODisable(DEBUG_UART_BASE);
#endif
dbg_puts("\r\n\n");
if (sl_Start(NULL, NULL, NULL) < 0) abort();
dbg_putc('S');
int cidx = get_active_boot_cfg_idx();
if (cidx < 0) abort();
dbg_putc('0' + cidx);
struct boot_cfg cfg;
if (read_boot_cfg(cidx, &cfg) < 0) abort();
dbg_puts(cfg.app_image_file);
dbg_putc('@');
print_addr(cfg.app_load_addr);
/*
* Zero memory before loading.
* This should provide proper initialisation for BSS, wherever it is.
*/
uint32_t *pstart = (uint32_t *) 0x20000000;
uint32_t *pend = (&_text_start - 0x100 /* our stack */);
for (uint32_t *p = pstart; p < pend; p++) *p = 0;
if (load_image(cfg.app_image_file, (_u8 *) cfg.app_load_addr) != 0) {
abort();
}
dbg_putc('.');
sl_Stop(0);
print_addr(*(((uint32_t *) cfg.app_load_addr) + 1));
dbg_puts("\r\n\n");
MAP_IntMasterDisable();
MAP_IntVTableBaseSet(cfg.app_load_addr);
run(cfg.app_load_addr); /* Does not return. */
abort();
return 0; /* not reached */
}
// Tозвраает состояние регистра PRIMASK перед изменением.
// 1 - если прерvвания уже бvли вvкліченv до вvзова функции.
int tn_cpu_int_disable()
{
return MAP_IntMasterDisable();
}
