bool_t osWaitForEvent(OsEvent *event, systime_t timeout)
{
Bool ret;
//Wait until the specified event is in the signaled state
if(timeout == 0)
{
//Non-blocking call
ret = Event_pend(event->handle, Event_Id_00,
Event_Id_NONE, BIOS_NO_WAIT);
}
else if(timeout == INFINITE_DELAY)
{
//Infinite timeout period
ret = Event_pend(event->handle, Event_Id_00,
Event_Id_NONE, BIOS_WAIT_FOREVER);
}
else
{
//Wait for the specified time interval
ret = Event_pend(event->handle, Event_Id_00,
Event_Id_NONE, OS_MS_TO_SYSTICKS(timeout));
}
//The return value tells whether the event is set
return ret;
}
Int32 fxnTest1(UInt32 size, UInt32 *data)
{
FxnArgs *args = (FxnArgs *)((UInt32)data + sizeof(map_info_type));
t1.buffer1 = t2.buffer1 = (int*)(args->a) ;
t1.buffer2 = t2.buffer2 = (int*)(args->b) ;
t1.buffer3 = t2.buffer3 = (int*)(args->c) ;
t1.start_indx = t2.start_indx = args->start_indx;
t1.end_indx = t2.end_indx = args->end_indx;
int unit_work = (t2.end_indx - t1.start_indx)/2 ;
t1.end_indx = t1.start_indx + unit_work ;
t2.start_indx = t2.start_indx + unit_work ;
task0 = Task_create((Task_FuncPtr)common_wrapper, &taskParams0, &eb0);
if (task0 == NULL) {
System_abort("Task create failed");
}
task1 = Task_create((Task_FuncPtr)common_wrapper, &taskParams1, &eb1);
if (task1 == NULL) {
// System_abort("Task create failed");
}
UInt events ;
events = Event_pend(edgeDetectEvent, Event_Id_00 + Event_Id_01, Event_Id_NONE, BIOS_WAIT_FOREVER) ;
Task_delete(&task0) ;
Task_delete(&task1) ;
Uint32 reta = 1 ;
return reta ;
}
// Task pends in function until all pend conditions are met. Allows execution
// of SNP and ICall flags while waiting. For use with SNP_LOCAL only.
uint32_t SNP_pend(Event_Handle event, uint32_t andFlags, uint32_t orFlags,
uint32_t timeout)
{
uint32_t flags;
uint32_t savedAndFlags = 0;
uint32_t savedOrFlags = 0;
while(1)
{
// Pend on event flags posted by the application and OR in the SNP
// Event flags. This includes AND flags to not block SNP when it is waiting
// on events.
flags = Event_pend(event, Event_Id_NONE, orFlags + andFlags + SNP_ALL_EVENTS, timeout);
// Check if timeout
if (!flags)
{
break;
}
if (flags & andFlags)
{
// Save the AND flags until we have all of them.
savedAndFlags |= flags & andFlags;
}
if (flags & orFlags)
{
// Save the OR flags until we have all of them.
savedOrFlags |= flags & orFlags;
}
if (flags & SNP_ALL_EVENTS)
{
// Process SNP events
SNP_processEvents();
}
// set savedFlags back in.
flags |= savedAndFlags | savedOrFlags;
// Check if flags OR savedAndFlags matches all andFlags AND at least one
// OR flag.
if (((flags & andFlags) == andFlags) && (flags & orFlags))
{
// All conditions were met.
break;
}
}
return flags;
}
void Race()
{
Bike.Mode = MODE_RACE;
Bike.Fault.all = 0;
Bike.Warning.all = 0;
W2000.BusVoltage = 0;
if(Event_pend(EVT_Change_Mode,Event_Id_00, Event_Id_NONE,BIOS_NO_WAIT) != 0)
{
Event_post(EVT_Change_Mode,Event_Id_00);
return;
}
GPIO_Race();
if(Event_pend(EVT_Change_Mode,Event_Id_00, Event_Id_NONE,BIOS_NO_WAIT) != 0)
{
Event_post(EVT_Change_Mode,Event_Id_00);
return;
}
if (Start_Race_Peripheral() == 1 || GpioDataRegs.GPBDAT.A_FUALT_OVERRIDE_SW == FAULT_OVERRIDE_ON)
{
Close_Contactors();
}
Event_Logger("MODE CHANGE TO RACE");
}
unsigned char Send_LCD(int a)
{
ScibRegs.SCITXBUF=a;
ScibRegs.SCIFFTX.bit.TXFFINTCLR=1; // Clear SCI Interrupt flag
unsigned char ret = Event_pend(EVT_LCD_Tx,Event_Id_00, Event_Id_NONE,LCD_TIMEOUT);
if(ret == 0)
{
LCD_S.timeout_count++;
}
else
{
LCD_S.timeout_count = 0;
}
return ret;
}
/*
* ======== reader ========
*/
Void reader(UArg arg0, UArg arg1)
{
MsgObj msg;
UInt posted;
for (;;) {
/* wait for (Event_Id_00 & Event_Id_01) | Event_Id_02 */
posted = Event_pend(evt,
Event_Id_00 + Event_Id_01, /* andMask */
Event_Id_02, /* orMask */
TIMEOUT);
if (posted == 0) {
System_printf("Timeout expired for Event_pend()\n");
break;
}
if ((posted & Event_Id_00) && (posted & Event_Id_01)) {
if (Semaphore_pend(sem, BIOS_NO_WAIT)) {
System_printf("Explicit posting of Event_Id_00 and Implicit posting of Event_Id_01\n");
}
else {
System_printf("Semaphore not available. Test failed!\n");
}
break;
}
else if (posted & Event_Id_02) {
System_printf("Implicit posting of Event_Id_02\n");
if (Mailbox_pend(mbx, &msg, BIOS_NO_WAIT)) {
/* print value */
System_printf("read id = %d and val = '%c'.\n",msg.id, msg.val);
}
else {
System_printf("Mailbox not available. Test failed!\n");
}
}
else {
System_printf("Unknown Event\n");
break;
}
}
BIOS_exit(0);
}
void SPI_HandleEvent()
{
UInt events;
while (TRUE)
{
events = Event_pend(SPI_Event, Event_Id_NONE, (SPI_TX_READY_EVENT | SPI_DONE_EVENT), BIOS_WAIT_FOREVER);
switch (events)
{
case SPI_TX_READY_EVENT:
{
SPI_SendTx(mDeviceInUse);
break;
}
case SPI_DONE_EVENT:
{
break;
}
}
}
}
/*
* ======== Adaptor_transferAgentTaskFxn ========
*/
Void Adaptor_transferAgentTaskFxn(UArg arg, UArg unused)
{
Adaptor_Entry *entry;
Bool status;
Bits32 mask;
if (ServiceMgr_transportFxns.startFxn != NULL) {
Adaptor_module->transportEventHandle =
ServiceMgr_transportFxns.startFxn(UIAPacket_HdrType_EventPkt);
}
/* Run in a loop. */
while (TRUE) {
/* Block on
* - the clock: request events to be sent.
* - incoming message: call Adaptor_sendToService
* - outgoing message: call Adaptor_sendToHost
* - energy request: call Adaptor_giveEnergy
*/
mask = Event_pend(Adaptor_module->event, Event_Id_NONE,
Event_Id_00 | Event_Id_01 | Event_Id_02 | Event_Id_03,
BIOS_WAIT_FOREVER);
/* If the Clock expired, gather data */
if (mask & Event_Id_00) {
Adaptor_runScheduledServices();
}
/* A msg has been sent from the host, handle all of them */
if (mask & Event_Id_01) {
entry = (Adaptor_Entry *)Queue_get(Adaptor_module->incomingQ);
while ((Queue_Elem *)entry !=
(Queue_Elem *)Adaptor_module->incomingQ) {
Adaptor_sendToService(entry);
/* Get next one */
entry = (Adaptor_Entry *)Queue_get(Adaptor_module->incomingQ);
}
}
/* Service requested energy */
if (mask & Event_Id_02) {
Adaptor_giveEnergy();
}
/* A msg need to be sent to the host, handle all of them */
if (mask & Event_Id_03) {
entry = (Adaptor_Entry *)Queue_get(Adaptor_module->outgoingQ);
while ((Queue_Elem *)entry !=
(Queue_Elem *)Adaptor_module->outgoingQ) {
/* Send message to host */
status = Adaptor_sendToHost((UIAPacket_Hdr *)&(entry->packet));
if (status == FALSE) {
Adaptor_freePacket((UIAPacket_Hdr *)&(entry->packet));
}
/* Get next one */
entry = (Adaptor_Entry *)Queue_get(Adaptor_module->outgoingQ);
}
}
}
}
/**
* /fn TransferFunction
* /brief Functions transfers read values from altitude/thermo click via uart7.
*
* /param arg0 not used.
* /param arg1 not used.
* /return void.
*/
static void TransferFunction(UArg arg0, UArg arg1) {
TransferMessageType message;
UInt firedEvents;
UART_Handle uart7;
UART_Params uart7Params;
int length;
int precision;
int width;
float value;
bool convert;
int len;
PositionType* position;
DateTimeType* dateTime;
char* destination;
UART_Params_init(&uart7Params);
uart7Params.writeDataMode = UART_DATA_BINARY;
uart7Params.readDataMode = UART_DATA_BINARY;
uart7Params.readReturnMode = UART_RETURN_FULL;
uart7Params.readEcho = UART_ECHO_OFF;
uart7Params.baudRate = 9600;
uart7 = UART_open(Board_UART3, &uart7Params);
if (uart7 == NULL) {
System_abort("Error opening the UART");
}
while (true) {
firedEvents = Event_pend(transferEvent, Event_Id_NONE,
TRANSFER_MESSAGE_EVENT,
BIOS_WAIT_FOREVER);
if (firedEvents & TRANSFER_MESSAGE_EVENT) {
// Get the posted message.
// Mailbox_pend() will not block since Event_pend()
// has guaranteed that a message is available.
Mailbox_pend(transferMailbox, &message, BIOS_NO_WAIT);
convert = true;
switch (message.kind) {
case TRANSFER_PRESSURE:
result[0] = ID_PRESSURE;
value = message.value / 100; /* hPa */
width = 1;
precision = PRESSURE_PRECISION;
if (PRESSURE_PRECISION > 0) {
width = 3;
}
break;
case TRANSFER_ALTITUDE:
result[0] = ID_ALTITUDE;
value = message.value;
precision = ALTITUDE_PRECISION;
if (ALTITUDE_PRECISION > 0) {
width = 3;
}
break;
case TRANSFER_GPS_LOCATION:
result[0] = ID_LOCATION;
convert = false;
position = message.data;
len = strlen(position->latitude);
memcpy(&result[1], position->latitude, len);
destination = result + 1 + len;
len = strlen(position->longitude);
memcpy(destination, position->longitude, len);
destination += len;
*destination = '\0';
break;
case TRANSFER_DATE_TIME:
result[0] = ID_DATE_TIME;
convert = false;
dateTime = message.data;
len = sizeof(dateTime->dateTimeString);
memcpy(&result[1], dateTime->dateTimeString, len);
break;
default:
System_printf(
"Error TransferFunction: Received unknown message %d.\n",
message.kind);
System_flush();
// unknown, nothing special
continue; /* no break, would be unreachable code */
}
if (convert) {
(void) snprintf(&result[1], 20, "%*.*f", width, precision,
value);
} else {
length = strlen(result) + 1;
}
length = strlen(result) + 1;
UART_write(uart7, &result[0], length);
#ifdef DEBUG
System_printf("%s transferred.\n", result);
System_flush();
#endif // DEBUG
}
}
}
//*****************************************************************************
//
//! ADC conversion Task.
//!
//! This task function do all the work related to the ADC conversion real
//!
//! \return None.
//
//*****************************************************************************
Void _task_ADC(UArg arg0, UArg arg1)
{
// initialize ADC
ADCSequenceConfigure(ADC0_BASE, 0, ADC_TRIGGER_PROCESSOR, 0);
ADCSequenceStepConfigure(ADC0_BASE, 0, 0, ADC_CTL_IE | ADC_CTL_CH0 | ADC_CTL_END);
ADCSequenceEnable(ADC0_BASE, 0);
ADCSoftwareOversampleConfigure(ADC0_BASE, 0, 64);
ADCIntClear(ADC0_BASE, 0);
ADCIntEnable(ADC0_BASE, 0);
// data from ADC
uint32_t AdcDataRaw = 0;
uint32_t AdcDataRaw2 = 0;
uint32_t AdcDataRaw3 = 0;
int32_t ReliableMeasure = 0;
uint32_t MeasureEnable = 0;
uint32_t EventPosted;
while(1)
{
EventPosted = Event_pend(FestoEvents,
Event_Id_NONE,
FESTO_EVENT_ADC_START,
0);
if (EventPosted & FESTO_EVENT_ADC_START)
{
MeasureEnable = 1;
ReliableMeasure = 99;
}
else
{
if (MeasureEnable == 1)
{
ADCProcessorTrigger(ADC0_BASE, 0);
while (ADCIntStatus(ADC0_BASE, 0, false) == false);
ADCSequenceDataGet(ADC0_BASE, 0, &AdcDataRaw);
ADCIntClear(ADC0_BASE, 0);
ADCProcessorTrigger(ADC0_BASE, 0);
while (ADCIntStatus(ADC0_BASE, 0, false) == false);
ADCSequenceDataGet(ADC0_BASE, 0, &AdcDataRaw2);
ADCIntClear(ADC0_BASE, 0);
AdcDataRaw3 = 0.5 * (AdcDataRaw + AdcDataRaw2);
MeasureEnable = 2;
}
else if (MeasureEnable == 2)
{
ADCProcessorTrigger(ADC0_BASE, 0);
while (ADCIntStatus(ADC0_BASE, 0, false) == false);
ADCSequenceDataGet(ADC0_BASE, 0, &AdcDataRaw);
ADCIntClear(ADC0_BASE, 0);
ADCProcessorTrigger(ADC0_BASE, 0);
while (ADCIntStatus(ADC0_BASE, 0, false) == false);
ADCSequenceDataGet(ADC0_BASE, 0, &AdcDataRaw2);
ADCIntClear(ADC0_BASE, 0);
AdcDataRaw = 0.5 * (0.5 * (AdcDataRaw + AdcDataRaw2) + AdcDataRaw3);
ReliableMeasure = AdcDataRaw - AdcDataRaw3;
if (ReliableMeasure > 3 || ReliableMeasure < -3)
{
MeasureEnable = 1;
}
else
{
Mailbox_post(ADCMailbox, &AdcDataRaw, BIOS_NO_WAIT);
MeasureEnable = 0;
System_printf("ADC data: %d\n", AdcDataRaw);
System_flush();
}
}
else
{
}
}
Task_sleep(100);
}
}
/**
Task for updating the LCD display every 16ms.
*/
Void _task_LCD(UArg arg0, UArg arg1)
{
// create the LCD context
tContext g_sContext;
// initialize LCD driver
Kentec320x240x16_SSD2119Init(120000000);
// initialize graphics context
GrContextInit(&g_sContext, &g_sKentec320x240x16_SSD2119);
// draw application frame
FrameDraw(&g_sContext, "Festo Station");
uint32_t EventPosted;
DisplayMessage MessageObject;
char StringBuffer[100];
tRectangle ClearRect;
ClearRect.i16XMax = 320;
ClearRect.i16XMin = 0;
ClearRect.i16YMax = 240;
ClearRect.i16YMin = 0;
while(1)
{
EventPosted = Event_pend(DisplayEvents,
Event_Id_NONE,
Event_Id_00,
10);
if (EventPosted & Event_Id_00)
{
if (Mailbox_pend(DisplayMailbox, &MessageObject, BIOS_NO_WAIT))
{
GrContextForegroundSet(&g_sContext, 0x00);
GrRectFill(&g_sContext, &ClearRect);
if (MessageObject.ScreenID == 0)
{
FrameDraw(&g_sContext, "Festo Station - Stopped");
GrStringDraw(&g_sContext, "Press [Up] to start.", -1, 10, 30, 0);
GrStringDraw(&g_sContext, "Press [Down] to stop.", -1, 10, 50, 0);
GrStringDraw(&g_sContext, "Press [Select] to calibrate.", -1, 10, 70, 0);
//Footer
sprintf(StringBuffer, "Uptime: %d [s]", MessageObject.uptimeSeconds);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 180, 0);
sprintf(StringBuffer, "Time: %s", MessageObject.timeString);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 200, 0);
}
if (MessageObject.ScreenID == 1)
{
FrameDraw(&g_sContext, "Festo Station - Running");
sprintf(StringBuffer, "Pieces processed = %d", MessageObject.piecesProcessed);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 30, 0);
sprintf(StringBuffer, "Orange A/R = %d/%d", MessageObject.orangeAccepted, MessageObject.orangeRejected);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 50, 0);
sprintf(StringBuffer, "Black A/R = %d/%d", MessageObject.blackAccepted, MessageObject.blackRejected);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 70, 0);
sprintf(StringBuffer, "Plastic A/R = %d/%d", MessageObject.plasticAccepted, MessageObject.plasticRejected);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 90, 0);
sprintf(StringBuffer, "Metallic Accepted/Rejected = %d/%d", MessageObject.metalAccepted, MessageObject.metalRejected);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 110, 0);
sprintf(StringBuffer, "Pieces processed/min = %.2f [p/min]", (float) 0.01 * MessageObject.piecesProcessedPerSecond);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 130, 0);
//Footer
sprintf(StringBuffer, "Uptime: %d [s]", MessageObject.uptimeSeconds);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 180, 0);
sprintf(StringBuffer, "Time: %s", MessageObject.timeString);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 200, 0);
}
if (MessageObject.ScreenID == 2)
{
FrameDraw(&g_sContext, "Festo Station - Calibration");
GrStringDraw(&g_sContext, "Initializing Calibration...", -1, 10, 30, 0);
//Footer
sprintf(StringBuffer, "Uptime: %d [s]", MessageObject.uptimeSeconds);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 180, 0);
sprintf(StringBuffer, "Time: %s", MessageObject.timeString);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 200, 0);
}
if (MessageObject.ScreenID == 3)
{
FrameDraw(&g_sContext, "Festo Station - Calibration");
//Body
GrStringDraw(&g_sContext, "Put the standard piece on platform and", -1, 10, 30, 0);
GrStringDraw(&g_sContext, "press [Select].", -1, 10, 50, 0);
//Footer
sprintf(StringBuffer, "Uptime: %d [s]", MessageObject.uptimeSeconds);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 180, 0);
sprintf(StringBuffer, "Time: %s", MessageObject.timeString);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 200, 0);
}
if (MessageObject.ScreenID == 4)
{
FrameDraw(&g_sContext, "Festo Station - Calibration");
//Body
GrStringDraw(&g_sContext, "Set the height using [Up] and [Down].", -1, 10, 30, 0);
GrStringDraw(&g_sContext, "When finished, press [Select].", -1, 10, 50, 0);
sprintf(StringBuffer, "Height = %.2f [mm]", (float) MessageObject.heightCalibrated / 10.0);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 120, 0);
//Footer
sprintf(StringBuffer, "Uptime: %d [s]", MessageObject.uptimeSeconds);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 180, 0);
sprintf(StringBuffer, "Time: %s", MessageObject.timeString);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 200, 0);
}
if (MessageObject.ScreenID == 5)
{
FrameDraw(&g_sContext, "Festo Station - Calibration");
//Body
GrStringDraw(&g_sContext, "Set the upper limit using [Up] and", -1, 10, 30, 0);
GrStringDraw(&g_sContext, "[Down]. When finished, press [Select].", -1, 10, 50, 0);
sprintf(StringBuffer, "Upper Limit = %.2f [mm]", (float) MessageObject.upperHeightCalibrated / 10.0);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 120, 0);
//Footer
sprintf(StringBuffer, "Uptime: %d [s]", MessageObject.uptimeSeconds);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 180, 0);
sprintf(StringBuffer, "Time: %s", MessageObject.timeString);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 200, 0);
}
if (MessageObject.ScreenID == 6)
{
FrameDraw(&g_sContext, "Festo Station - Calibration");
//Body
GrStringDraw(&g_sContext, "Set the lower limit using [Up] and", -1, 10, 30, 0);
GrStringDraw(&g_sContext, "[Down]. When finished, press [Select].", -1, 10, 50, 0);
sprintf(StringBuffer, "Lower Limit = %.2f [mm]", (float) MessageObject.lowerHeightCalibrated / 10.0);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 120, 0);
//Footer
sprintf(StringBuffer, "Uptime: %d [s]", MessageObject.uptimeSeconds);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 180, 0);
sprintf(StringBuffer, "Time: %s", MessageObject.timeString);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 200, 0);
}
if (MessageObject.ScreenID == 7)
{
FrameDraw(&g_sContext, "Festo Station - Calibration");
// Body
GrStringDraw(&g_sContext, "The Festo Station is calibrated!", -1, 10, 30, 0);
//Footer
sprintf(StringBuffer, "Uptime: %d [s]", MessageObject.uptimeSeconds);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 180, 0);
sprintf(StringBuffer, "Time: %s", MessageObject.timeString);
GrStringDraw(&g_sContext, StringBuffer, -1, 10, 200, 0);
}
}
}
Task_sleep(16);
}
}
//*****************************************************************************
//
//! 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);
}
}
void osResetEvent(OsEvent *event)
{
//Force the specified event to the nonsignaled state
Event_pend(event->handle, Event_Id_00, Event_Id_NONE, BIOS_NO_WAIT);
}
void TSK_LCD(UArg a0, UArg a1)
{
// System_printf("enter taskFxn()\n");
char msg[16];
short index = 0;
while (1)
{
Event_pend(EVT_LCD_CLK,Event_Id_00, Event_Id_NONE,BIOS_WAIT_FOREVER);
if(LCD_S.init == 1)
{
LCD_Clear();
LCD_Cursor(0,0);
if (Bike.Mode == MODE_CHARGE)
{
msg[0] = 'H';
msg[1] = '\0';
scib_msg(msg);
LCD_Cursor(1,0);
double2string((double)(bq_pack.highest_cell_volts/1000.0), 2.0, msg);
scib_msg(msg);
msg[0] = ' ';
msg[1] = 'L';
msg[2] = '\0';
scib_msg(msg);
double2string((double)(bq_pack.lowest_cell_volts/1000.0), 2.0, msg);
scib_msg(msg);
msg[0] = ' ';
msg[1] = 'T';
msg[2] = '\0';
scib_msg(msg);
unsigned short temp = bq_pack.highest_temp;
ltoa(temp,msg);
msg[2] = '\0';
scib_msg(msg);
}
if(Bike.Mode == MODE_RACE)
{
ltoa((long)(W2000.Odometer),msg); //mph
scib_msg(msg);
scib_msg(":\0");
ltoa((long)(W2000.PhaseATemp),msg); //degree C
scib_msg(msg);
scib_msg(":\0");
ltoa((long)(W2000.MotorVelocity),msg); //amps
}
LCD_Cursor(0,1);
if(Bike.Fault.all != 0)
{
index = 0;
if(Bike.Fault.bit.precharge != 0 && index < 9)
{
msg[0] = 'F';
msg[1] = ':';
msg[2] = 'P';
msg[3] = 'R';
msg[4] = 'E';
msg[5] = 'C';
msg[6] = 'H';
msg[7] = ' ';
msg[8] = '\0';
scib_msg(msg);
index = index + 8;
}
if(Bike.Fault.bit.BMM_Init != 0 && index < 11)
{
msg[0] = 'F';
msg[1] = ':';
msg[2] = 'B';
msg[3] = 'M';
msg[4] = 'M';
msg[5] = ' ';
msg[6] = '\0';
scib_msg(msg);
index = index + 6;
}
if(Bike.Fault.bit.OV != 0 && index < 12)
{
msg[0] = 'F';
msg[1] = ':';
msg[2] = 'O';
msg[3] = 'V';
msg[7] = ' ';
msg[8] = '\0';
scib_msg(msg);
index = index + 5;
}
if(Bike.Fault.bit.UV != 0 && index < 12)
{
msg[0] = 'F';
msg[1] = ':';
msg[2] = 'U';
msg[3] = 'V';
msg[7] = ' ';
msg[8] = '\0';
scib_msg(msg);
index = index + 5;
}
if(Bike.Fault.bit.estop != 0 && index < 9)
{
msg[0] = 'F';
msg[1] = ':';
msg[2] = 'E';
msg[3] = 'S';
msg[4] = 't';
msg[5] = 'o';
msg[6] = 'p';
msg[7] = ' ';
msg[8] = '\0';
//strcpy(msg,"F:EStop ");
scib_msg(msg);
index = index + 8;
}
if(Bike.Fault.bit.Charge_OC != 0 && index < 10)
{
msg[0] = 'F';
msg[1] = ':';
msg[2] = 'C';
msg[3] = 'h';
msg[4] = 'r';
msg[5] = 'g';
msg[6] = ' ';
msg[7] = '\0';
scib_msg(msg);
index = index + 7;
}
if(Bike.Fault.bit.Dis_OC != 0 && index < 11)
{
msg[0] = 'F';
msg[1] = ':';
msg[2] = 'D';
msg[3] = 'i';
msg[4] = 's';
msg[5] = ' ';
msg[6] = '\0';
scib_msg(msg);
index = index + 6;
}
if(Bike.Fault.bit.Switch_Error != 0 && index < 9)
{
msg[0] = 'F';
msg[1] = ':';
msg[2] = 'S';
msg[3] = '_';
msg[4] = 'E';
msg[5] = 'r';
msg[6] = 'r';
msg[7] = ' ';
msg[8] = '\0';
scib_msg(msg);
index = index + 8;
}
}
else if(Bike.Warning.all != 0)
{
index = 0;
if(Bike.Warning.bit.BMM_loss != 0 && index < 11)
{
msg[0] = 'W';
msg[1] = ':';
msg[2] = 'B';
msg[3] = 'M';
msg[4] = 'M';
msg[5] = ' ';
msg[6] = '\0';
scib_msg(msg);
index = index + 6;
}
if(Bike.Warning.bit.w2000_off != 0 && index < 9)
{
msg[0] = 'W';
msg[1] = ':';
msg[2] = 'W';
msg[3] = '2';
msg[4] = '0';
msg[5] = '0';
msg[6] = '0';
msg[7] = ' ';
msg[8] = '\0';
scib_msg(msg);
index = index + 8;
}
if(Bike.Warning.bit.bus_volt != 0 && index < 9)
{
msg[0] = 'W';
msg[1] = ':';
msg[2] = 'B';
msg[3] = 'V';
msg[4] = 'O';
msg[5] = 'L';
msg[6] = 'T';
msg[7] = ' ';
msg[8] = '\0';
scib_msg(msg);
index = index + 8;
}
if(Bike.Warning.bit.current_sense_error != 0 && index < 11)
{
msg[0] = 'W';
msg[1] = ':';
msg[2] = 'C';
msg[3] = 'S';
msg[4] = 'E';
msg[5] = ' ';
msg[6] = '\0';
scib_msg(msg);
index = index + 6;
}
if(Bike.Warning.bit.Charage_OC != 0 && index < 10)
{
msg[0] = 'W';
msg[1] = ':';
msg[2] = 'C';
msg[3] = 'h';
msg[4] = 'r';
msg[5] = 'g';
msg[6] = ' ';
msg[7] = '\0';
scib_msg(msg);
index = index + 7;
}
if(Bike.Warning.bit.Dis_OC != 0 && index < 11)
{
msg[0] = 'W';
msg[1] = ':';
msg[2] = 'D';
msg[3] = 'i';
msg[4] = 's';
msg[5] = ' ';
msg[6] = '\0';
scib_msg(msg);
index = index + 6;
}
if(Bike.Warning.bit.SD_Startup != 0 && index < 10)
{
msg[0] = 'W';
msg[1] = ':';
msg[2] = 'S';
msg[3] = 'D';
msg[4] = '_';
msg[5] = 'S';
msg[6] = ' ';
msg[7] = '\0';
scib_msg(msg);
index = index + 7;
}
if(Bike.Warning.bit.SD_Write != 0 && index < 10)
{
msg[0] = 'W';
msg[1] = ':';
msg[2] = 'S';
msg[3] = 'D';
msg[4] = '_';
msg[5] = 'W';
msg[6] = ' ';
msg[7] = '\0';
scib_msg(msg);
index = index + 7;
}
if(Bike.Warning.bit.Capacity != 0 && index < 11)
{
msg[0] = 'W';
msg[1] = ':';
msg[2] = 'C';
msg[3] = 'a';
msg[4] = 'p';
msg[5] = ' ';
msg[6] = '\0';
scib_msg(msg);
index = index + 6;
}
}
else
{
if(Bike.Mode == MODE_RACE)
{
//scib_msg("Vel:\0");
ltoa((long)(W2000.VehicleVelocity*(2.2)),msg); //mph
scib_msg(msg);
scib_msg(":\0");
ltoa((long)(W2000.MotorTemp),msg); //degree C
scib_msg(msg);
scib_msg(":\0");
ltoa((long)(W2000.BusCurrent),msg); //amps
scib_msg(msg);
}
else if (Bike.Mode == MODE_CHARGE)
{
scib_msg("V:\0");
ltoa((long)W2000.BusVoltage,msg);
scib_msg(msg);
scib_msg("C:\0");
ltoa((long)(W2000.BusCurrent),msg); //amps
scib_msg(msg);
}
/*
FOR REFERENCE
scib_msg("MBV:\0");
ltoa((long)W2000.BusVoltage,msg);
scib_msg(msg);
scib_msg("BV:\0");
ltoa((long)(bq_pack.voltage/1000),msg);
scib_msg(msg);
scib_msg("Vel:\0");
ltoa((long)(W2000.VehicleVelocity*(2.2)),msg); //mph
scib_msg(msg);
scib_msg("MT:\0");
ltoa((long)(W2000.MotorTemp),msg); //degree C
scib_msg(msg);
scib_msg("MC:\0");
ltoa((long)(W2000.BusCurrent),msg); //amps
scib_msg(msg);
*/
}
Task_sleep(2);
}
// System_printf("exit taskFxn()\n");
}
}
uint32_t Event::waitFor(xdc_UInt andMask, xdc_UInt orMask)
{
return Event_pend(EventHandle, andMask, orMask, BIOS_WAIT_FOREVER);
}
