void init_Interrupts(void){
// Setup Interrupt for Light Sensor
light_setRange(LIGHT_RANGE_4000); // sensing up to 3892 lux
light_setIrqInCycles(LIGHT_CYCLE_1);
light_clearIrqStatus();
light_enable();
// Determine initial light conditions
uint32_t initial_light_value = light_read();
if (initial_light_value > LIGHTNING_THRESHOLD) {
// Initialize interrupt to trigger on falling below threshold
aboveThreshold = 1;
light_setLoThreshold(LIGHTNING_THRESHOLD);
light_setHiThreshold(RANGE_K2-1); // disable high threshold
} else {
// Initialize interrupt to trigger on exceeding threshold
aboveThreshold = 0;
light_setHiThreshold(LIGHTNING_THRESHOLD);
light_setLoThreshold(0); // disable low threshold
}
// clear interrupts before enabling
LPC_GPIOINT->IO2IntClr = 1 << 5;
LPC_GPIOINT->IO2IntClr = 1 << 10;
LPC_GPIOINT->IO0IntClr = 1 << 24;
LPC_GPIOINT->IO0IntClr = 1 << 25;
LPC_GPIOINT->IO0IntClr = 1 << 17;
LPC_GPIOINT->IO0IntClr = 1 << 15;
LPC_GPIOINT->IO0IntClr = 1 << 16;
LPC_GPIOINT->IO2IntClr = 1 << 3;
LPC_GPIOINT->IO2IntClr = 1 << 4;
LPC_GPIOINT->IO2IntEnF |= 1 << 5; // light sensor 3000 lux interrupt (P2.5)
LPC_GPIOINT->IO2IntEnF |= 1 << 10; // SW3 (P2.10)
LPC_GPIOINT->IO0IntEnF |= 1 << 24; // rotary switch right
LPC_GPIOINT->IO0IntEnF |= 1 << 25; // rotary switch left
// joystick
LPC_GPIOINT->IO0IntEnF |= 1 << 17; // center
LPC_GPIOINT->IO0IntEnF |= 1 << 15; // down
LPC_GPIOINT->IO0IntEnF |= 1 << 16; // right
LPC_GPIOINT->IO2IntEnF |= 1 << 3; // up
LPC_GPIOINT->IO2IntEnF |= 1 << 4; // left
enableTimer(RGB, 1000); // RGB will blink throughout operation at 1000ms interval
enableTimer(SAMPLING, 2000);
enableTimer(PCA9532, 250);
}
int parseArgument(char * arg) {
if (strcmp(arg, "--debug") == 0 || strcmp(arg, "-d") == 0) {
enableDebugMode();
return 0;
} else {
char * flag_name = strtok(arg, "=");
char * flag_value = strtok(NULL, "=");
int flag_intValue;
if (strcmp(flag_name, "--timer") == 0 || strcmp(flag_name, "-t") == 0) {
if (flag_value != NULL) flag_intValue = getInteger(flag_value);
if (flag_intValue >= 1 && flag_intValue <= 1024) {
enableTimer();
setTimeSlice(flag_intValue);
return 0;
} else if (flag_intValue != 0) {
printf("Invalid arguement %d to timer flag. Timer value should be between 0 and 1024\n", flag_intValue);
return -1;
} else {
disableTimer();
return 0;
}
} else {
printf("Invalid arguement %s", arg);
return -1;
}
}
}
int main(void)
{
// Setup the various pins
setup();
ledPinConfig();
switchPinConfig();
enableTimer();
while(1){
// Delay between cycles
}
}
uint8_t Servo::attach(int pin, int min, int max)
{
if(this->servoIndex < MAX_SERVOS ) {
pinMode( pin, OUTPUT) ; // set servo pin to output
servos[this->servoIndex].Pin.nbr = pin;
// todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128
this->min = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS
this->max = (MAX_PULSE_WIDTH - max)/4;
boolean timer_active = isTimerActive();
servos[this->servoIndex].Pin.isActive = true;
if (!timer_active)
enableTimer();
}
return this->servoIndex ;
}
// Handle an incoming message from the keyboard controller.
// Return TRUE if the message was handled, otherwise FALSE.
UtlBoolean PsButtonTask::handlePhoneMessage(PsMsg& rMsg)
{
int buttonId;
int index = 0;
PsButtonInfo* pButtonInfo = NULL;
PsPhoneTask* pPhoneTask;
UtlBoolean processed;
OsStatus res;
OsWriteLock lock(mMutex); // acquire a write lock
res = OS_SUCCESS;
if (mpButtonInfo == NULL) // if not yet initialized, just return
return res;
switch (rMsg.getMsg()) // for messages related to button presses
{ // get the corresponding button info
case PsMsg::BUTTON_UP:
case PsMsg::BUTTON_DOWN:
case PsMsg::BUTTON_REPEAT:
buttonId = rMsg.getParam2();
pButtonInfo = NULL;
for (index=0; index <= mMaxBtnIdx; index++)
{
if (mpButtonInfo[index].getId() == buttonId)
{
rMsg.setStringParam1(mpButtonInfo[index].getName());
break;
}
}
assert(index <= mMaxBtnIdx);
pButtonInfo = &mpButtonInfo[index];
break;
default:
break;
}
processed = TRUE;
pPhoneTask = PsPhoneTask::getPhoneTask();
switch (rMsg.getMsg())
{
case PsMsg::BUTTON_UP:
// printf("BUTTON_UP: buttonId = %d, buttonName = %s\n",
// buttonId, mpButtonInfo[index].getName());
pButtonInfo->setState(PsButtonInfo::UP); // set the button state
if (pButtonInfo->getEventMask() & PsButtonInfo::BUTTON_UP)
{ // post message to the phone set
res = pPhoneTask->postMessage(rMsg);
assert(res == OS_SUCCESS);
}
disableTimer(index); // disable the repeat timer (if any)
break;
case PsMsg::BUTTON_DOWN:
case PsMsg::BUTTON_REPEAT:
// printf("BUTTON_DOWN/BUTTON_REPEAT: buttonId = %d, buttonName = %s\n",
// buttonId, mpButtonInfo[index].getName());
pButtonInfo->setState(PsButtonInfo::DOWN); // set the button state
if (pButtonInfo->getEventMask() & PsButtonInfo::BUTTON_DOWN)
{ // post message to the phone set
res = pPhoneTask->postMessage(rMsg);
assert(res == OS_SUCCESS);
}
if (pButtonInfo->getEventMask() & PsButtonInfo::BUTTON_REPEAT)
{ // enable and arm the repeat timer
enableTimer(index);
}
break;
case PsMsg::BUTTON_GET_INFO:
break;
case PsMsg::BUTTON_SET_INFO:
res = pPhoneTask->postMessage(rMsg);
assert(res == OS_SUCCESS);
break;
case PsMsg::KEYPAD_STATE:
// printf("KEYPAD_STATE: isKeyUp = %d, buttonId = %d\n",
// rMsg.getParam1(), rMsg.getParam2());
break;
case PsMsg::SCROLL_CHANGE:
// printf("SCROLL_CHANGE: delta = %d\n", rMsg.getParam1());
break;
case PsMsg::SCROLL_STATE:
// printf("SCROLL_STATE: count = %d\n", rMsg.getParam1());
break;
case PsMsg::TOUCHSCREEN_CHANGE:
// printf("TOUCHSCREEN_CHANGE: x=%d, y=%d\n",
// rMsg.getParam1(), rMsg.getParam2());
break;
case PsMsg::TOUCHSCREEN_STATE:
// printf("TOUCHSCREEN_STATE: x=%d, y=%d\n",
// rMsg.getParam1(), rMsg.getParam2());
break;
default:
assert(FALSE);
processed = FALSE; // unexpected message
break;
}
if (rMsg.getSentFromISR()) // indicate that we are done with the msg
((PsMsg&) rMsg).setInUse(FALSE);
return processed;
}
void _defaultHandleRequest(Request& req, Response& resp, Delegate<void()> cb) {
if(unlikely(timerState<2)) enableTimer();
defaultHandleRequest(req,resp,cb);
}
inline void _requestReceived() {
if(unlikely(timerState<2)) enableTimer();
performanceCounters.totalRequestsReceived++;
}
/******************************************************************
* Main-function
*/
int main(void) {
/**************************************
* Set the clock to run at 80 MHz
*/
SysCtlClockSet(SYSCTL_SYSDIV_2_5|SYSCTL_USE_PLL|SYSCTL_XTAL_16MHZ|SYSCTL_OSC_MAIN);
/**************************************
* Enable the floating-point-unit
*/
FPUEnable();
// We also want Lazystacking, so enable that too
FPULazyStackingEnable();
// We also want to put numbers close to zero to zero
FPUFlushToZeroModeSet(FPU_FLUSH_TO_ZERO_EN);
/**************************************
* Init variables
*/
uint16_t mapData[MAP_DATA_SIZE];
uint8_t stepDir = 0;
/**************************************
* Init peripherals used
*/
bluetooth_init();
init_stepper(); // Init the GPIOs used for the stepper and loading LED
InitI2C1(); // Init the communication with the lidar-unit through I2C
InitPWM();
setupTimers();
/**************************************
* State 2
*/
// Disable the timers that are used
disableTimer(TIMER1_BASE);
disableTimer(TIMER2_BASE);
// Enable all interrupts
IntMasterEnable();
/**************************************
* State 3
*/
// Indicate we should start with a scan regardless of what other things we have already got
// from UART-interrupt
// This means setting the appropriate bit in the status vector
stat_vec |= TAKE_MEAS;
/**************************************
* State 4
*/
// Contains main-loop where decisions should be made
for ( ; ; ) {
/**********************************
* Decision tree
*/
// Highest priority case first
// Check both interrupts at each iteration in the loop
if ( int_vec & UART_INT ) {
// Reset the indication
int_vec &= ~UART_INT;
// Remove drive-stop flag to enable movement
stat_vec &= ~DRIVE_STOP;
// Init data array
uint8_t dataArr[MAX_UART_MSG_SIZE];
// Collect the message
if ( readUARTMessage(dataArr, MAX_UART_MSG_SIZE) < SUCCESS ) {
// If we have recieved more data than fits in the vector we should simply
// go in here again and grab data
int_vec |= UART_INT;
}
// We have gathered a message
// and now need to determine what the message is
parseMsg(dataArr, MAX_UART_MSG_SIZE);
}
// Checking drive (movement) interrupt
if ( int_vec & TIMER2_INT ) {
int_vec &= ~TIMER2_INT;
// Disable TIMER2
disableTimer(TIMER2_BASE);
// Set drive-stop in status vector
stat_vec |= DRIVE_STOP;
}
// Checking measure interrupt
if ( int_vec & TIMER1_INT ) {
int_vec &= ~TIMER1_INT;
// Disable TIMER1
disableTimer(TIMER1_BASE);
// Take reading from LIDAR
mapData[stepCount++] = readLidar();
SysCtlDelay(2000);
// Take step
// Note: We need to take double meas at randvillkor (100) !!!!!
if ( stepCount > 0 && stepCount < 100 ) {
stepDir = 1;
}
else if ( stepCount >= 100 && stepCount < 200) {
stepDir = 0;
}
else {
stepDir = 1;
stepCount = 0;
// Reset busy-flag
stat_vec &= ~TAKE_MEAS;
}
step = takeStep(step, stepDir);
// Request reading from LIDAR
reqLidarMeas();
if ( stat_vec & TAKE_MEAS ) {
// Restart TIMER1
enableTimer(TIMER1_BASE, (SYSCLOCK / MEASUREMENT_DELAY));
}
else {
sendUARTDataVector(mapData, MAP_DATA_SIZE);
stat_vec &= ~BUSY;
}
}
// Check the drive_stop flag, which always should be set unless we should move
if ( stat_vec & DRIVE_STOP ) {
// Stop all movement
SetPWMLevel(0,0);
halt();
// MAKE SURE all drive-flags are not set
stat_vec &= ~(DRIVE_F | DRIVE_L | DRIVE_R | DRIVE_LL | BUSY);
}
// Should we drive?
else if ( stat_vec & DRIVE ) {
// Remove drive flag
stat_vec &= ~DRIVE;
// Increase PWM
increase_PWM(0,MAX_FORWARD_SPEED,0,MAX_FORWARD_SPEED);
if ( stat_vec & DRIVE_F ) {
enableTimer(TIMER2_BASE, DRIVE_FORWARD_TIME);
}
else if ( stat_vec & DRIVE_LL ) {
enableTimer(TIMER2_BASE, DRIVE_TURN_180_TIME);
}
else {
enableTimer(TIMER2_BASE, DRIVE_TURN_TIME);
}
}
if ( !(stat_vec & BUSY) ) {
// Tasks
switch ( stat_vec ) {
case ((uint8_t)DRIVE_F) :
// Call drive function
go_forward();
// Set the drive flag & BUSY
stat_vec |= DRIVE | BUSY;
break;
case ((uint8_t)DRIVE_L) :
// Call drive-left function
go_left();
// Set the drive flag
stat_vec |= DRIVE | BUSY;
break;
case ((uint8_t)DRIVE_R) :
// Call drive-right function
go_right();
// Set the drive flag
stat_vec |= DRIVE | BUSY;
break;
case ((uint8_t)DRIVE_LL) :
// Call turn 180-degrees function
go_back();
// Set the drive flag
stat_vec |= DRIVE | BUSY;
break;
case ((uint8_t)TAKE_MEAS) :
// Request reading from LIDAR
reqLidarMeas();
// Start TIMER1
enableTimer(TIMER1_BASE, (SYSCLOCK / MEASUREMENT_DELAY)); // if sysclock = 1 s, 1/120 = 8.3 ms
// We are busy
stat_vec |= BUSY;
break;
default:
break;
}
}
}
}
int main( int argc, char *argv[] )
{
unsigned int start, stop;
int i = 0;
int test = 0;
int NUM_LOAD_THREADS = 0;
thread_arg_t arg;
hthread_mutexattr_t block_attr;
hthread_mutexattr_t data_attr;
benchmark_t my_benchmark[TESTS];
// Turn on the Xilinx Timer
enableTimer();
for (test = 0; test < TESTS; test++)
{
// Set the # of load threads
NUM_LOAD_THREADS = test * 10;
// Init. benchmark structure
my_benchmark[test].num_threads_in_test = NUM_LOAD_THREADS;
*my_benchmark[test].name = "lock/unlock";
for (i = 0; i < RUNS; i++)
{
// Initialize thread argument fields
// * Setup mutexes
// * Initialize counter
block_attr.num = 0;
block_attr.type = HTHREAD_MUTEX_DEFAULT;
data_attr.num = 1;
data_attr.type = HTHREAD_MUTEX_DEFAULT;
hthread_mutex_init(&arg.block_mutex, &block_attr);
hthread_mutex_init(&arg.data_mutex, &data_attr);
arg.counter = 0;
arg.num_load_threads = NUM_LOAD_THREADS;
// Run the test
start = readTimer();
run_A(0,NUM_LOAD_THREADS,&arg);
stop = readTimer();
// Gather results
my_benchmark[test].results1[i] = calc_timediff(arg.lock_start, arg.lock_stop);
my_benchmark[test].results2[i] = calc_timediff(arg.unlock_start, arg.unlock_stop);
/*
printf(
"*** start = %u\n"
"*** stop = %u\n"
"*** elapsed = %u\n",
start, stop, stop - start
);
// Calculate timing results
printf( "\t*********************************\n"
"\t lock Start Time = %llu ticks\n"
"\t lock Stop Time = %llu ticks\n"
"\t diff = %llu ticks\n"
"\t lock Elapsed Time = %llu us\n",
arg.lock_start, arg.lock_stop, arg.lock_stop - arg.lock_start, calc_timediff_us(arg.lock_start, arg.lock_stop)
);
printf( "\t*********************************\n"
"\t unlock Start Time = %llu ticks\n"
"\t unlock Stop Time = %llu ticks\n"
"\t diff = %llu ticks\n"
"\t unlock Elapsed Time = %llu us\n",
arg.unlock_start, arg.unlock_stop, arg.unlock_stop - arg.unlock_start, calc_timediff_us(arg.unlock_start, arg.unlock_stop)
); */
}
}
report_benchmark_results(my_benchmark);
for (test = 0; test < TESTS; test++)
{
// Set the # of load threads
NUM_LOAD_THREADS = test * 10;
// Init. benchmark structure
my_benchmark[test].num_threads_in_test = NUM_LOAD_THREADS;
*my_benchmark[test].name = "turn_around";
for (i = 0; i < RUNS; i++)
{
// Initialize thread argument fields
// * Setup mutexes
// * Initialize counter
block_attr.num = 0;
block_attr.type = HTHREAD_MUTEX_DEFAULT;
data_attr.num = 1;
data_attr.type = HTHREAD_MUTEX_DEFAULT;
hthread_mutex_init(&arg.block_mutex, &block_attr);
hthread_mutex_init(&arg.data_mutex, &data_attr);
arg.counter = 0;
// Num loads threads + 1 b/c the 2nd measurement thread is a load thread and needs to increment the counter barrier
// before the main thread releases the blocking mutex
arg.num_load_threads = NUM_LOAD_THREADS+1;
// Run the test
start = readTimer();
run_B(0,NUM_LOAD_THREADS,&arg);
stop = readTimer();
// Gather results
my_benchmark[test].results1[i] = calc_timediff(arg.unlock_start, arg.measure_lock_stop);
my_benchmark[test].results2[i] = calc_timediff(arg.unlock_start, arg.measure_lock_stop);
/*
printf(
"*** start = %u\n"
"*** stop = %u\n"
"*** elapsed = %u\n",
start, stop, stop - start
);
// Calculate timing results
printf( "\t*********************************\n"
"\t lock Start Time = %llu ticks\n"
"\t lock Stop Time = %llu ticks\n"
"\t diff = %llu ticks\n"
"\t lock Elapsed Time = %llu us\n",
arg.lock_start, arg.lock_stop, arg.lock_stop - arg.lock_start, calc_timediff_us(arg.lock_start, arg.lock_stop)
);
printf( "\t*********************************\n"
"\t unlock Start Time = %llu ticks\n"
"\t unlock Stop Time = %llu ticks\n"
"\t diff = %llu ticks\n"
"\t unlock Elapsed Time = %llu us\n"
"\t m_lock Start Time = %llu ticks\n"
"\t m_lock Stop Time = %llu ticks\n"
"\t turn around = %llu us\n",
arg.unlock_start, arg.unlock_stop, arg.unlock_stop - arg.unlock_start, calc_timediff_us(arg.unlock_start, arg.unlock_stop), arg.measure_lock_start, arg.measure_lock_stop, calc_timediff_us(arg.unlock_start, arg.measure_lock_stop)); */
}
}
report_benchmark_results(my_benchmark);
return 0;
}