/*
* ======== 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);
}
