Void LcDTask(UArg arg0, UArg arg1) { LcdObj lcd_message; int index = 0; for(index = 0; index < 15; index++) { lcd_message.buffer[index] = ' '; } lcd_message.position = 1; while(1) { Mailbox_pend(LCD_Mbx, &lcd_message, BIOS_WAIT_FOREVER);// wait/block until post of msg, get msg.val int8_t buff[15] = " "; int loop_index = 0; for(loop_index = 0; loop_index < 15; loop_index++) { buff[loop_index] = lcd_message.buffer[loop_index]; } int position = lcd_message.position; write_lcd(buff,position); } }
void BlinkFxn(UArg arg0) { uint32_t number; Mailbox_Handle mbox = (Mailbox_Handle) arg0; while(1) { Mailbox_pend(mbox,&number, BIOS_WAIT_FOREVER); (void) display_on_matrix(number); } }
/* * ======== 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); }
/* * ======== grlibTaskFxn ======== * Drawing task * * It is pending for the message either from console task or from button ISR. * Once the messages received, it draws to the screen based on information * contained in the message. */ Void grlibTaskFxn(UArg arg0, UArg arg1) { DrawMessage curMsg; const UChar *pucCurImage; const Char *pcCurStr; UInt i = 0; UInt fontHeight = GrStringHeightGet(&context); while (TRUE) { Mailbox_pend(mailboxHandle, &curMsg, BIOS_WAIT_FOREVER); /* Clear screen before drawing */ clearDisplay(); /* Parse the message and draw */ switch (curMsg.drawCommand) { case IMAGE: pucCurImage = image_Gallery[curMsg.drawImageIndex]; /* Draw image at (0,0) */ GrImageDraw(&context, pucCurImage, 0, 0); break; case KeyBoadCOMMAND: output(4,curMsg.draw2048); break; case ScreenCOMMAND: pcCurStr = curMsg.drawString; GrContextForegroundSetTranslated(&context, 0xFF); GrStringDraw(&context, pcCurStr, -1, 0, 0, 0); break; default: break; } } }
Void roveCmdCntrl(UArg arg0, UArg arg1) { //const FOREVER hack to kill the 'unreachable statement' compiler warning extern const uint8_t FOREVER; //initialized in main extern PWM_Handle motor_0; extern PWM_Handle motor_1; extern PWM_Handle motor_2; extern PWM_Handle motor_3; extern PWM_Handle motor_4; extern PWM_Handle motor_5; base_station_msg_struct fromBaseMsg; char commandBuffer[MAX_COMMAND_SIZE + 4]; int messageSize; int deviceJack; int motor_speed = 0; int16_t arm_speed = 0; int i = 0; System_printf("roveCmdCntrlr init! \n\n"); System_flush(); while (FOREVER) { // System_printf("CmdCntrl Is PENDING FOR MAIL!\n\n"); // System_flush(); Mailbox_pend(fromBaseStationMailbox, &fromBaseMsg, BIOS_WAIT_FOREVER); switch (fromBaseMsg.id) { // case 0 hack to make a happy switch case 0: break; case motor_right_id: //the left motors must be the negative of the right motors. Their phase is backwards motor_speed = -(((struct motor_control_struct*) (&fromBaseMsg))->speed); DriveMotor(motor_0, motor_speed); DriveMotor(motor_1, motor_speed); DriveMotor(motor_2, -motor_speed); break; // end drive motor_right_id case motor_left_id: motor_speed = (((struct motor_control_struct*) (&fromBaseMsg))->speed); DriveMotor(motor_3, -motor_speed); DriveMotor(motor_4, motor_speed); DriveMotor(motor_5, -motor_speed); break; //end drive motor_left_id case bms_emergency_command_id: if((((struct bms_emergency_command*) (&fromBaseMsg)) -> command) == 1) { digitalWrite(SOFT_RESET_GPIO_PIN, HIGH); } break; default: deviceJack = getDeviceJack(fromBaseMsg.id); // flag for invalid struct size if (getStructSize(fromBaseMsg.id) != -1) { messageSize = buildSerialStructMessage((void *) &fromBaseMsg, commandBuffer); deviceWrite(deviceJack, commandBuffer, messageSize); } break; } //endswitch //debugging only: // i = 0; // // System_printf("Cmd Cntrl Just Sent!: "); // // while (i < (messageSize)) { // // System_printf("%d, ", commandBuffer[i]); // i++; // // } //end while } System_flush(); System_printf("Rove Cmd Cntrl Task Error: Forced Exit\n"); System_flush(); Task_exit(); } //endfnct: roveCmdCntrl() Task Thread
/** * /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 } } }
/** 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); } }
/* * ======== grlibTaskFxn ======== * Drawing task * * It is pending for the message either from console task or from button ISR. * Once the messages received, it draws to the screen based on information * contained in the message. */ Void grlibTaskFxn(UArg arg0, UArg arg1) { DrawMessage curMsg; const UChar *pucCurImage; UInt key; UInt fontHeight = GrStringHeightGet(&context); while (TRUE) { Mailbox_pend(mailboxHandle, &curMsg, BIOS_WAIT_FOREVER); fillBox(4); /* Clear screen before drawing */ clearDisplay(); /* Parse the message and draw */ switch (curMsg.drawCommand) { case IMAGE: pucCurImage = image_Gallery[curMsg.drawImageIndex]; /* Draw image at (0,0) */ GrImageDraw(&context, pucCurImage, 0, 0); break; case KeyBoadCOMMAND: switch(curMsg.MoveCommand){ case UP: // copy the data to message key = Gate_enterSystem(); up_remove_blank(4); up(4); copyFrom2048ArrayToDrawMessage(4,curMsg.draw2048 , Array2048); Gate_leaveSystem(key); break; case DOWN: // copy the data to message key = Gate_enterSystem(); down_remove_blank(4); down(4); copyFrom2048ArrayToDrawMessage(4,curMsg.draw2048 , Array2048); Gate_leaveSystem(key); break; case LEFT: // copy the data to message key = Gate_enterSystem(); left_remove_blank(4); left(4); copyFrom2048ArrayToDrawMessage(4,curMsg.draw2048 , Array2048); Gate_leaveSystem(key); break; case RIGHT: // copy the data to message key = Gate_enterSystem(); right_remove_blank(4); right(4); copyFrom2048ArrayToDrawMessage(4,curMsg.draw2048 , Array2048); Gate_leaveSystem(key); break; default: break; } output(4,curMsg.draw2048); break; case ScreenCOMMAND: // copy the data to message key = Gate_enterSystem(); fillBox(4); Gate_leaveSystem(key); output(4,curMsg.draw2048); System_printf("here\n"); // printf("data X %d , data Y %d /n",curMsg.touchPosition[0],curMsg.touchPosition[1]); break; default: break; } } }
Void adcCalc(UArg arg0, UArg arg1) { AdcObj adc_message; adc_message.x = 0; adc_message.y = 0; adc_message.z = 0; LcdObj lcd_message; int index = 0; for(index = 0; index < 15; index++) { lcd_message.buffer[index] = ' '; } lcd_message.position = 3; int old_diffs_x[50]; int old_diffs_y[50]; int old_diffs_z[50]; for(index = 0; index < 50; index++) { old_diffs_x[index] = 1000; old_diffs_y[index] = 1000; old_diffs_z[index] = 1000; } int old_x = 0; int old_y = 0; int old_z = 0; int new_x = 0; int new_y = 0; int new_z = 0; int average_x = 0; int average_y = 0; int average_z = 0; int counter = 0; int loop_index = 0; while(1) { Mailbox_pend(ADC_Mbx, &adc_message, BIOS_WAIT_FOREVER); new_x = adc_message.x; new_y = adc_message.y; new_z = adc_message.z; if((old_x - new_x) > 0 ) { old_diffs_x[counter] = old_x - new_x; } else { old_diffs_x[counter] = new_x - old_x; } if((old_y - new_y) > 0 ) { old_diffs_y[counter] = old_y - new_y; } else { old_diffs_x[counter] = new_y - old_y; } if((old_z - new_z) > 0 ) { old_diffs_z[counter] = old_z - new_z; } else { old_diffs_z[counter] = new_z - old_z; } old_x = new_x; old_y = new_y; old_z = new_z; counter++; if(counter > 49) { int sum_x = 0; int sum_y = 0; int sum_z = 0; for(loop_index = 0; loop_index < counter; loop_index++) { sum_x = sum_x + old_diffs_x[loop_index]; } average_x = sum_x/(counter+1); for(loop_index = 0; loop_index < counter; loop_index++) { sum_y = sum_y + old_diffs_y[loop_index]; } average_y = sum_y/(counter+1); for(loop_index = 0; loop_index < counter; loop_index++) { sum_z = sum_z + old_diffs_z[loop_index]; } average_z = sum_z/(counter+1); int index = 0; for(index = 0; index < 15; index++) { lcd_message.buffer[index] = ' '; } combine_ints_to_string(average_x, average_y, average_z, 12,lcd_message.buffer); counter = 0; lcd_message.position = 3; Mailbox_post(LCD_Mbx, &lcd_message, BIOS_WAIT_FOREVER); } } }
uint8_t mailbox_test_pop_message (mailbox_test_sample_message *m){ return Mailbox_pend(logging_mailbox, m, BIOS_NO_WAIT); }