void TestMain(void) {
FIFO buzz, lcd;
buzz = OS_InitFiFo();
lcd = OS_InitFiFo();
_sys_init_lcd();
OS_InitSem(S_BUZZ,1);
OS_InitSem(S_BUZZ_FIFO,1);
OS_InitSem(S_BUZZ_OUTPUT,1);
OS_InitSem(S_PORTE,1);
OS_InitSem(S_LCD,1);
/* Prints the operating system name and plays SOS though the speaker. */
OS_Wait(S_BUZZ_OUTPUT);
OS_Create(PrintLogo, 0, DEVICE, 800);
OS_Yield();
OS_Wait(S_BUZZ_OUTPUT);
/* Servers */
OS_Create(FIFOBuzz, buzz, DEVICE, 800); /* Beep in morse code, charactars from fifo. */
OS_Create(ReadLightSensors, buzz, DEVICE, 200); /* Write values representing the light sensors into the fifo */
OS_Create(ReadMicrophone, buzz, DEVICE, 500); /* Write values representing the sound level into the fifo */
OS_Create(ReadBumpers, lcd, DEVICE, 10); /* Reads bumper values into a FIFO, and moves the robot accordingly. */
/* Printing is too slow... */
//OS_Create(PrintBumperValue, lcd, PERIODIC, 40); /* Prints the bumper values to the screen. */
OS_Signal(S_BUZZ_OUTPUT);
}
// input ASCII character from UART
// block if RxFifo is empty
char UART_InChar(void){
char letter;
while(RxFifo_Get(&letter) == FIFOFAIL){
OS_Wait(&RxDataAvailable);
}
return(letter);
}
void Threader2(void){
Count2 = 0;
for(;;){
OS_Wait(&Ready);
Count2++;
}
}
void Thread5c(void){
for(;;){
OS_Wait(&Readyc);
Count5++; // Count2 + Count5 should equal Count1
Lost = Count1-Count5-Count2;
}
}
void PrintString (void) {
/* Printing takes a long time. Don't do it while buzzing because it will introduce long pauses. */
OS_Wait(S_BUZZ_OUTPUT);
sys_print_lcd((char *)OS_GetParam());
OS_Signal(S_BUZZ_OUTPUT);
OS_Signal(S_LCD);
}
// ******** RxFifo_Get ************
// Remove one character from the RxFifo
// Called in foreground, will wait if empty
// Inputs: pointer to place to save 8 bit data
// Outputs: none
void RxFifo_Get(unsigned char *datapt){
OS_Wait(&RxAvailable);
*datapt = *RxGetPt; // remove data
RxGetPt++; // place to get next
if (RxGetPt==&RxFifo[RXFIFOSIZE]){
RxGetPt = &RxFifo[0]; // wrap
}
}
// output ASCII character to UART
// block if TxFifo is full
void UART_OutChar(char data){
while(TxFifo_Put(data) == FIFOFAIL){
OS_Wait(&TxRoomLeft);
}
UART0_IM_R &= ~UART_IM_TXIM; // disable TX FIFO interrupt
copySoftwareToHardware();
UART0_IM_R |= UART_IM_TXIM; // enable TX FIFO interrupt
}
void Thread2d(void){
OS_InitSemaphore(&Readyd,0);
Count1 = 0;
Count2 = 0;
for(;;){
OS_Wait(&Readyd);
Count2++;
}
}
void ReadLightSensors(void) {
FIFO f = (FIFO)OS_GetParam();
int l, r;
while (1) {
l = 0;
r = 0;
OS_Wait(S_PORTE);
/* Activate A/D Converter...makes pins on Port E analog. */
Ports[M6811_OPTION] SET_BIT(BIT7);
/* Delay at least 100 microseconds */
OS_Yield();
/* Start converting the right photocell */
Ports[M6811_ADCTL] = 0;
/* Wait for conversion to complete. */
while (!(Ports[M6811_ADCTL] & BIT7)) {
OS_Yield();
}
r = Ports[M6811_ADR1];
/* Start converting the left photocell */
Ports[M6811_ADCTL] = 1;
/* Wait for conversion to complete. */
while (!(Ports[M6811_ADCTL] & BIT7)) {
OS_Yield();
}
l = Ports[M6811_ADR1];
OS_Signal(S_PORTE);
if (l > 100) {
OS_Wait(S_BUZZ_FIFO);
OS_Write(f,'l');
}
if (r > 100) {
OS_Wait(S_BUZZ_FIFO);
OS_Write(f,'r');
}
OS_Yield();
}
}
void Threader1(void){
Count1 = 0;
for(;;){
OS_Wait(&Ready);
Count1++;
OS_Signal(&Ready);
OS_Suspend();
}
}
void MotorY(void){
for(;;){
OS_Wait(numPulsosY>0);
if(dirMotorY==32) output_high(dirY);
if(dirMotorY==16) output_low(dirY);
if(--numPulsosY!=0) output_toggle(clockY);
else output_low(clockY);
OS_Delay(periodoPulsosY);
OS_Yield();
}
}
void MotorX(void){
for(;;){
OS_Wait(numPulsosX>0);
if(dirMotorX==64) output_high(dirX);
if(dirMotorX==128) output_low(dirX);
if(--numPulsosX!=0) output_toggle(clockX);
else output_low(clockX);
OS_Delay(periodoPulsosX);
OS_Yield();
}
}
void MotorZ(void){
for(;;){
OS_Wait(numPulsosZ>0);
if(dirMotorZ==8) output_high(dirZ);
if(dirMotorZ==4) output_low(dirZ);
if(--numPulsosZ!=0) output_toggle(clockZ);
else output_low(clockZ);
OS_Delay(periodoPulsosZ);
OS_Yield();
}
}
void Write1 (void) {
FIFO f = (FIFO)OS_GetParam();
OS_Wait(S_LOGO);
OS_Write(f,(int)'l');
OS_Yield();
OS_Write(f,(int)'O');
OS_Yield();
OS_Write(f,(int)'S');
OS_Signal(S_LOGO);
}
void MotorE(void){
for(;;){
OS_Wait(numPulsosE>0);
if(dirMotorE==1) output_high(dirE);
if(dirMotorE==2) output_low(dirE);
if(--numPulsosE!=0) output_toggle(clockE);
else output_low(clockE);
OS_Delay(periodoPulsosE);
OS_Yield();
}
}
//================================================================
int OS_file_sig_wait (int mode, void *data) {
//================================================================
// wait for signalfile & get its content
// mode 1=string, 2=int
/// RetCod: 0 OK, data valid
/// -1 cancelled by user
int i1;
char fn[240], *p1;
printf("OS_file_sig_wait %d\n",mode);
// clear signal-file
sprintf(fn, "%sGDW1.sig", OS_get_tmp_dir());
// printf(" fn=|%s|\n",fn);
OS_file_sig_cre (0, fn);
// wait for answer from GUI_DialogEntryCB
for(i1=0; i1<100; ++i1) { // 100 sec
GUI_update__ ();
OS_Wait (200); // millisecs
// printf(" i1=%d\n",i1);
if(OS_checkFilExist(fn, 1) == 0) continue;
goto L_finish;
}
for(;;) {
GUI_update__ ();
OS_Wait (600); // millisecs
if(OS_checkFilExist(fn, 1) == 0) continue;
goto L_finish;
}
// get entry back
L_finish:
mode *= -1; // read
return OS_file_sig_cre (mode, data); // get file-content
}
unsigned long OS_Fifo_Get(void)
{
OS_Wait(&dataAvailable);
OS_bWait(&mutex);
unsigned long toReturn;
toReturn = TxFifo[OS_TxGetI&(TXFIFOSIZE-1)];
OS_TxGetI++; // Success, update
samplesConsumed++;
OS_bSignal(&mutex);
//OS_Signal(&roomLeft);
return toReturn;
}
void Thread2c(void){
OS_InitSemaphore(&Readyc,0);
Count1 = 0; // number of times signal is called
Count2 = 0;
Count5 = 0; // Count2 + Count5 should equal Count1
NumCreated += OS_AddThread(&Thread5c,128,3);
OS_AddPeriodicThread(&BackgroundThread1c,1,TIME_1MS,1);
for(;;){
OS_Wait(&Readyc);
Count2++; // Count2 + Count5 should equal Count1, Count5 may be 0
}
}
//******** OS_ReceiveData ***************
// Get a 16-bit data from the DataFifo
// called from the foreground
// Will wait if the DataFifo is empty
// Inputs: none
// Outputs: data
unsigned short OS_ReceiveData(void){ unsigned short data;
SETBIT(PTT,1); // for profiling
OS_Wait(&DataAvailable);
// must be atomic if more than one consumer
data = (*DataGetPt); // remove data
DataGetPt++; // Place to Put next
if (DataGetPt == &DataFifo[DATAFIFOSIZE]){
DataGetPt = &DataFifo[0];
}
CLRBIT(PTT,1); // for profiling
return(data);
}
// ******** CAN0_Fifo_Get ************
// Remove one data sample from the Fifo
// Called in foreground, will block if empty
// Inputs: none
// Outputs: data
uint8_t CAN0_Fifo_Get(void){
int8_t returnValue; // holds the value poped off of fifo.
OS_Wait(&CAN0CurrentSize); // block for data
OS_bWait(&CAN0FifoMutex); // only one thread can access the fifo at any given time
returnValue = *CAN0GetPt; // pop element off fifo
CAN0GetPt++;
if(CAN0GetPt == &CAN0Fifo[CAN_FIFOSIZE]){
CAN0GetPt = &CAN0Fifo[0]; // wrap around
}
OS_bSignal(&CAN0FifoMutex); //release the fifo
return returnValue;
}
// ******** OS_Fifo_Get ************
// Remove one data sample from the Fifo
// Called in foreground, will spin/block if empty
// Inputs: none
// Outputs: data
unsigned long OS_Fifo_Get(void) {
unsigned long data;
OS_Wait(&CurrentSize);
OS_bWait(&FifoMutex);
data = *(OS_GetPt++);
if(OS_GetPt == &OS_Fifo[FIFOSIZE]) {
OS_GetPt = &OS_Fifo[0];
}
OS_bSignal(&FifoMutex);
return data;
}
void ReadMicrophone(void) {
FIFO f = (FIFO)OS_GetParam();
int s;
while (1) {
s = 0;
OS_Wait(S_PORTE);
/* Activate A/D Converter...makes pins on Port E analog. */
Ports[M6811_OPTION] SET_BIT(BIT7);
/* Delay at least 100 microseconds */
OS_Yield();
/* Don't listen while buzzing. */
OS_Wait(S_BUZZ_OUTPUT);
/* Start converting the microphone. */
Ports[M6811_ADCTL] = 2;
OS_Signal(S_BUZZ_OUTPUT);
/* Wait for conversion to complete. */
while (!(Ports[M6811_ADCTL] & BIT7)) {
OS_Yield();
}
s = Ports[M6811_ADR1];
OS_Signal(S_PORTE);
/* Rectify the sound sample around 128. */
if (s >= 128) { s -= 128; }
else { s = 128 - s; }
if (s > 35) {
OS_Wait(S_BUZZ_FIFO);
OS_Write(f,'s');
}
OS_Yield();
}
}
//******** Display ***************
// foreground thread, accepts data from consumer
// displays calculated results on the LCD
// inputs: none
// outputs: none
void Display(void){
unsigned long data,voltage;
oLED_Message(0,0,"Run length is",(RUNLENGTH)/1000); // top half used for Display
while(NumSamples < RUNLENGTH) {
oLED_Message(0,1,"Time left is",(RUNLENGTH-NumSamples)/1000); // top half used for Display
OS_Wait(&MailBoxFull);
data = OS_MailBox_Recv();
OS_Signal(&MailBoxEmpty);
voltage = 3000*data/1024; // calibrate your device so voltage is in mV
oLED_Message(0,2,"v(mV) =",voltage);
}
OS_Kill(); // done
}
CANmsgType BCAN_get(void)
{
long sr;
CANmsgType data;
OS_Wait(&CANmessagesReceived);
sr = OS_StartCritical();
while(!CAN_RX_FIFOFifo_Get(&data)) //keep checking if the FIFO has data
{
#ifdef OS_COOP_SPINLOCK //if the interrupt fires and stuffs the FIFO
OS_Suspend(); //before the systick triggers a threadswitch
#endif //it will get its data faster (but other threads will have to wait)
}
OS_EndCritical(sr);
return data;
}
void Gla(void){
for(;;){
OS_Wait(GlaoGlc==3);
//OS_Bsem_Wait(BS_GLAGLC_FREE);
output_toggle(PIN_B7);
//-- 1023/255=4.012, necesita adaptarse al mismo rango
//-- porque el valorR que llega desde el GUI tiene max=255
r=(float)(valorR*4.012);
control=(unsigned int16)r;
set_pwm1_duty(control);
OS_Delay(1000);
//OS_Bsem_Set(BS_GLAGLC_FREE);
OS_Yield();
}
}
void Glc_PIDdiscreto(void){
for(;;){
OS_Wait(GlaoGlc==2); //2
//OS_Bsem_Wait(BS_GLAGLC_FREE);
output_toggle(PIN_B7);
valorY=read_adc(ADC_READ_ONLY);
//-- debido al ADC 10bits, la conversion se realiza con 10bits,
//-- entonces valorY tiene rango de 0 a 1023
y=(float)valorY;
//-- 1023/255=4.012, necesita adaptarse al mismo rango
//-- porque el valorR que llega desde el GUI tiene max=255
r=(float)(valorR*4.012);
//----------------------------------------------------------------------
//-- Calculo PID por metodo tustin para termino integral
//-- y metodo de diferencias hacia atras para termino derivativo
//-- Sea e(kT)=e; e(kT-T)=e1
e=r-y;
//Sea p(kT)=p; i(kT)=i; i(kT-T)=i1; d(kT)=d
p=Kpz*e;
//i=i1+Kiz*e; //diferencia hacia atras
i=i1+Kiz*(e+e1); //tustin
d=Kdz*(e-e1); //diferencia hacia atras
//Sea u(kT)=u
u=p+i+d;
//-- Anti-windup solo al termino integral para evitar que se infle
//-- y se haga muy grande si la accion de control se satura, por tanto
//-- es necesario impedir que cambie i
//if((u>max) | (u<min)) {i=i-Ki*T*e;} //diferencia hacia atras
//if((u>max) | (u<min)) i=i-Kiz*(e+e1); //tustin
if(u>max)u=max;
if(u<min)u=min;
//-- realizar la conversion final
control=(unsigned int16)u;
set_pwm1_duty(control);
e1=e;
i1=i;
OS_Delay(5000);
//OS_Bsem_Set(BS_GLAGLC_FREE);
OS_Yield();
}
}
void PrintFIFO (void) {
FIFO f = (FIFO)OS_GetParam();
char *s = " ";
int i;
int fi;
i = 0;
while(i < 6) {
if(OS_Read(f,&fi)) {
s[i++] = (char)fi;
}
else {
OS_Yield();
}
}
OS_Wait(S_LCD);
OS_Create(PrintString, (int)s, PERIODIC, 50);
}
SF_STATUS FIFOPutBlocking(FIFO* fifo, void* item) {
int status = 0;
ASSERT(fifo != NULL && fifo->fifoDataPtr != NULL);
ASSERT(fifo->length < fifo->capacity);
OS_Wait(&(fifo->fifoFree));
// copy in the item
memcpy(((char*)fifo->fifoDataPtr) + fifo->tailIndex*fifo->itemSizeBytes, item,
fifo->itemSizeBytes);
// move the tail index up. wrap around if necessary
fifo->tailIndex += 1;
fifo->tailIndex %= fifo->capacity;
fifo->length ++;
OS_Signal(&(fifo->fifoFree));
return (SUCCESS);
}
SF_STATUS FIFOGetBlocking(FIFO* fifo, void* destination) {
int status = 0;
ASSERT(fifo != NULL && fifo->fifoDataPtr != NULL);
ASSERT(fifo->length > 0);
OS_Wait(&(fifo->fifoFree));
// copy the item
memcpy(destination, ((char*)fifo->fifoDataPtr) + fifo->headIndex*fifo->itemSizeBytes,
fifo->itemSizeBytes);
// move the head index up. wrap around if necesssary
fifo->headIndex += 1;
fifo->headIndex %= fifo->capacity;
fifo->length --;
OS_Signal(&(fifo->fifoFree));
return (SUCCESS);
}
/*
* Adds an item to the FIFO
* fifo - fifo structure that was initialized and created by FIFOCreate
* item - a pointer to the item that is to be added to the FIFO
*
* returns - SUCCESS if putting is successful
* FAILURE if putting is not successful (i.e. the structure is full)
*/
SF_STATUS FIFOPut(FIFO* fifo, void* item) {
int status = 0;
ASSERT(fifo != NULL && fifo->fifoDataPtr != NULL);
ASSERT(fifo->length < fifo->capacity);
ASSERT(fifo->fifoFree.count > 0); // Make sure FIFO is free before continuing
OS_Wait(&(fifo->fifoFree));
//status = StartCritical();
// copy in the item
memcpy(((char*)fifo->fifoDataPtr) + fifo->tailIndex*fifo->itemSizeBytes, item,
fifo->itemSizeBytes);
// move the tail index up. wrap around if necessary
fifo->tailIndex += 1;
fifo->tailIndex %= fifo->capacity;
fifo->length ++;
OS_Signal(&(fifo->fifoFree));
//EndCritical(status);
return (SUCCESS);
}