void printTime()
{
String buf;
uint32_t curSeconds;
int hours, minutes, seconds;
SPI.begin();
myScreen.clearBuffer();
myScreen.text(5, 10, "Energia");
curSeconds = Seconds_get();
seconds = (int) curSeconds % 60;
minutes = (int) ((curSeconds / 60) % 60);
hours = (int) ((curSeconds / (60*60)) % 24);
if (hours < 10) {
buf += "0";
}
buf += hours;
buf += ":";
if (minutes < 10) {
buf += "0";
}
buf += minutes;
buf += ":";
if (seconds < 10) {
buf += "0";
}
buf += seconds;
myScreen.text(1, 40, buf);
myScreen.text(25, 70, "Time");
myScreen.flush();
SPI.end(); /* to save Power */
}
//*****************************************************************************
//
//! 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);
}
}
