//Hwi to handle adc14 interript
void ADC14_IRQHandler(void)
{
uint64_t status;
AdcObj adc_message;
adc_message.x = 0;
adc_message.y = 0;
adc_message.z = 0;
MAP_Interrupt_disableInterrupt(INT_ADC14);
status = MAP_ADC14_getEnabledInterruptStatus();
MAP_ADC14_clearInterruptFlag(status);
/* ADC_MEM2 conversion completed */
if(status & ((uint32_t)0x00000004))
{
/* Store ADC14 conversion results */
adc_message.x = ADC14_getResult(ADC_MEM0);
adc_message.y = ADC14_getResult(ADC_MEM1);
adc_message.z = ADC14_getResult(ADC_MEM2);
adc_message.z = ADC14_getResult(ADC_MEM2);
Mailbox_post(ADC_Mbx, &adc_message, BIOS_WAIT_FOREVER);
}
}
void mavlink_rx(SerialDevice *dev){
COMM_FRAME frame;
frame.direction = CHANNEL_IN;
frame.channel = CHANNEL_APP_UART;
int c;
while((c = serial_getc(dev)) >= 0) {
if(mavlink_parse_char(CHANNEL_APP_UART, (uint8_t)c, &(frame.mavlink_message), &mavlink_status)){
// --> deal with received message...
Mailbox_post(comm_mailbox, &frame, BIOS_NO_WAIT);
}
}
}
/*
* ======== gpioButtonFxn1 ========
* Callback function for the right button
*
* It posts a message with image index 1 to display.
*/
Void gpioButtonFxn1(Void)
{
UInt key;
DrawMessage drawMsg;
drawMsg.drawCommand = IMAGE;
drawMsg.drawImageIndex = 1;
key = Gate_enterSystem();
/* Clear the last command */
lastCommand[0] = 0x0;
Gate_leaveSystem(key);
/* Do not wait if there is no room for the new mail */
Mailbox_post(mailboxHandle, &drawMsg, BIOS_NO_WAIT);
GPIO_clearInt(EK_TM4C123GXL_SW2);//??
}
/*
* ======== writer ========
*/
Void writer(UArg arg0, UArg arg1)
{
MsgObj msg;
Int i;
for (i=0; i < NUMMSGS; i++) {
/* fill in value */
msg.id = i;
msg.val = i + 'a';
System_printf("writing message id = %d val = '%c' ...\n",
msg.id, msg.val);
/* enqueue message */
Mailbox_post(mbx, &msg, TIMEOUT);
}
System_printf("writer done.\n");
}
/*
* ======== consoleTaskFxn ========
* Console task
*
* This task listens to the key pressed in the keyboard through USBCDC.
* The string ended with return character '\n' will trigger the task
* to send this string to the mailbox.
* For example, when the user enter "ls\n", this task will scan all the
* files in the root of SD card and send the file list to the mailbox to
* inform the drawing task to display on the screen.
* The up/down arrow can be used to scroll up/down to display more files
* in the SD card.
*/
Void consoleTaskFxn (UArg arg0, UArg arg1)
{
unsigned int count;
unsigned int cpuLoad;
char input[128];
UInt key;
DrawMessage drawMsg;
count = 1;
/* printf goes to the UART com port */
printf("\f======== Welcome to the Console ========\n");
printf("Enter a command followed by return.\n"
"Type help for a list of commands.\n\n");
printf("%d %% ", count++);
fflush(stdout);
/* Loop forever receiving commands */
while(true) {
/* Get the user's input */
scanf("%s", input);
/* Flush the remaining characters from stdin since they are not used. */
fflush(stdin);
if (!strcmp(input, "a")) {
/* Print the CPU load */
cpuLoad = Load_getCPULoad();
printf("CPU Load: %d\n", cpuLoad);
drawMsg.drawCommand = KeyBoadCOMMAND;
up_remove_blank(4);
up(4);
// copy the data to message
key = Gate_enterSystem();
copyFrom2048ArrayToDrawMessage(4,drawMsg.draw2048 , Array2048);
Gate_leaveSystem(key);
Mailbox_post(mailboxHandle, &drawMsg, BIOS_WAIT_FOREVER);
}
else if (!strcmp(input, "d")) {
/* Print the CPU load */
cpuLoad = Load_getCPULoad();
printf("CPU Load: %d\n", cpuLoad);
drawMsg.drawCommand = KeyBoadCOMMAND;
down_remove_blank(4);
down(4);
// copy the data to message
key = Gate_enterSystem();
copyFrom2048ArrayToDrawMessage(4,drawMsg.draw2048 , Array2048);
Gate_leaveSystem(key);
Mailbox_post(mailboxHandle, &drawMsg, BIOS_WAIT_FOREVER);
} else if (!strcmp(input, "w")) {
/* Print the CPU load */
cpuLoad = Load_getCPULoad();
printf("CPU Load: %d\n", cpuLoad);
drawMsg.drawCommand = KeyBoadCOMMAND;
left_remove_blank(4);
left(4);
// copy the data to message
key = Gate_enterSystem();
copyFrom2048ArrayToDrawMessage(4,drawMsg.draw2048 , Array2048);
Gate_leaveSystem(key);
Mailbox_post(mailboxHandle, &drawMsg, BIOS_WAIT_FOREVER);
} else if (!strcmp(input, "s")) {
/* Print the CPU load */
cpuLoad = Load_getCPULoad();
printf("CPU Load: %d\n", cpuLoad);
drawMsg.drawCommand = KeyBoadCOMMAND;
right_remove_blank(4);
right(4);
// copy the data to message
key = Gate_enterSystem();
copyFrom2048ArrayToDrawMessage(4,drawMsg.draw2048 , Array2048);
Gate_leaveSystem(key);
Mailbox_post(mailboxHandle, &drawMsg, BIOS_WAIT_FOREVER);
}
else if (!strcmp(input, "exit")) {
/* Exit the console task */
printf("Are you sure you want to exit the console? Y/N: ");
fflush(stdout);
scanf("%s", input);
fflush(stdin);
if ((input[0] == 'y' || input[0] == 'Y') && input[1] == 0x00) {
printf("Exiting console, goodbye.\n");
Task_exit();
}
}
else {
/* Print a list of valid commands. */
printf("Valid commands:\n"
"- w: move up.\n"
"- a: move left.\n"
"- s: move down.\n"
"- a: move right.\n"
"- exit: Exit the console task.\n");
}
fillBox(4);
// printf("%d %% ", count++);
fflush(stdout);
}
}
//*****************************************************************************
//
//! 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);
}
}
//*****************************************************************************
//
//! 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 uartFxn(UArg arg0, UArg arg1)
{
int8_t input[] = " ";
LcdObj lcd_message;
int index = 0;
for(index = 0; index < 15; index++) {
lcd_message.buffer[index] = ' ';
}
lcd_message.position = 1;
UART_Handle uart;
UART_Params uartParams;
/* Create a UART with data processing off. */
UART_Params_init(&uartParams);
uartParams.writeDataMode = UART_DATA_BINARY;
uartParams.readDataMode = UART_DATA_BINARY;
uartParams.readReturnMode = UART_RETURN_FULL;
uartParams.readEcho = UART_ECHO_OFF;
uartParams.baudRate = 9600;
uart = UART_open(Board_UART0, &uartParams);
if (uart == NULL) {
System_abort("Error opening the UART");
}
Semaphore_pend(start_data_semaphore, BIOS_WAIT_FOREVER); // wait on button semaphore
//send this char for handshaking with PC
int8_t prompt = 65;
UART_write(uart, &prompt, 1);
int total_x = 0;
int total_y = 0;
int total_z = 0;
int average_x = 0;
int average_y = 0;
int average_z = 0;
int counter = 0;
int rx = 0;
while (1) {
UART_read(uart, &input, 2);
if(input[0] == '$' && input[1] == '$') {
P2OUT &= ~BIT1;
P1OUT |= BIT0;
BIOS_exit(1);
}
rx = (int)input[1];
if(input[0] == '-') {
rx = 0 - rx;
}
total_x = total_x + rx;
UART_read(uart, &input, 2);
if(input[0] == '$' && input[1] == '$') {
P2OUT &= ~BIT1;
P1OUT |= BIT0;
BIOS_exit(1);
}
rx = (int)input[1];
if(input[0] == '-') {
rx = 0 - rx;
}
total_y = total_y + rx;
UART_read(uart, &input, 2);
if(input[0] == '$' && input[1] == '$') {
P2OUT &= ~BIT1;
P1OUT |= BIT0;
BIOS_exit(1);
}
rx = (int)input[1];
if(input[0] == '-') {
rx = 0 - rx;
}
total_z = total_z + rx;
/*
if((counter % 3) == 0) {
total_x = total_x + rx;
} else if ((counter % 3) == 1) {
total_y = total_y + rx;
} else {
total_z = total_z + rx;
}*/
counter++;
if(counter > 50) {
average_x = total_x/counter;
average_y = total_y/counter;
average_z = total_z/counter;
combine_ints_to_string(average_x, average_y, average_z, 15,lcd_message.buffer);
counter = 0;
total_x = 0;
total_y = 0;
total_z = 0;
lcd_message.position = 1;
Mailbox_post(LCD_Mbx, &lcd_message, BIOS_WAIT_FOREVER);
}
MAP_Interrupt_enableInterrupt(INT_ADC14);
}
}
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_post_message(mailbox_test_sample_message *m){
return Mailbox_post(logging_mailbox, m, BIOS_NO_WAIT);
}
