/* * ======== Power_LF_clockFunc ======== */ Void Power_LF_clockFunc(UArg arg) { UInt32 sourceLF; /* query LF clock source */ sourceLF = OSCClockSourceGet(OSC_SRC_CLK_LF); /* is LF source either RCOSC_LF or XOSC_LF yet? */ if ((sourceLF == OSC_RCOSC_LF) || (sourceLF == OSC_XOSC_LF)) { /* yes, disable the LF clock qualifiers */ DDI16BitfieldWrite( AUX_DDI0_OSC_BASE, DDI_0_OSC_O_CTL0, DDI_0_OSC_CTL0_BYPASS_XOSC_LF_CLK_QUAL_M| DDI_0_OSC_CTL0_BYPASS_RCOSC_LF_CLK_QUAL_M, DDI_0_OSC_CTL0_BYPASS_RCOSC_LF_CLK_QUAL_S, 0x3 ); /* now finish by releasing the standby disallow constraint */ Power_releaseConstraint(Power_SB_DISALLOW); } /* not yet, LF still derived from HF, restart clock to check back later */ else { /* retrigger LF Clock to fire in 100 msec */ Clock_setTimeout( ti_sysbios_family_arm_cc26xx_Power_Module_State_lfClockObj(), (100000 / Clock_tickPeriod)); Clock_start( ti_sysbios_family_arm_cc26xx_Power_Module_State_lfClockObj()); } }
/********************************************************************* * @fn Util_startClock * * @brief Start a clock. * * @param pClock - pointer to clock struct * * @return none */ void Util_startClock(Clock_Struct *pClock) { Clock_Handle handle = Clock_handle(pClock); // Start clock instance Clock_start(handle); }
/* * ======== Clock_Instance_init ======== */ Void Clock_Instance_init(Clock_Object *obj, Clock_FuncPtr func, UInt timeout, const Clock_Params *params) { Queue_Handle clockQ; Assert_isTrue((BIOS_clockEnabled == TRUE), Clock_A_clockDisabled); Assert_isTrue(((BIOS_getThreadType() != BIOS_ThreadType_Hwi) && (BIOS_getThreadType() != BIOS_ThreadType_Swi)), Clock_A_badThreadType); Assert_isTrue(!(params->startFlag && (timeout == 0)), (Assert_Id)NULL); obj->timeout = timeout; obj->period = params->period; obj->fxn = func; obj->arg = params->arg; obj->active = FALSE; /* * Clock object is always placed on Clock work Q */ clockQ = Clock_Module_State_clockQ(); Queue_put(clockQ, &obj->elem); if (params->startFlag) { Clock_start(obj); } }
/********************************************************************* * @fn Util_rescheduleClock * * @brief Reschedule a clock by changing the timeout and period values. * * @param pClock - pointer to clock struct * @param clockPeriod - longevity of clock timer in milliseconds * @return none */ void Util_rescheduleClock(Clock_Struct *pClock, uint32_t clockPeriod) { bool running; uint32_t clockTicks; Clock_Handle handle; handle = Clock_handle(pClock); running = Clock_isActive(handle); if (running) { Clock_stop(handle); } // Convert period in milliseconds to ticks. clockTicks = clockPeriod * (1000 / Clock_tickPeriod); Clock_setTimeout(handle, clockTicks); Clock_setPeriod(handle, clockTicks); if (running) { Clock_start(handle); } }
/* * ======== main ======== */ Void main() { Swi_post(swi0); Swi_post(swi1); Clock_start(clk1); BIOS_start(); }
Int32 NullSrcLink_drvStart(NullSrcLink_Obj * pObj) { Clock_start(pObj->timer); #ifdef SYSTEM_DEBUG_NULL Vps_printf(" %d: NULL_SRC: Start Done !!!\n", Utils_getCurTimeInMsec()); #endif return FVID2_SOK; }
static Int32 IpcFramesInLink_reconfigPrdObj(IpcFramesInLink_Obj * pObj, UInt period) { UTILS_assert(pObj->prd.clkHandle != NULL); if (TRUE == pObj->prd.clkStarted) { Clock_stop(pObj->prd.clkHandle); } Clock_setPeriod(pObj->prd.clkHandle, 0); Clock_setTimeout(pObj->prd.clkHandle, period); Clock_start(pObj->prd.clkHandle); return IPC_FRAMES_IN_LINK_S_SUCCESS; }
void OneMsTaskTimer::start(uint32_t timer_index) { Clock_Params clockParams; Error_Block eb; Error_init(&eb); if (myClock == NULL){ Clock_Params_init(&clockParams); clockParams.period = (uint32_t)1000 / (uint64_t)Clock_tickPeriod; clockParams.startFlag = FALSE; clockParams.arg = (UArg)0x5555; myClock = Clock_create(OneMsTaskTimer_int, clockParams.period, &clockParams, &eb); } Clock_start(myClock); }
void LCD_Startup() { InitScibGpio(); scib_fifo_init(); // Init SCI-B // LCD_Reset(); LCD_Clear(); LCD_Backlight(30); Clock_start(LCD_CLOCK); LCD_S.init = 1; }
/* * ======== main ======== */ Void main() { Clock_Handle clk2; Clock_Params clkParams; /* Create a periodic Clock Instance with period = 5 system time units */ Clock_Params_init(&clkParams); clkParams.period = 5; clkParams.startFlag = TRUE; clkParams.arg = (UArg)0x5555; Clock_create(clk0Fxn, 5, &clkParams, NULL); /* Create an one-shot Clock Instance with timeout = 11 system time units */ clkParams.period = 0; clkParams.startFlag = FALSE; clkParams.arg = (UArg)0x6666; clk2 = Clock_create(clk1Fxn, 11, &clkParams, NULL); Clock_start(clk2); BIOS_start(); }
/********************************************************************* * @fn Util_restartClock * * @brief Restart a clock by changing the timeout. * * @param pClock - pointer to clock struct * @param clockTimeout - longevity of clock timer in milliseconds * * @return none */ void Util_restartClock(Clock_Struct *pClock, uint32_t clockTimeout) { uint32_t clockTicks; Clock_Handle handle; handle = Clock_handle(pClock); if (Clock_isActive(handle)) { // Stop clock first Clock_stop(handle); } // Convert timeout in milliseconds to ticks. clockTicks = clockTimeout * (1000 / Clock_tickPeriod); // Set the initial timeout Clock_setTimeout(handle, clockTicks); // Start clock instance Clock_start(handle); }
/* * ======== main ======== */ int main(void) { Clock_Handle clkHandle; Clock_Params clkParams; /* Call board init functions */ Board_initGeneral(); Board_initGPIO(); Board_initUART(); /* Turn on user LED */ GPIO_write(Board_LED0, Board_LED_OFF); GPIO_write(Board_LED1, Board_LED_ON); GPIOPinWrite(GPIO_PORTN_BASE, GPIO_PIN_0, GPIO_PIN_0); System_printf("Hello World\n"); /* SysMin will only print to the console when you call flush or exit */ System_flush(); /* Create a periodic Clock Instance with period = 5 system time units */ Clock_Params_init(&clkParams); clkParams.period = 1000; clkParams.startFlag = TRUE; Clock_create(clk0Fxn, 5, &clkParams, NULL); /* Create an one-shot Clock Instance with timeout = 11 system time units */ clkParams.period = 0; clkParams.startFlag = FALSE; clkHandle = Clock_create(clk1Fxn, 11, &clkParams, NULL); Clock_start(clkHandle); /* Start BIOS */ BIOS_start(); return (0); }
static Int32 IpcFramesInLink_startPrdObj(IpcFramesInLink_Obj * pObj, UInt period, Bool oneShotMode) { UTILS_assert(pObj->prd.clkHandle != NULL); if (FALSE == pObj->prd.clkStarted) { if (TRUE == oneShotMode) { Clock_setPeriod(pObj->prd.clkHandle, 0); } else { Clock_setPeriod(pObj->prd.clkHandle, period); } Clock_setTimeout(pObj->prd.clkHandle, period); Clock_start(pObj->prd.clkHandle); pObj->prd.clkStarted = TRUE; } return IPC_FRAMES_IN_LINK_S_SUCCESS; }
/********************************************************************* * @fn SensorTagIO_processCharChangeEvt * * @brief Process a change in the IO characteristics * * @return none */ void SensorTagIO_processCharChangeEvt(uint8_t paramID) { if( paramID == SENSOR_CONF ) { Io_getParameter(SENSOR_CONF, &ioMode); if (ioMode == IO_MODE_SELFTEST) { ioValue = sensorTestResult(); Io_setParameter(SENSOR_DATA, 1, &ioValue); } else { // Mode change: make sure LEDs and buzzer are off Io_setParameter(SENSOR_DATA, 1, &ioValue); PIN_setOutputValue(hGpioPin, Board_LED1, Board_LED_OFF); PIN_setOutputValue(hGpioPin, Board_LED2, Board_LED_OFF); Clock_stop(buzzClockHandle); PIN_setOutputValue(hGpioPin, Board_BUZZER, Board_BUZZER_OFF); } } else if (paramID == SENSOR_DATA) { Io_getParameter(SENSOR_DATA, &ioValue); } if (ioMode == IO_MODE_REMOTE) { // Control by remote client: // - possible to operate the LEDs and buzzer // - right key functionality overridden (will not terminate connection) if (!!(ioValue & IO_DATA_LED1)) { PIN_setOutputValue(hGpioPin, Board_LED1, Board_LED_ON); } else { PIN_setOutputValue(hGpioPin, Board_LED1, Board_LED_OFF); } if (!!(ioValue & IO_DATA_LED2)) { PIN_setOutputValue(hGpioPin, Board_LED2, Board_LED_ON); } else { PIN_setOutputValue(hGpioPin, Board_LED2, Board_LED_OFF); } if (!!((ioValue & IO_DATA_BUZZER))) { Clock_start(buzzClockHandle); } else { Clock_stop(buzzClockHandle); PIN_setOutputValue(hGpioPin, Board_BUZZER, Board_BUZZER_OFF); } } }
/* * ======== Clock_Module_startup ======== */ Int SecondsClock_Module_startup(Int phase) { Clock_TimerProxy_Handle timer; Types_FreqHz freq; UInt32 period; UInt32 absDrift; UInt32 c1, c2; Int32 drift; if (!Clock_Module_startupDone()) { return Startup_NOTDONE; } timer = Clock_getTimerHandle(); Clock_TimerProxy_getFreq(timer, &freq); period = Clock_TimerProxy_getPeriod(timer); /* * Calculate the clock drift: * * drift = timerFreq - clockFreq * clockTimerPeriod * * Example: The timer frequency is 32Khz (frequency is actually 32768), * and clock tick is 1 millisecond. The timer period register will be * set to 32. The drift is: * 32768 - 1000 * 32 = 768 * * This means that our 'second' is really short by 768 timer ticks, or * 768 / 32768 of a second. (This problem would be easily solved by just * setting the period of the SecondsClock to 1024 msecs instead of 1000 * msecs, but it serves as a simple example.) * * If the drift is negative, our seconds are too long. For example, if * the timer period register were 33 in the above example instead of 32, * we would get * drift = 32768 - 1000 * 33 = -768 * * To adjust the seconds, we will periodically add (drift > 0) or subtract * (drift < 0) a second. The period to do this will be: * * c1 = floor(timer freq / |drift|) * * In the above example (for both 32 and 33 timer periods), we will adjust * every floor(32768 / 768) = 42 seconds. * * In this example, we are adjusting a little too quickly, since * 32768 / 768 = 42 2/3. Too compensate, periodically, we will adjust by * not adding or subtracting a second. * * To calculate the period for not adjusting: Calculate what the * 2nd drift would be after the first adjustment: * * drift2 = timerFreq - c1 * |drift| * * drift2 is accumulated every c1 seconds. If drift2 != 0, set * * c2 = floor(timerFreq / drift2) * * and every c1 * c2 seconds, do not do the adjustment. * * If we wanted, we could continue in this manner, calculating * drift3 = timerFreq - c2 * |drift2| * c3 = floor(timerFreq / drift3) * * and every c1 * c2 * c3 seconds, do the adjustment. */ drift = freq.lo - (1000000 / Clock_tickPeriod) * period; absDrift = (drift < 0) ? -drift : drift; /* Only calculate drift for frequencies less than 4GHz */ if (freq.lo && drift) { c1 = c2 = 0; c1 = freq.lo / absDrift; c2 = (freq.lo != c1 * absDrift) ? freq.lo / (freq.lo - c1 * absDrift) : 0; SecondsClock_module->c1Inc = (drift > 0) ? -1 : 1; SecondsClock_module->c1 = c1; SecondsClock_module->c2 = c2; } Clock_start(SecondsClock_Module_State_clock()); return Startup_DONE; }
/* * ======== PowerCC3200_sleepPolicy ======== */ void PowerCC3200_sleepPolicy() { bool returnFromSleep = FALSE; uint32_t constraintMask; uint32_t ticks; uint64_t time; uint64_t match; uint64_t curr; uint64_t remain; uint32_t taskKey; uint32_t swiKey; /* disable interrupts */ CPUcpsid(); /* disable Swi and Task scheduling */ swiKey = Swi_disable(); taskKey = Task_disable(); /* query the declared constraints */ constraintMask = Power_getConstraintMask(); /* * Do not go into LPDS if not allowed into DEEPSLEEP. * Check to see if we can go into LPDS (lowest level sleep). * If not allowed, then attempt to go into DEEPSLEEP. * If not allowed in DEEPSLEEP then just SLEEP. */ /* check if we are allowed to go to LPDS */ if ((constraintMask & ((1 << PowerCC3200_DISALLOW_LPDS) | (1 << PowerCC3200_DISALLOW_DEEPSLEEP))) == 0) { /* * Check how many ticks until the next scheduled wakeup. A value of * zero indicates a wakeup will occur as the current Clock tick period * expires; a very large value indicates a very large number of Clock * tick periods will occur before the next scheduled wakeup. */ /* Get the time remaining for the RTC timer to expire */ ticks = Clock_getTicksUntilInterrupt(); /* convert ticks to microseconds */ time = ticks * Clock_tickPeriod; /* check if can go to LPDS */ if (time > Power_getTransitionLatency(PowerCC3200_LPDS, Power_TOTAL)) { /* get the current and match values for RTC */ match = MAP_PRCMSlowClkCtrMatchGet(); curr = MAP_PRCMSlowClkCtrGet(); remain = match - curr - (((uint64_t)PowerCC3200_TOTALTIMELPDS * 32768) / 1000000); /* set the LPDS wakeup time interval */ MAP_PRCMLPDSIntervalSet(remain); /* enable the wake source to be timer */ MAP_PRCMLPDSWakeupSourceEnable(PRCM_LPDS_TIMER); /* go to LPDS mode */ Power_sleep(PowerCC3200_LPDS); /* set 'returnFromSleep' to TRUE*/ returnFromSleep = TRUE; } } /* check if we are allowed to go to DEEPSLEEP */ if ((constraintMask & (1 << PowerCC3200_DISALLOW_DEEPSLEEP) == 0) && (!returnFromSleep)) { /* * Check how many ticks until the next scheduled wakeup. A value of * zero indicates a wakeup will occur as the current Clock tick period * expires; a very large value indicates a very large number of Clock * tick periods will occur before the next scheduled wakeup. */ ticks = Clock_getTicksUntilInterrupt(); /* convert ticks to microseconds */ time = ticks * Clock_tickPeriod; /* check if can go to DEEPSLEEP */ if (time > Power_getTransitionLatency(PowerCC3200_DEEPSLEEP, Power_TOTAL)) { /* schedule the wakeup event */ ticks -= PowerCC3200_RESUMETIMEDEEPSLEEP / Clock_tickPeriod; Clock_setTimeout(Clock_handle(&clockObj), ticks); Clock_start(Clock_handle(&clockObj)); /* go to DEEPSLEEP mode */ Power_sleep(PowerCC3200_DEEPSLEEP); Clock_stop(Clock_handle(&clockObj)); /* set 'returnFromSleep' to TRUE so we don't go to sleep (below) */ returnFromSleep = TRUE; } } /* re-enable interrupts */ CPUcpsie(); /* restore Swi scheduling */ Swi_restore(swiKey); /* restore Task scheduling */ Task_restore(taskKey); /* sleep only if we are not returning from one of the sleep modes above */ if (!(returnFromSleep)) { MAP_PRCMSleepEnter(); } }
void ReadLightW(Semaphore_Handle Sema, unsigned int *results){ char RMS_str[5]; char RMS_cnt_str[5]; //Set pins as output and set light sensor high. GPIOPinTypeGPIOOutput(GPIO_PORTE_BASE, GPIO_PIN_1); GPIOPinWrite(GPIO_PORTE_BASE, GPIO_PIN_1, GPIO_PIN_1); Timer_start(timer1); //Turn on timer1 for 12 microseconds. //Pend on LightSema Semaphore_pend(Sema, BIOS_WAIT_FOREVER); switch (adState) { case 0: // no line dtected if (*results && (black_count >= 20)){ //make sure it is a full line. Clock_start(DataClock); wasWhite = 0; //record state. adState = 1; //next state. } break; case 1: //first rising edge detected //falling edge of first line if (!(*results) && !wasWhite && (black_count >= 20)) { wasWhite = 1; adState = 2; if ((black_count <= 40) && (black_count >= 20)) { //single line detected if (drive_state == FOLLOW1 || prev_follow_state == FOLLOW1){ //First black line found, progress to follow2 // and start acquiring data. drive_state = FOLLOW2; prev_follow_state = FOLLOW2; RMS_cnt = 0; RMS = 0; Clock_start(DataClock); } else if (drive_state == FOLLOW2 || prev_follow_state == FOLLOW2){ //Second black line found, progress to follow3 // and stop acquiring data. prev_follow_state = FOLLOW3; drive_state = FOLLOW3; Clock_stop(DataClock); //RMS /= RMS_cnt; //finalize RMS //RMS = sqrt(RMS); intToStr(RMS_cnt, RMS_cnt_str, 4); strcpy(FullBufferPtr, "\0"); strcpy(FullBufferPtr, "R=\0"); //Clear the fullBuffer. intToStr(RMS, RMS_str, 4); strcat(FullBufferPtr, RMS_str); strcat(FullBufferPtr, "\nC=\0"); strcat(FullBufferPtr, RMS_cnt_str); strcat(FullBufferPtr, "\n\0"); writeFrame("",FullBufferPtr); //Semaphore_post(TxDataSema); //Let writeFrame continue. //Clock_stop(DataClock); } }else if (black_count > 40){ //double line detected StopDrivingFn(); Clock_stop(DataClock); } black_count = 0; } break; case 2: //first falling edge detected if (*results && wasWhite && (black_count >= 20)){ wasWhite = 0; adState = 3; } break; case 3: //second rising edge detected //falling edge of second line if (!(*results) && !wasWhite && (black_count >= 20)) { wasWhite = 1; adState = 4; if ((black_count <= 40) && (black_count >= 20)) { //single line detected if (drive_state == FOLLOW1 || prev_follow_state == FOLLOW1){ //First black line found, progress to follow2 // and start acquiring data. drive_state = FOLLOW2; prev_follow_state = FOLLOW2; RMS_cnt = 0; RMS = 0; Clock_start(DataClock); } else if (drive_state == FOLLOW2 || prev_follow_state == FOLLOW2){ //First black line found, progress to follow3 // and stop acquiring data. prev_follow_state = FOLLOW3; drive_state = FOLLOW3; //Clock_stop(DataClock); Clock_stop(DataClock); //RMS /= RMS_cnt; //finalize RMS //RMS = sqrt(RMS); intToStr(RMS_cnt, RMS_cnt_str, 4); strcpy(FullBufferPtr, "\0"); strcpy(FullBufferPtr, "R=\0"); //Clear the fullBuffer. intToStr(RMS, RMS_str, 4); strcat(FullBufferPtr, RMS_str); strcat(FullBufferPtr, "\nC=\0"); strcat(FullBufferPtr, RMS_cnt_str); strcat(FullBufferPtr, "\n\0"); writeFrame("",FullBufferPtr); //Semaphore_post(TxDataSema); //Let writeFrame continue. } }else if (black_count > 40){//double line detected //StopDrivingFn(); Clock_stop(DataClock); } black_count = 0; } break; case 4: //second falling edge detected. Done collecting data. //Clock_stop(DataClock); //StopAcquireData(); Clock_stop(DataClock); //RMS /= RMS_cnt; //finalize RMS //RMS = sqrt(RMS); intToStr(RMS_cnt, RMS_cnt_str, 4); strcpy(FullBufferPtr, "\0"); strcpy(FullBufferPtr, "R=\0"); //Clear the fullBuffer. intToStr(RMS, RMS_str, 4); strcat(FullBufferPtr, RMS_str); strcat(FullBufferPtr, "\nC=\0"); strcat(FullBufferPtr, RMS_cnt_str); strcat(FullBufferPtr, "\n\0"); writeFrame("",FullBufferPtr); //Semaphore_post(TxDataSema); //Let writeFrame continue. //push whats remaining in the buffer out to transfer it. strcpy(FullBufferPtr, "\0"); strcat(pingptr1, "\n\0"); strcpy(FullBufferPtr, pingptr1); Semaphore_post(TxDataSema); strcpy(pingptr1, "\0"); numChars = 0; wasWhite = 0; adState = 0; break; default: break; } }
void DriveFn (){ static int lightFlag = 0; while(1){ Semaphore_pend(DriveSema, BIOS_WAIT_FOREVER); switch (drive_state) { case START: //enable motors forward. PWMOutputState(PWM0_BASE, MOTOR_LF_EN , true); PWMOutputState(PWM0_BASE, MOTOR_RF_EN , true); //accelerate to speed setSpeed (200, MOTOR_RF_EN); setSpeed (200, MOTOR_LF_EN); drive_state = FOLLOW1; GPIO_write(Board_LED0, Board_LED_ON); break; //Follow1,2 and 3 grouped together since they do the same thing. //They do progress depending on the lines the robot has passed, but that //is handled in another area. case FOLLOW1: case FOLLOW2: case FOLLOW3: prev_follow_state = drive_state; if (lightFlag == 0) //acquire data lightFlag = !lightFlag; if (!wall_det) { drive_state = INTERSECTION; } else if (front_wall_det == 1){ drive_state = UTURN; //stop if front wall detected } //read right wall distance PID_Algorithm(); break; case INTERSECTION: if (Clock_isActive(DataClock) ){ //if collecting data Clock_stop(DataClock); //don't collect data in intersection motor_RightTurn(); Clock_start(DataClock); } else //if not collecting data motor_RightTurn(); //move along break; case UTURN: if (Clock_isActive(DataClock) ){ //if collecting data Clock_stop(DataClock); //don't collect data in uturn motor_Uturn(); Clock_start(DataClock); } else //if not collecting data motor_Uturn(); //move along break; case STOP: //decelerate to a stop setSpeed (0, MOTOR_LF_EN); setSpeed (0, MOTOR_RF_EN); StopDrivingFn(); drive_state = OFF; break; case OFF: //disable motors PWMOutputState(PWM0_BASE, MOTOR_LF_EN , false); PWMOutputState(PWM0_BASE, MOTOR_RF_EN , false); break; } } }
//***************************************************************************** // //! Festo Station Task. //! //! This task function do all the work related to controlling the Festo //! Station. The task will first initialize the Driver and then run a infinite //! loop, waiting and processing external events. //! //! \return None. // //***************************************************************************** Void _task_FESTO(UArg arg0, UArg arg1) { // initialize driver FestoStationDriver DriverInstance; FestoStationDriver* Driver = &DriverInstance; Festo_Driver_Init(Driver); uint32_t EventPosted; DisplayMessage MessageObject; MessageObject.ScreenID = 0; MessageObject.uptimeSeconds = 0; MessageObject.piecesProcessed = 0; MessageObject.blackAccepted = 0; MessageObject.blackRejected = 0; MessageObject.plasticAccepted = 0; MessageObject.plasticRejected = 0; MessageObject.orangeAccepted = 0; MessageObject.orangeRejected = 0; MessageObject.piecesProcessedPerSecond = 0; MessageObject.heightCalibrated = 230; MessageObject.upperHeightCalibrated = 245; MessageObject.lowerHeightCalibrated = 227; time_t t1 = time(NULL); strcpy((char*) MessageObject.timeString, asctime(localtime(&t1))); uint32_t uptimeSeconds = 0; uint32_t piecesProcessed = 0; uint32_t blackAccepted = 0; uint32_t blackRejected = 0; uint32_t plasticAccepted = 0; uint32_t plasticRejected = 0; uint32_t orangeAccepted = 0; uint32_t orangeRejected = 0; uint32_t metallicAccepted = 0; uint32_t metallicRejected = 0; uint32_t piecesProcessedPerSecond = 0; uint32_t *ColourAccepted; uint32_t *ColourRejected; uint32_t *MaterialAccepted; uint32_t *MaterialRejected; uint8_t colour = 0; uint8_t material = 0; uint32_t i = 0; uint32_t FestoState = 0; // 0 = stopped // 1 = idle // 2 = initial state // 10 = cal int32_t LowerLimit = 1200; int32_t UpperLimit = 1500; int32_t Uptime = 0; int32_t Time0 = 0; int32_t Time1 = 0; uint8_t Running = 0; Clock_Params clockParams; Clock_Handle myClock; Clock_Params_init(&clockParams); clockParams.arg = (UArg) Board_ACTUATOR_EJECTOR; myClock = Clock_create(_Festo_Deactivate_Ejector, 200, &clockParams, NULL); uint32_t heightMeasured = 0; uint32_t heightCalibratedADC = 1380; uint32_t heightCalibrated10mm = 225; //float ConvertFactor = 0.1*MessageObject.heightCalibrated/1200; GPIO_write(Board_LED0, Board_LED_OFF); GPIO_write(Board_LED1, Board_LED_OFF); GPIO_write(Board_LED2, Board_LED_ON); Mailbox_post(DisplayMailbox, &MessageObject, BIOS_NO_WAIT); Clock_start(Clock_1_sec); while(1) { EventPosted = Event_pend(FestoEvents, Event_Id_NONE, FESTO_EVENT_BUTTON_UP + FESTO_EVENT_BUTTON_DOWN + FESTO_EVENT_BUTTON_SELECT + FESTO_EVENT_RISER_DOWN + FESTO_EVENT_RISER_UP + FESTO_EVENT_ADC_FINISH + FESTO_EVENT_PIECE_IN_PLACE + FESTO_EVENT_EJECTOR_FINISHED + FESTO_EVENT_TICK + FESTO_EVENT_COOLDOWN + FESTO_EVENT_PIECE_NOT_IN_PLACE, FESTO_TIMEOUT); if (EventPosted & FESTO_EVENT_BUTTON_UP) { if (FestoState == 0) { Festo_Control_Driver(Driver, FESTO_ENABLED); FestoState = 1; Running = 1; GPIO_write(Board_LED0, Board_LED_ON); GPIO_write(Board_LED1, Board_LED_OFF); GPIO_write(Board_LED2, Board_LED_OFF); MessageObject.ScreenID = 1; Mailbox_post(DisplayMailbox, &MessageObject, BIOS_NO_WAIT); Time0 = Clock_getTicks(); } else if (FestoState == 12) { MessageObject.heightCalibrated++; Mailbox_post(DisplayMailbox, &MessageObject, BIOS_NO_WAIT); } else if (FestoState == 13) { MessageObject.upperHeightCalibrated++; Mailbox_post(DisplayMailbox, &MessageObject, BIOS_NO_WAIT); } else if (FestoState == 14) { MessageObject.lowerHeightCalibrated++; Mailbox_post(DisplayMailbox, &MessageObject, BIOS_NO_WAIT); } } else if (EventPosted & FESTO_EVENT_BUTTON_DOWN) { if (FestoState == 1) { Festo_Control_Ejector(Driver, FESTO_EJECTOR_RETRACT); Festo_Control_Platform(Driver, FESTO_PLATFORM_LOWER); Festo_Control_Driver(Driver, FESTO_DISABLED); FestoState = 0; Running = 0; GPIO_write(Board_LED0, Board_LED_OFF); GPIO_write(Board_LED1, Board_LED_OFF); GPIO_write(Board_LED2, Board_LED_ON); MessageObject.ScreenID = 0; Mailbox_post(DisplayMailbox, &MessageObject, BIOS_NO_WAIT); } else if (FestoState == 12) { MessageObject.heightCalibrated--; Mailbox_post(DisplayMailbox, &MessageObject, BIOS_NO_WAIT); } else if (FestoState == 13) { MessageObject.upperHeightCalibrated--; Mailbox_post(DisplayMailbox, &MessageObject, BIOS_NO_WAIT); } else if (FestoState == 14) { MessageObject.lowerHeightCalibrated--; Mailbox_post(DisplayMailbox, &MessageObject, BIOS_NO_WAIT); } } else if (EventPosted & FESTO_EVENT_BUTTON_SELECT) { if (FestoState == 0) { GPIO_write(Board_LED0, Board_LED_OFF); GPIO_write(Board_LED1, Board_LED_ON); GPIO_write(Board_LED2, Board_LED_OFF); Festo_Control_Driver(Driver, FESTO_ENABLED); FestoState = 10; MessageObject.ScreenID = 2; Mailbox_post(DisplayMailbox, &MessageObject, BIOS_NO_WAIT); Festo_Control_Ejector(Driver, FESTO_EJECTOR_RETRACT); if (Festo_Sense_Riser_Down(Driver) != 1) { Festo_Control_Platform(Driver, FESTO_PLATFORM_LOWER); } else { FestoState = 11; MessageObject.ScreenID = 3; Mailbox_post(DisplayMailbox, &MessageObject, BIOS_NO_WAIT); } } else if (FestoState == 11) { FestoState = 12; MessageObject.ScreenID = 4; MessageObject.heightCalibrated = 230; Mailbox_post(DisplayMailbox, &MessageObject, BIOS_NO_WAIT); } else if (FestoState == 12) { FestoState = 13; Festo_Control_Platform(Driver, FESTO_PLATFORM_RAISE); Event_post(FestoEvents, FESTO_EVENT_ADC_START); } else if (FestoState == 13) { FestoState = 14; MessageObject.ScreenID = 6; MessageObject.lowerHeightCalibrated = 227; UpperLimit = MessageObject.upperHeightCalibrated * (1.0 * heightCalibratedADC)/(1.0 * heightCalibrated10mm); System_printf("Upper Limit Cal: %d for %d *0.1 mm\n", UpperLimit, MessageObject.upperHeightCalibrated); System_flush(); Mailbox_post(DisplayMailbox, &MessageObject, BIOS_NO_WAIT); } else if (FestoState == 14) { FestoState = 15; MessageObject.ScreenID = 7; LowerLimit = MessageObject.lowerHeightCalibrated * (1.0 * heightCalibratedADC)/(1.0 * heightCalibrated10mm); System_printf("Lower Limit Cal: %d for %d *0.1 mm\n", LowerLimit, MessageObject.lowerHeightCalibrated); System_flush(); Festo_Control_Ejector(Driver, FESTO_EJECTOR_RETRACT); Festo_Control_Platform(Driver, FESTO_PLATFORM_LOWER); Mailbox_post(DisplayMailbox, &MessageObject, BIOS_NO_WAIT); } else if (FestoState == 15) { FestoState = 0; MessageObject.ScreenID = 0; GPIO_write(Board_LED0, Board_LED_OFF); GPIO_write(Board_LED1, Board_LED_OFF); GPIO_write(Board_LED2, Board_LED_ON); Mailbox_post(DisplayMailbox, &MessageObject, BIOS_NO_WAIT); } else { } } else if (EventPosted & FESTO_EVENT_RISER_UP) { if (FestoState == 3) { if (Festo_Sense_Piece_Placed(Driver) == 1) { FestoState = 4; // wait estabilize for (i = 0; i < 1000000; i++); Event_post(FestoEvents, FESTO_EVENT_ADC_START); } else { FestoState = 1; Festo_Control_Ejector(Driver, FESTO_EJECTOR_RETRACT); Festo_Control_Platform(Driver, FESTO_PLATFORM_LOWER); } } else if (FestoState == 13) { Event_post(FestoEvents, FESTO_EVENT_ADC_START); } else { } } else if (EventPosted & FESTO_EVENT_RISER_DOWN) { if (FestoState == 2) { if (Festo_Sense_Piece_Placed(Driver) == 1) { // get a lot of samples for (i = 0; i < 1200; i++) { // this waits to get a reliable reading. If the reading is different from before, reset. if (colour != Festo_Sense_Piece_Colour(Driver) || material != Festo_Sense_Piece_Material(Driver)) { i = 0; } colour = Festo_Sense_Piece_Colour(Driver); material = Festo_Sense_Piece_Material(Driver); } FestoState = 3; Festo_Control_Platform(Driver, FESTO_PLATFORM_RAISE); } else { FestoState = 1; } } if (FestoState == 5) { Festo_Control_Ejector(Driver, FESTO_EJECTOR_EXTEND); Clock_start(myClock); } if (FestoState == 6) { Festo_Control_Ejector(Driver, FESTO_EJECTOR_RETRACT); Clock_start(myClock); FestoState = 7; } if (FestoState == 10) { FestoState = 11; MessageObject.ScreenID = 3; Mailbox_post(DisplayMailbox, &MessageObject, BIOS_NO_WAIT); } } else if (EventPosted & FESTO_EVENT_ADC_FINISH) { if (Mailbox_pend(ADCMailbox, &heightMeasured, BIOS_NO_WAIT)) { Festo_Sense_Set_Piece_Height(Driver, heightMeasured); } else { Event_post(FestoEvents, FESTO_EVENT_ADC_START); } if (FestoState == 4) { if (colour == FESTO_COLOR_ORANGE && material == FESTO_PIECE_OTHER) { ColourAccepted = &orangeAccepted; ColourRejected = &orangeRejected; MaterialAccepted = &plasticAccepted; MaterialRejected = &plasticRejected; } else if (colour == FESTO_COLOR_OTHER && material == FESTO_PIECE_OTHER) { ColourAccepted = &blackAccepted; ColourRejected = &blackRejected; MaterialAccepted = &plasticAccepted; MaterialRejected = &plasticRejected; } else if (material == FESTO_PIECE_METALLIC) { ColourAccepted = NULL; ColourRejected = NULL; MaterialAccepted = &metallicAccepted; MaterialRejected = &metallicRejected; } else { ColourAccepted = NULL; ColourRejected = NULL; MaterialAccepted = NULL; MaterialRejected = NULL; } if (heightMeasured < UpperLimit && heightMeasured > LowerLimit)//withn range { if (ColourAccepted != NULL) { (*ColourAccepted)++; } if (MaterialAccepted != NULL) { (*MaterialAccepted)++; piecesProcessed++; } FestoState = 6; System_printf("Piece is acceptable\n"); System_flush(); Festo_Control_Ejector(Driver, FESTO_EJECTOR_EXTEND); Clock_start(myClock); } else // out of range { if (ColourAccepted != NULL) { (*ColourRejected)++; } if (MaterialAccepted != NULL) { (*MaterialRejected)++; piecesProcessed++; } System_printf("Piece is NOT acceptable\n"); System_flush(); FestoState = 5; Festo_Control_Platform(Driver, FESTO_PLATFORM_LOWER); } } else if (FestoState == 13) { heightCalibratedADC = heightMeasured; heightCalibrated10mm = MessageObject.heightCalibrated; System_printf("ADC Cal: %d for %d *0.1 mm\n", heightCalibratedADC, heightCalibrated10mm); System_flush(); MessageObject.ScreenID = 5; MessageObject.upperHeightCalibrated = 245; Mailbox_post(DisplayMailbox, &MessageObject, BIOS_NO_WAIT); } } else if (EventPosted & FESTO_EVENT_EJECTOR_FINISHED) { if (FestoState == 6) { Festo_Control_Platform(Driver, FESTO_PLATFORM_LOWER); Clock_start(myClock); } else if (FestoState == 7) { FestoState = 1; } else { Festo_Control_Ejector(Driver, FESTO_EJECTOR_RETRACT); Clock_start(myClock); FestoState = 7; } } else if (EventPosted & FESTO_EVENT_TICK) { if (Running) { Time1 = Clock_getTicks(); Uptime += (Time1 - Time0); Time0 = Time1; uptimeSeconds = Uptime * 0.001; piecesProcessedPerSecond = 100 * piecesProcessed/(0.016667*uptimeSeconds); MessageObject.piecesProcessed = piecesProcessed; MessageObject.blackAccepted = blackAccepted; MessageObject.blackRejected = blackRejected; MessageObject.plasticAccepted = plasticAccepted; MessageObject.plasticRejected = plasticRejected; MessageObject.orangeAccepted = orangeAccepted; MessageObject.orangeRejected = orangeRejected; MessageObject.piecesProcessedPerSecond = piecesProcessedPerSecond; MessageObject.uptimeSeconds = uptimeSeconds; MessageObject.metalAccepted = metallicAccepted; MessageObject.metalRejected = metallicRejected; } Seconds_set(Seconds_get()+1); t1 = time(NULL); strcpy((char*) MessageObject.timeString, asctime(localtime(&t1))); Mailbox_post(DisplayMailbox, &MessageObject, BIOS_NO_WAIT); } else if (EventPosted & FESTO_EVENT_PIECE_NOT_IN_PLACE) { if (FestoState <= 4) { FestoState = 1; Festo_Control_Ejector(Driver, FESTO_EJECTOR_RETRACT); Festo_Control_Platform(Driver, FESTO_PLATFORM_LOWER); } } else { if (FestoState == 1) { if (Festo_Sense_Piece_Placed(Driver) == 1) { FestoState = 2; if (Festo_Sense_Riser_Down(Driver) == 0) { Festo_Control_Platform(Driver, FESTO_PLATFORM_LOWER); } else { Event_post(FestoEvents, FESTO_EVENT_RISER_DOWN); } } } } Task_sleep(100); } }
/************************************************************************************************** * @fn macBackoffTimerUpdateWakeup * * @brief Recomputes and provides weakup timing hint to power module. * This function does not read timing variables in a critical * section. The caller must call this function within a critical * section where the timing variable is modified in order * to have more accurate notification. * * @param none * * @return none ************************************************************************************************** */ static void macBackoffTimerUpdateWakeup(void) { int_fast64_t ticks; /* Replicate the timer in order to inform power module to wake up properly * in case the device enters sleep state in the future. */ /* First find out which RAT channel between backoffTimerTrigger and * macBackoffTimerRollover is supposed to expire next. */ ticks = MAC_RADIO_BACKOFF_COUNT(); if (backoffTimerTrigger > ticks && backoffTimerTrigger < macBackoffTimerRollover) { ticks = backoffTimerTrigger - ticks; } else if (macBackoffTimerRollover > ticks) { ticks = macBackoffTimerRollover - ticks; } else { /* Note that current count might have exceeded already set thresholds, * as the code is executed while interrupt is flagged. * In such a case, this function shall be called again anyways and * hence this condition can be ignored. */ return; } /* TODO: For subG, backoff timer unit is not necessarily backoff period * but fixed 320us and hence the following constant * (MAC_SPEC_USECS_PER_BACKOFF) should be replaced with backoff * timer unit period in usecs. */ /* Convert backoff timer unit to RTOS tick unit */ ticks *= MAC_SPEC_USECS_PER_BACKOFF; #ifdef USE_ICALL ticks -= (MAC_BACKOFF_TIMER_ADDITIONAL_WAKEUP_LATENCY + ICall_pwrGetXOSCStartupTime(ticks/1000)); #elif defined OSAL_PORT2TIRTOS ticks -= MAC_BACKOFF_TIMER_ADDITIONAL_WAKEUP_LATENCY + Power_getXoscStartupTime(ticks/1000); #endif /* defined OSAL_PORT2TIRTOS */ if (ticks > 0) { #ifdef USE_ICALL ticks /= osal_tickperiod; #elif defined OSAL_PORT2TIRTOS ticks /= Clock_tickPeriod; #endif /* defined OSAL_PORT2TIRTOS */ } if (ticks > 0) { #ifdef USE_ICALL ICall_setTimer((uint_fast32_t) ticks, macBackoffTimerICallTimerCback, NULL, &macBackoffTimerICallTimerID); #endif /* USE_ICALL */ #ifdef OSAL_PORT2TIRTOS Clock_stop(macBackoffWakeupClock); Clock_setTimeout(macBackoffWakeupClock, (UInt32) ticks); Clock_start(macBackoffWakeupClock); #endif /* OSAL_PORT2TIRTOS */ /* Allow entering sleep state */ macBackoffTimerImpending = FALSE; #ifdef DEBUG_SW_TRACE DBG_PRINTL1(DBGSYS, "BO %u", ticks); #endif /* DEBUG_SW_TRACE */ } else { /* Timing is too close. Suppress sleep. */ macBackoffTimerImpending = TRUE; } macPwrVote(); }
void Clock::start() { Clock_start(ClockHandle); }