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);
}
