Exemplo n.º 1
0
// Callback function that is called by the LCDTimer
//   Sends a message to the queue that is read by the LCD Task
void LCDTimerCallback(xTimerHandle pxTimer)
{
	if (pxTimer == NULL) {
		VT_HANDLE_FATAL_ERROR(0);
	} else {
		// When setting up this timer, I put the pointer to the 
		//   LCD structure as the "timer ID" so that I could access
		//   that structure here -- which I need to do to get the 
		//   address of the message queue to send to 
		vtLCDStruct *ptr = (vtLCDStruct *) pvTimerGetTimerID(pxTimer);
		// Make this non-blocking *but* be aware that if the queue is full, this routine
		// will not care, so if you care, you need to check something
		if (SendLCDTimerMsg(ptr,lcdWRITE_RATE_BASE,0) == errQUEUE_FULL) {
			// Here is where you would do something if you wanted to handle the queue being full
			VT_HANDLE_FATAL_ERROR(0);
		}
	}
}
void vStartSomeTask( unsigned portBASE_TYPE uxPriority, i2cTempStruct *params)
{
	/* Start the task */
	portBASE_TYPE retval;

	if ((retval = xTaskCreate( SomeTask, ( signed char * ) "i2cTemp", i2cSTACK_SIZE, (void *) params, uxPriority, ( xTaskHandle * ) NULL )) != pdPASS) {
		VT_HANDLE_FATAL_ERROR(retval);
	}
}
Exemplo n.º 3
0
// This is the actual task that is run
static portTASK_FUNCTION( vConductorUpdateTask, pvParameters )
{
	uint8_t rxLen, status;
	uint8_t Buffer[vtI2CMLen];
	uint8_t *valPtr = &(Buffer[0]);
	
	// Get the parameters
	vtConductorStruct *param = (vtConductorStruct *) pvParameters;
	// Get the I2C device pointer
	vtI2CStruct *devPtr = param->dev;
	// Get the LCD information pointer
	vtVoltStruct *voltData = param->voltData;
	uint8_t recvMsgType;

	// Like all good tasks, this should never exit
	for(;;)
	{
		// Wait for a message from an I2C operation
		if (vtI2CDeQ(devPtr,vtI2CMLen,Buffer,&rxLen,&recvMsgType,&status) != pdTRUE) {
			VT_HANDLE_FATAL_ERROR(0);
		}

		// Decide where to send the message 
		//   This just shows going to one task/queue, but you could easily send to
		//   other Q/tasks for other message types
		// This isn't a state machine, it is just acting as a router for messages
		switch(recvMsgType) {
		case vtI2CMsgTypeVoltInit: {
			SendVoltValueMsg(voltData,recvMsgType,Buffer,0,portMAX_DELAY);
			break;
		}
		case vtI2CMsgTypeVoltRead: {
			SendVoltValueMsg(voltData,recvMsgType,Buffer,8,portMAX_DELAY);
			break;
		}
		default: {
			VT_HANDLE_FATAL_ERROR(recvMsgType);
			break;
		}
		}


	}
}
Exemplo n.º 4
0
// This is the actual task that is run
static portTASK_FUNCTION( vi2cUpdateTask, pvParameters )
{
    // Get the parameters
    param = (myI2CStruct *) pvParameters;
    // Get the I2C device pointer
    devPtr = param->dev;

    // Like all good tasks, this should never exit
    for(;;)
    {
        // Wait for a message from either a timer or from an I2C operation
        if (xQueueReceive(param->inQ,(void *) &msgBuffer,portMAX_DELAY) != pdTRUE) {
            VT_HANDLE_FATAL_ERROR(Q_RECV_ERROR);
        }
        switch(getMsgType(&msgBuffer)) {
            case i2cTimerMsgType: {
                // Poll local 2680 for data
                notifyRequestSent();
                if (vtI2CEnQ(devPtr,vtI2CReadMsgType,SLAVE_ADDR,0,0,I2C_MSG_SIZE) != pdTRUE) {
                    VT_HANDLE_FATAL_ERROR(VT_I2C_Q_FULL);
                }
                break;
            }
            case vtI2CMotorMsgType: {
                // Send motor command to local 2680
                if (vtI2CEnQ(devPtr,vtI2CMotorMsgType,SLAVE_ADDR,msgBuffer.length,msgBuffer.buf,0) != pdTRUE){
                    VT_HANDLE_FATAL_ERROR(VT_I2C_Q_FULL);
                }
                break;
            }
            case notifyRqstRecvdMsgType: {
                if(requestSent == 0){
                    // Send I2C Error Message to Web Server
                }
                requestSent = 0;
                break;
            }
            default: {
                VT_HANDLE_FATAL_ERROR(UNKNOWN_I2C_MSG_TYPE);
                break;
            }
        }
    }
}
Exemplo n.º 5
0
// Public API
void vStartConductorTask(vtConductorStruct *params,unsigned portBASE_TYPE uxPriority, vtI2CStruct *i2c,vtVoltStruct *voltage)
{
	/* Start the task */
	portBASE_TYPE retval;
	params->dev = i2c;
	params->voltData = voltage;
	if ((retval = xTaskCreate( vConductorUpdateTask, ( signed char * ) "Conductor", conSTACK_SIZE, (void *) params, uxPriority, ( xTaskHandle * ) NULL )) != pdPASS) {
		VT_HANDLE_FATAL_ERROR(retval);
	}
}
Exemplo n.º 6
0
portBASE_TYPE SendTempValueMsg(vtTempStruct *tempData,uint8_t msgType,uint8_t *value,portTickType ticksToBlock)
{
	vtTempMsg tempBuffer;

	if (tempData == NULL) {
		VT_HANDLE_FATAL_ERROR(0);
	}
	//TODO: unhard code this shiz
	tempBuffer.length = 2;
	if (tempBuffer.length > vtTempMaxLen) {
		// no room for this message
		VT_HANDLE_FATAL_ERROR(tempBuffer.length);
	}
	//memcpy(tempBuffer.buf,&value,2);
	tempBuffer.buf[0] = value[0];
	tempBuffer.buf[1] = value[1];
	tempBuffer.msgType = msgType;
	return(xQueueSend(tempData->inQ,(void *) (&tempBuffer),ticksToBlock));
}
Exemplo n.º 7
0
Arquivo: LCDtask.c Projeto: benwr/blab
portBASE_TYPE SendLCDPointMsg(vtLCDStruct *lcdData,
                              int length,
                              uint8_t *pString,
                              portTickType ticksToBlock)
{
  if (lcdData == NULL) {
    VT_HANDLE_FATAL_ERROR(0);
  }
  vtLCDMsg lcdBuffer;

  if (length > LCD_MAX_LEN) {
    // no room for this message
    VT_HANDLE_FATAL_ERROR(lcdBuffer.length);
  }
  lcdBuffer.length = strnlen(pString, LCD_MAX_LEN);
  lcdBuffer.msgType = LCDMsgTypePoint;
  strncpy((char *)lcdBuffer.buf, pString, LCD_MAX_LEN);
  return (xQueueSend(lcdData->inQ, (void *)(&lcdBuffer), ticksToBlock));
}
Exemplo n.º 8
0
// Public API
void vStartConductorTask(vtConductorStruct *params,unsigned portBASE_TYPE uxPriority, vtI2CStruct *i2c,vtInfraredStruct *sensorData, vtMotorStruct *motorData)
{
	/* Start the task */
	portBASE_TYPE retval;
	params->dev = i2c;
	params->sensorData = sensorData;
	params->motorData = motorData;
	if ((retval = xTaskCreate( vConductorUpdateTask, ( signed char * ) "Conductor", conSTACK_SIZE, (void *) params, uxPriority, ( xTaskHandle * ) NULL )) != pdPASS) {
		VT_HANDLE_FATAL_ERROR(retval);
	}
}
Exemplo n.º 9
0
portBASE_TYPE AIUpdateDistances(navigationStruct *navData, uint8_t l1, uint8_t l2, uint8_t l3, uint8_t r1, uint8_t r2, uint8_t r3)
{
    if (navData == NULL) {
        VT_HANDLE_FATAL_ERROR(0);
    }
    navigationMsg buffer;
    buffer.length = 6;
    if (buffer.length > maxNavigationMsgLen) {
        // no room for this message
        VT_HANDLE_FATAL_ERROR(INCORRECT_NAVIGATION_MSG_FORMAT);
    }
    buffer.buf[0] = l1;
    buffer.buf[1] = l2;
    buffer.buf[2] = l3;
    buffer.buf[3] = r1;
    buffer.buf[4] = r2;
    buffer.buf[5] = r3;
    buffer.msgType = AIUpdateDistancesMsgType;
    return(xQueueSend(navData->inQ,(void *) (&buffer),portMAX_DELAY));
}
Exemplo n.º 10
0
portBASE_TYPE SendmotorERRORMsg(motorStruct *motorData, uint8_t errorType, portTickType ticksToBlock)
{
	motorMsg errorMsg;

	if (motorData == NULL) {
		VT_HANDLE_FATAL_ERROR(0);
	}
	errorMsg.msgType = errorType;

	return(xQueueSend(motorData->inQ,(void *) (&errorMsg),ticksToBlock));
}
Exemplo n.º 11
0
void vApplicationStackOverflowHook( xTaskHandle *pxTask, signed char *pcTaskName )
{
	/* This function will get called if a task overflows its stack. */

	( void ) pxTask;
	( void ) pcTaskName;

	// MTJ: I have directed this to the fatal error handler
	VT_HANDLE_FATAL_ERROR(0);
	for( ;; );
}
Exemplo n.º 12
0
// Callback function that is called by the TemperatureTimer
//   Sends a message to the queue that is read by the Temperature Task
void adcTimerCallback(xTimerHandle pxTimer)
{
	if (pxTimer == NULL) {
		VT_HANDLE_FATAL_ERROR(0);
	} else {
		// When setting up this timer, I put the pointer to the 
		//   Temperature structure as the "timer ID" so that I could access
		//   that structure here -- which I need to do to get the 
		//   address of the message queue to send to 
		adcStruct *ptr = (adcStruct *) pvTimerGetTimerID(pxTimer);
		// Make this non-blocking *but* be aware that if the queue is full, this routine
		// will not care, so if you care, you need to check something
		GPIO_SetValue(0,0x8000);
		if (SendadcTimerMsg(ptr) == errQUEUE_FULL) {
			// Here is where you would do something if you wanted to handle the queue being full
			VT_HANDLE_FATAL_ERROR(0);
		}
		GPIO_ClearValue(0,0x8000);
	}
}
Exemplo n.º 13
0
// Public API
void vStartConductorTask(vtConductorStruct *params,unsigned portBASE_TYPE uxPriority, vtI2CStruct *i2c,vtTempStruct *temperature, myNavStruct *navs, myMapStruct *maps)
{
	/* Start the task */
	portBASE_TYPE retval;
	params->dev = i2c;
	params->tempData = temperature;
	params->navData = navs;
	params->mapData = maps;
	if ((retval = xTaskCreate( vConductorUpdateTask, ( signed char * ) "Conductor", conSTACK_SIZE, (void *) params, uxPriority, ( xTaskHandle * ) NULL )) != pdPASS) {
		VT_HANDLE_FATAL_ERROR(retval);
	}
}
Exemplo n.º 14
0
portBASE_TYPE SendmotorMoveMsg(motorStruct *motorData, uint8_t moveType, uint8_t distance, portTickType ticksToBlock)
{
	motorMsg moveMsg;

	if (motorData == NULL) {
		VT_HANDLE_FATAL_ERROR(0);
	}
	moveMsg.msgType = SENSORTASK_MSG;
	moveMsg.moveType = moveType;
	moveMsg.distance = distance;
	return(xQueueSend(motorData->inQ,(void *) (&moveMsg),ticksToBlock));
}
Exemplo n.º 15
0
portBASE_TYPE sendi2cMotorMsg(myI2CStruct *i2cData, uint8_t leftValue, uint8_t rightValue, portTickType ticksToBlock)
{
    if (i2cData == NULL) {
        VT_HANDLE_FATAL_ERROR(0);
    }
    myi2cMsg buffer;
    buffer.length = 8;
    if (buffer.length > vti2cMaxLen) {
        // no room for this message
        VT_HANDLE_FATAL_ERROR(INCORRECT_I2C_MSG_FORMAT);
    }
    buffer.buf[0] = 0xBB;       //i2c id
    buffer.buf[1] = 0x00;       //class id
    buffer.buf[2] = 0x00;       //parity
    buffer.buf[3] = 0x00;       //count
    buffer.buf[4] = leftValue;  //data[0]
    buffer.buf[5] = rightValue; //data[1]
    buffer.buf[6] = 0x00;       //data[2]
    buffer.buf[7] = 0x00;       //data[3]
    buffer.msgType = vtI2CMotorMsgType;
    return(xQueueSend(i2cData->inQ,(void *) (&buffer),ticksToBlock));
}
Exemplo n.º 16
0
Arquivo: LCDtask.c Projeto: benwr/blab
void StartLCDTask(vtLCDStruct *ptr, unsigned portBASE_TYPE uxPriority)
{
  if (ptr == NULL) {
    VT_HANDLE_FATAL_ERROR(0);
  }

  // Create the queue that will be used to talk to this task
  if ((ptr->inQ = xQueueCreate(vtLCDQLen, sizeof(vtLCDMsg))) == NULL) {
    VT_HANDLE_FATAL_ERROR(0);
  }
  /* Start the task */
  portBASE_TYPE retval = xTaskCreate(vLCDUpdateTask,
                                     (signed char *)"LCD",
                                     lcdSTACK_SIZE,
                                     (void *)ptr,
                                     uxPriority,
                                     (xTaskHandle *)NULL);

  if (retval != pdPASS) {
    VT_HANDLE_FATAL_ERROR(retval);
  }
}
Exemplo n.º 17
0
void vStartNavigationTask(navStruct* navData,unsigned portBASE_TYPE uxPriority, g9ZigBeeStruct* zigBeePtr){
	// Create the queue that will be used to talk to this task
	if ((navData->inQ = xQueueCreate(NavQLen,sizeof(g9Msg))) == NULL) {
		VT_HANDLE_FATAL_ERROR(0);
	}
	// malloc the mems
	int i;
	for(i = 0;i<TRACK_MEM_SIZE;i++){
		trackMem[i] = malloc(sizeof(trackMem_t));
		curMemLoc = 0;
		if(trackMem[i]==0)
			VT_HANDLE_FATAL_ERROR(0);
	}


	/* Start the task */
	portBASE_TYPE retval;
	navData->zigBeePtr = zigBeePtr;
	if ((retval = xTaskCreate( navigationUpdateTask, ( signed char * ) "Navi", navSTACK_SIZE, (void *) navData, uxPriority, ( xTaskHandle * ) NULL )) != pdPASS) {
		VT_HANDLE_FATAL_ERROR(retval);
	}
}
Exemplo n.º 18
0
portTASK_FUNCTION(sensorimotorTask, pvParameters)
{
    uint8_t rxLen, status;
    uint8_t buffer[I2C_MAX_LEN];

    struct SensorimotorParams * params = (struct SensorimotorParams *) pvParameters;
    vtI2CStruct * i2cDev = params->i2cDev;

    // LCD Information Pointer
    struct localizationParams * localization = params->localization;
    enum sensorType receivedMessageType;

    for (;;) {
        if (vtI2CDeQ(i2cDev, 
                     I2C_MAX_LEN, 
                     buffer, 
                     &rxLen, 
                     &receivedMessageType, 
                     &status) 
            != pdTRUE) {
            VT_HANDLE_FATAL_ERROR(0);
        }
            
        struct sensorMessage message;
        message.messageID = buffer[0];
        message.type = receivedMessageType;
        memcpy(message.values, &buffer[3], SENSOR_MESSAGE_LEN);
        message.updateMask = (buffer[1] << 8) | (buffer[2] << 8);

        switch(receivedMessageType) {
        case STATUS:
            sendLocalizationSensorMessage(localization, &message, portMAX_DELAY);
            break;
        default:
            VT_HANDLE_FATAL_ERROR(receivedMessageType);
            break;
        }
    }
}
Exemplo n.º 19
0
// Added by Matthew Ibarra 2/4/2013
portBASE_TYPE SendLCDADCMsg(vtLCDStruct *lcdData, int data, portTickType ticksToBlock)
{
	if(lcdData == NULL) {
		VT_HANDLE_FATAL_ERROR(0);
	}

	vtLCDMsg lcdBuffer;

	lcdBuffer.length = sizeof(data);
	lcdBuffer.msgType = LCDMsgTypeADC;
	lcdBuffer.buf[0] = (uint8_t) data;
	lcdBuffer.buf[1] = (uint8_t)(data>>8);
	return(xQueueSend(lcdData->inQ, (void *) (&lcdBuffer), ticksToBlock));
}
Exemplo n.º 20
0
// Convert from HSL colormap to RGB values in this weird colormap
// H: 0 to 360
// S: 0 to 1
// L: 0 to 1
// The LCD has a funky bitmap.  Each pixel is 16 bits (a "short unsigned int")
//   Red is the most significant 5 bits
//   Blue is the least significant 5 bits
//   Green is the middle 6 bits
static unsigned short hsl2rgb(float H,float S,float L)
{
	float C = (1.0 - fabs(2.0*L-1.0))*S;
	float Hprime = H / 60;
	unsigned short t = Hprime / 2.0;
	t *= 2;
	float X = C * (1-abs((Hprime - t) - 1));
	unsigned short truncHprime = Hprime;
	float R1, G1, B1;

	switch(truncHprime) {
		case 0: {
			R1 = C; G1 = X; B1 = 0;
			break;
		}
		case 1: {
			R1 = X; G1 = C; B1 = 0;
			break;
		}
		case 2: {
			R1 = 0; G1 = C; B1 = X;
			break;
		}
		case 3: {
			R1 = 0; G1 = X; B1 = C;
			break;
		}
		case 4: {
			R1 = X; G1 = 0; B1 = C;
			break;
		}
		case 5: {
			R1 = C; G1 = 0; B1 = X;
			break;
		}
		default: {
			// make the compiler stop generating warnings
			R1 = 0; G1 = 0; B1 = 0;
			VT_HANDLE_FATAL_ERROR(Hprime);
			break;
		}
	}
	float m = L - 0.5*C;
	R1 += m; G1 += m; B1 += m;
	unsigned short red = R1*32; if (red > 31) red = 31;
	unsigned short green = G1*64; if (green > 63) green = 63;
	unsigned short blue = B1*32; if (blue > 31) blue = 31;
	unsigned short color = (red << 11) | (green << 5) | blue;
	return(color); 
}
Exemplo n.º 21
0
portBASE_TYPE SendmotorMoveMsg(motorStruct *motorData,uint8_t length,uint8_t* value,portTickType ticksToBlock)
{
	motorMsg moveMsg;

	if (motorData == NULL) {
		VT_HANDLE_FATAL_ERROR(0);
	}
	moveMsg.msgType = SENSORTASK_MSG;
	uint8_t i;
	for( i = 0; i < length; i = i+1) {
		moveMsg.data[i] = value[i];
	}
	return(xQueueSend(motorData->inQ,(void *) (&moveMsg),ticksToBlock));
}
Exemplo n.º 22
0
// Public API
void vStartConductorTask(vtConductorStruct *params, unsigned portBASE_TYPE uxPriority, vtI2CStruct *i2c, myI2CStruct * myi2c, motorControlStruct *motorControl, irControlStruct *irData, speedLimitControlStruct *speedData, powerStruct *powerData, vtLCDStruct *lcdData)
{
	/* Start the task */
	portBASE_TYPE retval;
	params->dev = i2c;
	params->i2cData = myi2c;
	params->motorControl = motorControl;
	params->irData = irData;
	params->speedData = speedData;
	params->powerData = powerData;
	params->lcdData = lcdData;
	if ((retval = xTaskCreate( vConductorUpdateTask, ( signed char * ) "Conductor", conSTACK_SIZE, (void *) params, uxPriority, ( xTaskHandle * ) NULL )) != pdPASS) {
		VT_HANDLE_FATAL_ERROR(retval);
	}
}
Exemplo n.º 23
0
void startSensorimotorTask(struct SensorimotorParams * params,
                           unsigned portBASE_TYPE uxPriority,
                           vtI2CStruct * i2c,
                           struct localizationParams * localization)
{
    portBASE_TYPE retval;
    params->i2cDev = i2c;
    params->localization = localization;
    if ((retval = xTaskCreate(sensorimotorTask,
                              (signed char *) "Sensorimotor",
                              SENSORIMOTOR_STACK_SIZE,
                              (void *) params,
                              uxPriority,
                              (xTaskHandle *) NULL)) != pdPASS) {
        VT_HANDLE_FATAL_ERROR(retval);
    }
}
Exemplo n.º 24
0
// Public API
void startSensorTask(SensorAStruct *sensorT, unsigned portBASE_TYPE uxPriority, vtI2CStruct *i2c, MoveTaskStruct *moveT, mapTStruct *mapT, unsigned int unit_distance)
{
	/* Start the task */
	portBASE_TYPE retval;
	sensorT->i2cDev = i2c;
	sensorT->moveT = moveT;
	sensorT->mapT = mapT;
	sensorT->unit_distance = unit_distance;
    
	if ((retval =
         xTaskCreate( sensorTask,
                     ( signed char * ) "Sensor Analyzer", sensorSTACK_SIZE, (void *) sensorT,
                     uxPriority, ( xTaskHandle * ) NULL )) != pdPASS) {
             VT_HANDLE_FATAL_ERROR(retval);
         }
	// initialize pin and set direction
	pinState_respRcvd = initPin();
	GPIO_SetDir(0, pinMask_respRcvd, 1);
}
Exemplo n.º 25
0
// This is the actual task that is run
static portTASK_FUNCTION( vConductorUpdateTask, pvParameters )
{
	// Get the parameters
	vtConductorStruct *param = (vtConductorStruct *) pvParameters;
	vtRoverStatusStruct *roverStatus = param->roverStatus;
	UARTmsg msgbuf;

	uint8_t msglen;
	uint8_t *message;

	for(;;) {

		// Wait for message, then deQ it
		if (uartDeQ(param->uartDev, &msgbuf) != pdTRUE) {
			VT_HANDLE_FATAL_ERROR(0);
		}

		msglen = msgbuf.rxLen;
		message = msgbuf.data;
		
		sendToRoverStatusTask(roverStatus, msglen, message);
	}
}
Exemplo n.º 26
0
// This is the actual task that is run
static portTASK_FUNCTION( vLCDUpdateTask, pvParameters )
{
	#if LCD_EXAMPLE_OP==0
	unsigned short screenColor = 0;
	unsigned short tscr;
	unsigned char curLine;
	unsigned timerCount = 0;
	int xoffset = 0, yoffset = 0;
	unsigned int xmin=0, xmax=0, ymin=0, ymax=0;
	unsigned int x, y;
	int i, j;
	float hue=0, sat=0.2, light=0.2;
	#elif LCD_EXAMPLE_OP==1
	unsigned char picIndex = 0;
	#else
	Bad definition
	#endif
	vtLCDMsg msgBuffer;
	vtLCDStruct *lcdPtr = (vtLCDStruct *) pvParameters;

	#ifdef INSPECT_STACK
	// This is meant as an example that you can re-use in your own tasks
	// Inspect to the stack remaining to see how much room is remaining
	// 1. I'll check it here before anything really gets started
	// 2. I'll check during the run to see if it drops below 10%
	// 3. You could use break points or logging to check on this, but
	//    you really don't want to print it out because printf() can
	//    result in significant stack usage.
	// 4. Note that this checking is not perfect -- in fact, it will not
	//    be able to tell how much the stack grows on a printf() call and
	//    that growth can be *large* if version 1 of printf() is used.   
	unsigned portBASE_TYPE InitialStackLeft = uxTaskGetStackHighWaterMark(NULL);
	unsigned portBASE_TYPE CurrentStackLeft;
	float remainingStack = InitialStackLeft;
	remainingStack /= lcdSTACK_SIZE;
	if (remainingStack < 0.10) {
		// If the stack is really low, stop everything because we don't want it to run out
		// The 0.10 is just leaving a cushion, in theory, you could use exactly all of it
		VT_HANDLE_FATAL_ERROR(0);
	}
	#endif

	/* Initialize the LCD and set the initial colors */
	GLCD_Init();
	tscr = Red; // may be reset in the LCDMsgTypeTimer code below
	screenColor = White; // may be reset in the LCDMsgTypeTimer code below
	GLCD_SetTextColor(tscr);
	GLCD_SetBackColor(screenColor);
	GLCD_Clear(screenColor);

	// Added by Matthew Ibarra 2/2/2013
	int xPos = 0;

	// Note that srand() & rand() require the use of malloc() and should not be used unless you are using
	//   MALLOC_VERSION==1
	#if MALLOC_VERSION==1
	srand((unsigned) 55); // initialize the random number generator to the same seed for repeatability
	#endif

	curLine = 5;
	// This task should never exit
	for(;;)
	{	
		#ifdef INSPECT_STACK   
		CurrentStackLeft = uxTaskGetStackHighWaterMark(NULL);
		float remainingStack = CurrentStackLeft;
		remainingStack /= lcdSTACK_SIZE;
		if (remainingStack < 0.10) {
			// If the stack is really low, stop everything because we don't want it to run out
			VT_HANDLE_FATAL_ERROR(0);
		}
		#endif

		#if LCD_EXAMPLE_OP==0
		// Wait for a message
		if (xQueueReceive(lcdPtr->inQ,(void *) &msgBuffer,portMAX_DELAY) != pdTRUE) {
			VT_HANDLE_FATAL_ERROR(0);
		}
		

		#if EXAMPLE_COLOR_CHANGE==1
		//Log that we are processing a message -- more explanation of logging is given later on
		vtITMu8(vtITMPortLCDMsg,getMsgType(&msgBuffer));
		vtITMu8(vtITMPortLCDMsg,getMsgLength(&msgBuffer));

		// Take a different action depending on the type of the message that we received
		switch(getMsgType(&msgBuffer)) {
		case LCDMsgTypePrint: {
			// This will result in the text printing in the last five lines of the screen
			char   lineBuffer[lcdCHAR_IN_LINE+1];
			copyMsgString(lineBuffer,&msgBuffer,lcdCHAR_IN_LINE);
			// clear the line
			GLCD_ClearLn(curLine,1);
			// show the text
			GLCD_DisplayString(curLine,0,1,(unsigned char *)lineBuffer);
			curLine++;
			if (curLine == lcdNUM_LINES) {
				curLine = 5;
			}
			break;
		}
		case LCDMsgTypeTimer: {
			// Note: if I cared how long the timer update was I would call my routine
			//    unpackTimerMsg() which would unpack the message and get that value
			// Each timer update will cause a circle to be drawn on the top half of the screen
			//   as explained below
			if (timerCount == 0) {
				/* ************************************************** */
				// Find a new color for the screen by randomly (within limits) selecting HSL values
				// This can be ignored unless you care about the color map
				#if MALLOC_VERSION==1
				hue = rand() % 360;
				sat = (rand() % 1024) / 1023.0; sat = sat * 0.5; sat += 0.5;
				light = (rand() % 1024) / 1023.0;	light = light * 0.8; light += 0.10;
				#else
				hue = (hue + 1); if (hue >= 360) hue = 0;
				sat+=0.01; if (sat > 1.0) sat = 0.20;	
				light+=0.03; if (light > 1.0) light = 0.20;
				#endif
				screenColor = hsl2rgb(hue,sat,light);
				// Now choose a complementary value for the text color
				hue += 180;
				if (hue >= 360) hue -= 360;
				tscr = hsl2rgb(hue,sat,light);
				GLCD_SetTextColor(tscr);
				GLCD_SetBackColor(screenColor);
				// End of playing around with figuring out a random color
				/* ************************************************** */

				// clear the top half of the screen
				GLCD_ClearWindow(0,0,320,120,screenColor); 

				// Now we are going to draw a circle in the upper left corner of the screen
				int	count = 200;
				float radius;
				float inc, val, offset = MAX_RADIUS;
				unsigned short circleColor;
				inc = 2*M_PI/count;
				xmax = 0;
				ymax = 0;
				xmin = 50000;
				ymin = 50000;
				val = 0.0;
				for (i=0;i<count;i++) {
					// Make the circle a little thicker
					// by actually drawing three circles w/ different radii
					float cv = cos(val), sv=sin(val);
					circleColor = (val*0xFFFF)/(2*M_PI);
					GLCD_SetTextColor(circleColor);
					for (radius=MAX_RADIUS-2.0;radius<=MAX_RADIUS;radius+=1.0) {
						x = round(cv*radius+offset);
						y = round(sv*radius+offset);
						if (x > xmax) xmax = x;
						if (y > ymax) ymax = y;
						if (x < xmin) xmin = x;
						if (y < ymin) ymin = y;
						GLCD_PutPixel(x,y);	
					}
					val += inc;
				}
				// Now we are going to read the upper left square of the LCD's
				// memory (see its data sheet for details on that) and save
				// that into a buffer for fast re-drawing later
				if (((xmax+1-xmin)*(ymax+1-ymin)) > BUF_LEN) {
					// Make sure we have room for the data
					VT_HANDLE_FATAL_ERROR(0);
				}
				unsigned short int *tbuffer = buffer;
				unsigned int width = (xmax+1-xmin);
				for (j=ymin;j<=ymax;j++) {
					GLCD_GetPixelRow (xmin,j,width,tbuffer);
					tbuffer += width;
				}
				// end of reading in the buffer
				xoffset = xmin;
				yoffset = ymin;
			} else {
				// We are going to write out the data read into the buffer
				//   back onto the screen at a new location
				// This is *very* fast

				// First, clear out where we were
				GLCD_ClearWindow(xoffset,yoffset,xmax+1-xmin,ymax+1-ymin,screenColor);
				// Pick the new location
				#if MALLOC_VERSION==1
				xoffset = rand() % (320-(xmax+1-xmin));
				yoffset = rand() % (120-(ymax+1-ymin));
				#else
				xoffset = (xoffset + 10) % (320-(xmax+1-xmin));
				yoffset = (yoffset + 10) % (120-(ymax+1-ymin));
				#endif
				// Draw the bitmap
				GLCD_Bitmap(xoffset,yoffset,xmax+1-xmin,ymax-1-ymin,(unsigned char *)buffer);
			}
			timerCount++;
			if (timerCount >= 40) {	  
				// every so often, we reset timer count and start again
				// This isn't for any important reason, it is just to for this example code to do "stuff"
				timerCount = 0;
			}
			break;
		}
		default: {
			// In this configuration, we are only expecting to receive timer messages
			VT_HANDLE_FATAL_ERROR(getMsgType(&msgBuffer));
			break;
		}
		} // end of switch()
		#endif
		#if MILESTONE_1==1
		// Added by Matthew Ibarra 2/2/2013
			if(timerCount==0) {
				GLCD_Clear(screenColor);
				
				// Draw the vertical gridlines onto the LCD
				int i;
				for(i = 320/6; i < 315; i = i + 320/6) {
					GLCD_ClearWindow(i, 0, 1, 240, Red);
				}
				
				// Draw the vertical gridlines onto the LCD
				for(i = 240/4; i < 235; i = i + 240/4) {
					GLCD_ClearWindow(0, i, 320, 1, Red);
				}
				
				//Output Scale on LCD
				GLCD_DisplayString(29, 0, 0, (unsigned char*) "V/div=2.5 s/div=3");
				timerCount++;
			}
			int adcValue;
			getMsgValue(&adcValue, &msgBuffer);
			int displayValue;
			displayValue = 120 - (adcValue * 120)/(0x100);
			GLCD_ClearWindow(xPos, displayValue, 2, 2, Black);
			xPos += 2;
			if(xPos > 320) {
				timerCount = 0;
				xPos = 0;
			}
		#endif

		// Here is a way to do debugging output via the built-in hardware -- it requires the ULINK cable and the
		//   debugger in the Keil tools to be connected.  You can view PORT0 output in the "Debug(printf) Viewer"
		//   under "View->Serial Windows".  You have to enable "Trace" and "Port0" in the Debug setup options.  This
		//   should not be used if you are using Port0 for printf()
		// There are 31 other ports and their output (and port 0's) can be seen in the "View->Trace->Records"
		//   windows.  You have to enable the prots in the Debug setup options.  Note that unlike ITM_SendChar()
		//   this "raw" port write is not blocking.  That means it can overrun the capability of the system to record
		//   the trace events if you go too quickly; that won't hurt anything or change the program execution and
		//   you can tell if it happens because the "View->Trace->Records" window will show there was an overrun.
		//vtITMu16(vtITMPortLCD,screenColor);

		#elif 	LCD_EXAMPLE_OP==1
		// In this alternate version, we just keep redrawing a series of bitmaps as
		//   we receive timer messages
		// Wait for a message
		if (xQueueReceive(lcdPtr->inQ,(void *) &msgBuffer,portMAX_DELAY) != pdTRUE) {
			VT_HANDLE_FATAL_ERROR(0);
		}
		if (getMsgType(&msgBuffer) != LCDMsgTypeTimer) {
			// In this configuration, we are only expecting to receive timer messages
			VT_HANDLE_FATAL_ERROR(getMsgType(&msgBuffer));
		}
  		/* go through a  bitmap that is really a series of bitmaps */
		picIndex = (picIndex + 1) % 9;
		GLCD_Bmp(99,99,120,45,(unsigned char *) &ARM_Ani_16bpp[picIndex*(120*45*2)]);
		#else
		Bad setting
		#endif	
	}
}
Exemplo n.º 27
0
// This is the actual task that is run
static portTASK_FUNCTION( vi2cSensorUpdateTask, pvParameters )
{
	// Get the parameters
	vtInfraredStruct *param = (vtInfraredStruct *) pvParameters;
	// Get the I2C device pointer
	vtI2CStruct *devPtr = param->dev;
	// Get the LCD information pointer
	vtLCDStruct *lcdData = param->lcdData;
	// Get the Navigation task pointer
	vtNavStruct *navData = param->navData;
	// String buffer for printing
	char lcdBuffer[vtLCDMaxLen+1];
	// Buffer for receiving messages
	vtInfraredMsg msgBuffer;

	// Assumes that the I2C device (and thread) have already been initialized

	// Like all good tasks, this should never exit
	for(;;)
	{
		// Wait for a message from either a timer or from an I2C operation
		if (xQueueReceive(param->inQ,(void *) &msgBuffer,portMAX_DELAY) != pdTRUE) {
			printf("error getting sensor msg\n");
			//VT_HANDLE_FATAL_ERROR(0);
			continue;
		}

		// Now, based on the type of the message, we decide on the action to take
		switch(getMsgType(&msgBuffer)) {
			case SensorMsgTypeTimer: {
				#if DEBUG == 1
				GPIO_SetValue(0,0x20000);
				#endif 
				// Send query to the sensors
				if (vtI2CEnQ(devPtr,vtI2CMsgTypeSensorRead,0x4F,sizeof(i2cCmdReadVals),i2cCmdReadVals,vtInfraredMaxLen) != pdTRUE) {
					// If we can't get a complete message from the rover in time, give up and try again
					printf("couldn't get sensor data back in time\n");
					break;
				}
				#if DEBUG == 1
				GPIO_ClearValue(0,0x20000);
				#endif
				break;
			}
			case vtI2CMsgTypeSensorRead: {
				// Ensure msg was intended for this task. If not, break out and wait for next sensor msg
				if (msgBuffer.buf[0] != 0xF0) {
					break;
				}
				// Check msg integrity. 
				uint8_t i;
				uint8_t badMsg = 0;
				for (i = 1; i < vtInfraredMaxLen; i++) {
					if (msgBuffer.buf[i] == 0xF0 || msgBuffer.buf[i] == 0xF1)  badMsg = 1;
				}
				// If we've gotten a bad msg, break and wait for next sensor msg
				if (badMsg) {
					break;
				}

				#if DEBUG == 1
				printf("Distance1 %d inches\n",msgBuffer.buf[1]);
				printf("Distance2 %d inches\n",msgBuffer.buf[2]);
				printf("Distance3 %d inches\n",msgBuffer.buf[3]);
				printf("Distance4 %d inches\n",msgBuffer.buf[4]);
				printf("Distance5 %d inches\n",msgBuffer.buf[5]);
				printf("Distance6 %d inches\n",msgBuffer.buf[6]);
				#endif
//				if (lcdData != NULL) {
//					for (i = 0; i < vtInfraredMaxLen; i++) {
//						sprintf(lcdBuffer,"d%d=%d inches",i,msgBuffer.buf[i]);
//						if (SendLCDPrintMsg(lcdData,strnlen(lcdBuffer,vtLCDMaxLen),lcdBuffer,portMAX_DELAY) != pdTRUE) {
//							VT_HANDLE_FATAL_ERROR(0);
//						}
//						if (distance1 != 0) {
//							if (SendLCDGraphMsg(lcdData,distance1,portMAX_DELAY) != pdTRUE) {
//								VT_HANDLE_FATAL_ERROR(0);
//							}
//						}
//					}
//				}

				// Send data to navigation task
				if (navData != NULL) {
					if (SendNavValueMsg(navData,vtNavMsgSensorData,&msgBuffer.buf[1],portMAX_DELAY) != pdTRUE) {
						printf("error sending sensor data to nav\n");
						VT_HANDLE_FATAL_ERROR(0);
					}
				}
				
				break;
			}
			default: {
				printf("bad sensor msg\n");
				VT_HANDLE_FATAL_ERROR(getMsgType(&msgBuffer));
				break;
			}
		}
	}
}
// This is the actual task that is run
static portTASK_FUNCTION( I2CTask, pvParameters )
{
	portTickType xUpdateRate, xLastUpdateTime;
	const uint8_t ReturnADCValue = 0xAA;
	
	//const uint8_t InsteonReserved;	 //reserved for Insteon if we are only requesting insteon data
	//uint8_t InsteonSendValue[12]; //reserved for Insteon send
	
	uint8_t MidiSendCount = 1;
	uint8_t MidiSendValue[9] = {0xAF, 0x80, 0x64, 0x64, 0x00, 0x00, 0x00, 0x00, 0x00};
	uint8_t InstSendValue[9] = {0x11, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
	int msgcount = 0;
	
	uint8_t temp1, rxLen, status;
	uint8_t ADCValueReceived[12];
	uint8_t MidiReceived[2];


	// Get the parameters
	i2cParamStruct *param = (i2cParamStruct *) pvParameters;
	
	// Get the I2C device pointer
	vtI2CStruct *devPtr = param->i2cDev;
	
	// Get the LCD information pointer
	vtLCDMsgQueue *lcdData = param->lcdQ;
	vtLCDMsg lcdBuffer;

	MasterMsgQueue * masterData = param->masterQ;
	MasterMsgQueueMsg masterBuffer;

	I2CMsgQueue * i2cQ = param->i2cQ;
	I2CMsgQueueMsg i2cBuffer;

	vTaskDelay(10/portTICK_RATE_MS);

	xUpdateRate = i2cREAD_RATE_BASE / portTICK_RATE_MS;

	/* We need to initialise xLastUpdateTime prior to the first call to vTaskDelayUntil(). */
	xLastUpdateTime = xTaskGetTickCount();
	for(;;)
	{
		//delay for some amount of time before looping again
		//vTaskDelayUntil( &xLastUpdateTime, xUpdateRate );
		
		//Send a request to the PIC for ADC values
		if (vtI2CEnQ(devPtr,0x01,0x4F,1,&ReturnADCValue,12) != pdTRUE) {
			VT_HANDLE_FATAL_ERROR(0);
		}

		//wait for message from I2C
		//bit0 is first portion of ADC value
		//bit1 is last portion of ADC value
		//bit2 is timer value
		//bit3 thru bit7 are garbage (all 0's)
		//bit8 is the size of the Midi buffer on the PIC
		//bit9 is the count
		//bit10 is the ADC channel number
		//bit11 is the OPCode we sent (0xAA)
		
		if (vtI2CDeQ(devPtr,12,&ADCValueReceived[0],&rxLen,&status) != pdTRUE) {
			//VT_HANDLE_FATAL_ERROR(0);
		}

		//check the message returned for errors:
		//0xAA signifying it's the correct op-code, verify the ADC is in the right order, 
		//ensure the proper range of channel numbers is used. 
	

	 	if ((ADCValueReceived[11] != 0xAA || ADCValueReceived[0] > 3 || ADCValueReceived[10] < 0 || ADCValueReceived[10] > 5) && ADCValueReceived[11] != 0xBB)
		{
			FlipBit(6);
		}
		else 
		{
		 	//check the inbound i2c message queue from messages either from the Main Thread or the LCD
			//thread. Forward them to the PIC depending on op-code
			if (ADCValueReceived[8] < 4 && uxQueueMessagesWaiting(i2cQ->inQ) > 0)//check for room on the PIC and if a message exists
			{
				if (xQueueReceive(i2cQ->inQ,(void *) &i2cBuffer,portMAX_DELAY) != pdTRUE) //receive message from message queue
				{
					VT_HANDLE_FATAL_ERROR(0);
				}
	
				/*if (i2cBuffer.buf[0] == 0x13)
				{
					InstSendValue[0] = i2cBuffer.buf[0];
					InstSendValue[1] = 0x02;
					InstSendValue[2] = 0x62;
					InstSendValue[3] = 0x12;
					InstSendValue[4] = 0x07;
					InstSendValue[5] = 0x4F;
					InstSendValue[6] = 0x00;
					InstSendValue[7] = 0x11;
					InstSendValue[8] = i2cBuffer.buf[1];

					if (i2cBuffer.buf[1] == 8)
						FlipBit(3);

					if (vtI2CEnQ(devPtr,0x00,0x4F,9,InstSendValue,0) != pdTRUE) {
						VT_HANDLE_FATAL_ERROR(0);
					}
			
					//wait for message from I2C
					if (vtI2CDeQ(devPtr,0,&MidiReceived[0],&rxLen,&status) != pdTRUE) {
						//VT_HANDLE_FATAL_ERROR(0);
					}
				} */

				else if (i2cBuffer.buf[0] == 0x4) //MIDI message to be forwarded
				{
					MidiSendValue[1] = i2cBuffer.buf[1]; //Here are the three MIDI control bytes sent
					MidiSendValue[2] = i2cBuffer.buf[2]; //They are formulated in the mainthread and sent here
					MidiSendValue[3] = i2cBuffer.buf[3];
		
					//Midi message to I2C to the PIC, there is also a count to maintain messages
					if (MidiSendCount > 100)
						MidiSendCount = 1;
					MidiSendValue[4] = MidiSendCount;
				 	if (vtI2CEnQ(devPtr,0x00,0x4F,9,MidiSendValue,0) != pdTRUE) {
						VT_HANDLE_FATAL_ERROR(0);
					}
			
					//wait for message from I2C
					if (vtI2CDeQ(devPtr,0,&MidiReceived[0],&rxLen,&status) != pdTRUE) {
						//VT_HANDLE_FATAL_ERROR(0);
					}

					//Whenever a "note on" message is being sent to the MIDI device, we want to update the LED display as well
					//this checks the command portion of the MIDI message then changes the LED accordingly. The 0x90 signifies
					//that the message came from instrument 1 since it operates on MIDI channel 0;  0x91 means Instrument 2 since
					//it operates on MIDI channel 1
					if (i2cBuffer.buf[1] == 0x90)
					{
						if (i2cBuffer.buf[2] == 60)
							InstSendValue[2] = 0x80;
						else if (i2cBuffer.buf[2] == 62)
							InstSendValue[2] = 0x40;
						else if (i2cBuffer.buf[2] == 64)
							InstSendValue[2] = 0x20;
						else if (i2cBuffer.buf[2] == 65)
							InstSendValue[2] = 0x10;
						else if (i2cBuffer.buf[2] == 67)
							InstSendValue[2] = 0x08;
						else if (i2cBuffer.buf[2] == 69)
							InstSendValue[2] = 0x04;
						else if (i2cBuffer.buf[2] == 71)
							InstSendValue[2] = 0x02;
						else if (i2cBuffer.buf[2] == 72)
							InstSendValue[2] = 0x01;
	
						InstSendValue[0] = 0x00;
						InstSendValue[1] = 0x16;
						//InstSendValue[2] = 0x80; 
						InstSendValue[3] = 0x00;
						InstSendValue[4] = 0x00;
						InstSendValue[5] = 0x00;
	
						
	
						if (vtI2CEnQ(devPtr,0x00,0x38,6,InstSendValue,0) != pdTRUE) {
							VT_HANDLE_FATAL_ERROR(0);
						}

						if (vtI2CDeQ(devPtr,0,&MidiReceived[0],&rxLen,&status) != pdTRUE) {
						//VT_HANDLE_FATAL_ERROR(0);
						}
						MidiSendCount++;
					}


					else if (i2cBuffer.buf[1] == 0x91)
					{
						if (i2cBuffer.buf[2] == 60)
							InstSendValue[2] = 0x80;
						else if (i2cBuffer.buf[2] == 62)
							InstSendValue[2] = 0x40;
						else if (i2cBuffer.buf[2] == 64)
							InstSendValue[2] = 0x20;
						else if (i2cBuffer.buf[2] == 65)
							InstSendValue[2] = 0x10;
						else if (i2cBuffer.buf[2] == 67)
							InstSendValue[2] = 0x08;
						else if (i2cBuffer.buf[2] == 69)
							InstSendValue[2] = 0x04;
						else if (i2cBuffer.buf[2] == 71)
							InstSendValue[2] = 0x02;
						else if (i2cBuffer.buf[2] == 72)
							InstSendValue[2] = 0x01;
	
						InstSendValue[0] = 0x00;
						InstSendValue[1] = 0x16;
						//InstSendValue[2] = 0x80; 
						InstSendValue[3] = 0x00;
						InstSendValue[4] = 0x00;
						InstSendValue[5] = 0x00;
	
						
	
						if (vtI2CEnQ(devPtr,0x00,0x3B,6,InstSendValue,0) != pdTRUE) {
							VT_HANDLE_FATAL_ERROR(0);
						}

						if (vtI2CDeQ(devPtr,0,&MidiReceived[0],&rxLen,&status) != pdTRUE) {
						//VT_HANDLE_FATAL_ERROR(0);
						}
						MidiSendCount++;
					}
					
					//Here are similar checks on the MIDI messages for a change in pitch.  The same checkes are made
					//for different channels
					if (i2cBuffer.buf[1] == 0xE0)
					{
						if (i2cBuffer.buf[3] == 0x00)
							InstSendValue[1] = 0x04;
						else if (i2cBuffer.buf[3] == 0x40)
							InstSendValue[1] = 0x02;
						else if (i2cBuffer.buf[3] == 0x7F)
							InstSendValue[1] = 0x01;
												
	
						InstSendValue[0] = 0x02;
						InstSendValue[3] = 0x00;
						InstSendValue[2] = 0x00; 
						
						InstSendValue[4] = 0x00;
						InstSendValue[5] = 0x00;
	
						
	
						if (vtI2CEnQ(devPtr,0x00,0x38,6,InstSendValue,0) != pdTRUE) {
							VT_HANDLE_FATAL_ERROR(0);
						}

						if (vtI2CDeQ(devPtr,0,&MidiReceived[0],&rxLen,&status) != pdTRUE) {
						//VT_HANDLE_FATAL_ERROR(0);
						}
						MidiSendCount++;
					}

					else if (i2cBuffer.buf[1] == 0xE1)
					{
						if (i2cBuffer.buf[3] == 0x00)
							InstSendValue[1] = 0x04;
						else if (i2cBuffer.buf[3] == 0x40)
							InstSendValue[1] = 0x02;
						else if (i2cBuffer.buf[3] == 0x7F)
							InstSendValue[1] = 0x01;
												
	
						InstSendValue[0] = 0x02;
						InstSendValue[3] = 0x00;
						InstSendValue[2] = 0x00; 
						
						InstSendValue[4] = 0x00;
						InstSendValue[5] = 0x00;
	
						
	
						if (vtI2CEnQ(devPtr,0x00,0x3B,6,InstSendValue,0) != pdTRUE) {
							VT_HANDLE_FATAL_ERROR(0);
						}

						if (vtI2CDeQ(devPtr,0,&MidiReceived[0],&rxLen,&status) != pdTRUE) {
						//VT_HANDLE_FATAL_ERROR(0);
						}
						MidiSendCount++;
					}
			
					//wait for message from I2C
					
				}
			}
			//If the message is not NULL then it is an ADC message, this message gets forwarded to MainThread.c where
			//the data is used for calculations.
			FlipBit(7);
			if (lcdData != NULL) 
			{
				//message sent to the master message queue
				masterBuffer.length = 13;
				masterBuffer.buf[0] = 0x08; //means the message is from I2C	- change to 0x09 for Nick's program
				masterBuffer.buf[1] = ADCValueReceived[0];
				masterBuffer.buf[2] = ADCValueReceived[1];
				masterBuffer.buf[3] = ADCValueReceived[2];
				masterBuffer.buf[4] = ADCValueReceived[3];
				masterBuffer.buf[5] = ADCValueReceived[4];
				masterBuffer.buf[6] = ADCValueReceived[5];
				masterBuffer.buf[7] = ADCValueReceived[6];
				masterBuffer.buf[8] = ADCValueReceived[7];
				masterBuffer.buf[9] = ADCValueReceived[8];
				masterBuffer.buf[10] = ADCValueReceived[9];
				masterBuffer.buf[11] = ADCValueReceived[10];
				masterBuffer.buf[12] = ADCValueReceived[11];
				
				if (xQueueSend(masterData->inQ,(void *) (&masterBuffer),portMAX_DELAY) != pdTRUE) {  
					VT_HANDLE_FATAL_ERROR(0);
				} 
			}
		}  
	}
}
Exemplo n.º 29
0
static portTASK_FUNCTION(vmotorTask, pvParameters) {
	motorStruct *param = (motorStruct *) pvParameters;
	motorMsg msg;
	
	const uint8_t *motorCommand;
	const uint8_t forward_ten[]= {0xBA, 0x9F, 0x1F, 0x64, 0x00};
	const uint8_t forward_five[]= {0xBA, 0x9F, 0x1F, 0x32, 0x00};
	const uint8_t turn_right[]= {0xBA, 0x9F, 0x62, 0x0B, 0x00};
	const uint8_t turn_left[]= {0xBA, 0xE1, 0x1F, 0x0B, 0x00};
	const uint8_t backwards_five[]= {0xBA, 0xE1, 0x62, 0x32, 0x00};
	const uint8_t stop[]= {0xBA, 0x00, 0x00, 0x00, 0x00};
	unsigned int demoInt = 0;
	
	SendLCDPrintMsg(param->lcdData,20,"motorTask Init",portMAX_DELAY);
	
	for( ;; ) {
		//wait forever or until queue has something
		if (xQueueReceive(param->inQ,(void *) &msg,portMAX_DELAY) != pdTRUE) {
			VT_HANDLE_FATAL_ERROR(0);
		}

		switch(getMsgType(&msg)) {
			//only one type of message so far
			case SENSORTASK_MSG: {


				
				//this is where the motorTask will translate from human readable movement to block of sabertooth stuff



				//current slave address is 0x4F, take note
				if(demoInt == 0)
					motorCommand = forward_ten;
				else if(demoInt == 1)
					motorCommand = turn_left;
				else if (demoInt == 2)
					motorCommand = forward_five;
				else
					motorCommand = stop;
				if (vtI2CEnQ(param->dev,vtRoverMovementCommand,0x4F, 5, motorCommand, 3) != pdTRUE) {
					VT_HANDLE_FATAL_ERROR(0);
				}
				demoInt = demoInt + 1;
				SendLCDPrintMsg(param->lcdData,20,"SND: Move Command",portMAX_DELAY);
			break;
			}
			case ROVERACK_ERROR: {
				//this is where the arm will re-request the movement ack from the rover
				if(demoInt == 0)
					motorCommand = forward_ten;
				else if(demoInt == 1)
					motorCommand = turn_left;
				else if (demoInt == 2)
					motorCommand = forward_five;
				else
					motorCommand = stop;
				if (vtI2CEnQ(param->dev,vtRoverMovementCommand,0x4F, 5, motorCommand, 3) != pdTRUE) {
					VT_HANDLE_FATAL_ERROR(0);
				}
				demoInt = demoInt + 1;
				SendLCDPrintMsg(param->lcdData,20,"RSND: Move Command",portMAX_DELAY);
			break;
			}
		}

	}
}
Exemplo n.º 30
0
Arquivo: LCDtask.c Projeto: benwr/blab
// This is the actual task that is run
static portTASK_FUNCTION(vLCDUpdateTask, pvParameters)
{
#if LCD_EXAMPLE_OP == 0
  unsigned short screenColor = 0;
  unsigned short tscr;
  unsigned char curLine;
  unsigned timerCount = 0;
  int xoffset = 0, yoffset = 0;
  unsigned int xmin = 0, xmax = 0, ymin = 0, ymax = 0;
  unsigned int x, y;
  int i;
  float hue = 0, sat = 0.2, light = 0.2;
#else  // if LCD_EXAMPLE_OP==1
  unsigned char picIndex = 0;
#endif
  vtLCDMsg msgBuffer;
  vtLCDStruct *lcdPtr = (vtLCDStruct *)pvParameters;

#ifdef INSPECT_STACK
  // This is meant as an example that you can re-use in your own tasks
  // Inspect to the stack remaining to see how much room is remaining
  // 1. I'll check it here before anything really gets started
  // 2. I'll check during the run to see if it drops below 10%
  // 3. You could use break points or logging to check on this, but
  //    you really don't want to print it out because printf() can
  //    result in significant stack usage.
  // 4. Note that this checking is not perfect -- in fact, it will not
  //    be able to tell how much the stack grows on a printf() call and
  //    that growth can be *large* if version 1 of printf() is used.
  unsigned portBASE_TYPE InitialStackLeft = uxTaskGetStackHighWaterMark(NULL);
  unsigned portBASE_TYPE CurrentStackLeft;
  float remainingStack = InitialStackLeft;
  remainingStack /= lcdSTACK_SIZE;
  if (remainingStack < 0.10) {
    // If the stack is really low, stop everything because we don't want it to
    // run out
    // The 0.10 is just leaving a cushion, in theory, you could use exactly all
    // of it
    VT_HANDLE_FATAL_ERROR(0);
  }
#endif

  // Graph initialization
  const unsigned graphXoff = 30;
  const unsigned graphYoff = 10;
  const unsigned graphWidth = 260;
  const unsigned graphHeight = 160;
  uint8_t graphVals[graphWidth];
  for (i = 0; i < graphWidth; i++) {
    graphVals[i] = 0;
  }
  uint8_t graphStart = 0;

  /* Initialize the LCD and set the initial colors */
  GLCD_Init();
  tscr = White;         // may be reset in the LCDMsgTypeTimer code below
  screenColor = Black;  // may be reset in the LCDMsgTypeTimer code below
  GLCD_SetTextColor(tscr);
  GLCD_SetBackColor(screenColor);
  GLCD_Clear(screenColor);
  GLCD_ClearWindow(graphXoff - 1, graphYoff - 1, 1, graphHeight + 1, Blue);
  GLCD_ClearWindow(
      graphXoff - 1, graphYoff + graphHeight + 1, graphWidth + 1, 1, Blue);
  GLCD_DisplayString(22, 47, 0, (unsigned char *)"0min");
  GLCD_DisplayString(22, 37, 0, (unsigned char *)"-1min");
  GLCD_DisplayString(22, 27, 0, (unsigned char *)"-2min");
  GLCD_DisplayString(22, 17, 0, (unsigned char *)"-3min");
  GLCD_DisplayString(22, 7, 0, (unsigned char *)"-4min");
  GLCD_DisplayString(22, 0, 0, (unsigned char *)"0.0m");
  GLCD_DisplayString(14, 0, 0, (unsigned char *)"0.5m");
  GLCD_DisplayString(8, 0, 0, (unsigned char *)"1.0m");
  GLCD_DisplayString(2, 0, 0, (unsigned char *)"1.5m");

// Note that srand() & rand() require the use of malloc() and should not be used
// unless you are using
//   MALLOC_VERSION==1
#if MALLOC_VERSION == 1
  srand((unsigned)55);  // initialize the random number generator to the same
                        // seed for repeatability
#endif

  curLine = lcdNUM_SMALL_LINES - 1;
  // This task should never exit
  for (;;) {
#ifdef INSPECT_STACK
    CurrentStackLeft = uxTaskGetStackHighWaterMark(NULL);
    float remainingStack = CurrentStackLeft;
    remainingStack /= lcdSTACK_SIZE;
    if (remainingStack < 0.10) {
      // If the stack is really low, stop everything because we don't want it to
      // run out
      VT_HANDLE_FATAL_ERROR(0);
    }
#endif

#if LCD_EXAMPLE_OP == 0
    // Wait for a message
    if (xQueueReceive(lcdPtr->inQ, (void *)&msgBuffer, portMAX_DELAY) !=
        pdTRUE) {
      VT_HANDLE_FATAL_ERROR(0);
    }

    // Log that we are processing a message -- more explanation of logging is
    // given later on
    vtITMu8(vtITMPortLCDMsg, getMsgType(&msgBuffer));
    vtITMu8(vtITMPortLCDMsg, getMsgLength(&msgBuffer));

    // Take a different action depending on the type of the message that we
    // received
    switch (getMsgType(&msgBuffer)) {
    case LCDMsgTypePrint: {
      // This will result in the text printing in the last five lines of the
      // screen
      char lineBuffer[lcdCHAR_IN_LINE + 1];
      copyMsgString(lineBuffer, &msgBuffer, lcdCHAR_IN_LINE);
      // clear the line
      GLCD_ClearLn(curLine, 1);
      // show the text
      GLCD_DisplayString(curLine, 0, 0, (unsigned char *)lineBuffer);
      curLine++;
      if (curLine == lcdNUM_SMALL_LINES) {
        curLine = lcdNUM_SMALL_LINES - 1;
      }
      break;
    }
    case LCDMsgTypePoint: {
      graphVals[graphStart] = getMsgPoint(&msgBuffer);
      break;
    }
    case LCDMsgTypeTimer: {
      // Note: if I cared how long the timer update was I would call my
      // routine
      //    unpackTimerMsg() which would unpack the message and get that value
      // Each timer update will cause a circle to be drawn on the top half of
      // the screen
      //   as explained below

      if (timerCount % 2 == 0) {
        /* ************************************************** */
        // Find a new color for the screen by randomly (within limits)
        // selecting
        // HSL values
        // This can be ignored unless you care about the color map
        /*
                        #if MALLOC_VERSION==1
                        hue = rand() % 360;
                        sat = (rand() % 1024) / 1023.0; sat = sat * 0.5; sat
           +=
           0.5;
                        light = (rand() % 1024) / 1023.0;	light = light *
           0.8; light += 0.10;
                        #else
                        hue = (hue + 1); if (hue >= 360) hue = 0;
                        sat+=0.01; if (sat > 1.0) sat = 0.20;
                        light+=0.03; if (light > 1.0) light = 0.20;
                        #endif
                        screenColor = hsl2rgb(hue,sat,light);

                        // Now choose a complementary value for the text color
                        hue += 180;
                        if (hue >= 360) hue -= 360;
                        tscr = hsl2rgb(hue,sat,light);
                        GLCD_SetTextColor(tscr);
                        GLCD_SetBackColor(screenColor);

                        unsigned short int *tbuffer = buffer;
                        //int i;
                        for(i = BUF_LEN; i--;) {
                                tbuffer[i] = screenColor;
                        }
        */
        // End of playing around with figuring out a random color
        /* ************************************************** */

        // clear the top half of the screen
        // GLCD_ClearWindow(0,0,320,120,screenColor);

        /*
                        // Now we are going to draw a circle in the buffer
                        // count is how many pixels are in the circle
                        int	count = 50;
                        float radius;
                        float inc, val, offset = MAX_RADIUS;
                        unsigned short circleColor;
                        inc = 2*M_PI/count;
                        xmax = 0;
                        ymax = 0;
                        xmin = 50000;
                        ymin = 50000;
                        val = 0.0;
                        for (i=0;i<count;i++) {
                                // Make the circle a little thicker
                                // by actually drawing three circles w/
           different radii
                                float cv = cos(val), sv=sin(val);
                                circleColor = (val*0xFFFF)/(2*M_PI);
                                GLCD_SetTextColor(circleColor);
                                for
           (radius=MAX_RADIUS-2.0;radius<=MAX_RADIUS;radius+=1.0) {
                                        x = round(cv*radius+offset);
                                        y = round(sv*radius+offset);
                                        if (x > xmax) xmax = x;
                                        if (y > ymax) ymax = y;
                                        if (x < xmin) xmin = x;
                                        if (y < ymin) ymin = y;
                                        //tbuffer[(y*((MAX_RADIUS*2)+1)) + x]
           =
           circleColor;
                                }
                                val += inc;
                        }
        */
        // GLCD_ClearWindow(graphXoff, graphYoff, graphWidth, graphHeight,
        // screenColor);
        graphStart = (graphStart + 1) % graphWidth;

        DEBUG_OUT(0xf);
        for (i = 0; i < graphWidth; i++) {
          unsigned index = (i + graphStart) % graphWidth;
          unsigned y = graphHeight - graphVals[index] + graphYoff;
          unsigned x = i + graphXoff;
          GLCD_ClearWindow(x, graphYoff, 1, graphHeight, screenColor);
          GLCD_PutPixel(x, y);
        }
        DEBUG_OUT(0x0);

      }  // else {

      // We are going to write out the buffer
      //   back onto the screen at a new location
      // This is *very* fast

      /*
      // First, clear out where we were
      GLCD_ClearWindow(xoffset,yoffset,xmax+1-xmin,ymax+1-ymin,screenColor);
      // Pick the new location
      #if MALLOC_VERSION==1
      xoffset = rand() % (320-(xmax+1-xmin));
      yoffset = rand() % (120-(ymax+1-ymin));
      #else
      xoffset = (xoffset + 10) % (320-(xmax+1-xmin));
      yoffset = (yoffset + 10) % (120-(ymax+1-ymin));
      #endif
      // Draw the bitmap
      GLCD_Bitmap(xoffset,yoffset,xmax+1-xmin,ymax-1-ymin,(unsigned
      char*)buffer);
      */
      //}

      timerCount++;
      if (timerCount >= 100) {
        // every so often, we reset timer count and start again
        // This isn't for any important reason, it is just to for this example
        // code to do "stuff"
        timerCount = 0;
      }
      break;
    }
    default: {
      // In this configuration, we are only expecting to receive timer
      // messages
      VT_HANDLE_FATAL_ERROR(getMsgType(&msgBuffer));
      break;
    }
    }  // end of switch()

// Here is a way to do debugging output via the built-in hardware -- it requires
// the ULINK cable and the debugger in the Keil tools to be connected.  You can
// view PORT0 output in the "Debug(printf) Viewer" under "View->Serial Windows".
// You have to enable "Trace" and "Port0" in the Debug setup options.  This
// should not be used if you are using Port0 for printf(). There are 31 other
// ports and their output (and port 0's) can be seen in the
// "View->Trace->Records" windows.  You have to enable the ports in the Debug
// setup options.  Note that unlike ITM_SendChar() this "raw" port write is not
// blocking.  That means it can overrun the capability of the system to record
// the trace events if you go too quickly; that won't hurt anything or change
// the program execution and you can tell if it happens because the
// "View->Trace->Records" window will show there was an overrun.

// vtITMu16(vtITMPortLCD,screenColor);

#elif LCD_EXAMPLE_OP == 1
    // In this alternate version, we just keep redrawing a series of bitmaps as
    // we receive timer messages

    // Wait for a message
    if (xQueueReceive(lcdPtr->inQ, (void *)&msgBuffer, portMAX_DELAY) !=
        pdTRUE) {
      VT_HANDLE_FATAL_ERROR(0);
    }
    if (getMsgType(&msgBuffer) != LCDMsgTypeTimer) {
      // In this configuration, we are only expecting to receive timer messages
      VT_HANDLE_FATAL_ERROR(getMsgType(&msgBuffer));
    }
    /* go through a  bitmap that is really a series of bitmaps */
    picIndex = (picIndex + 1) % 9;
    GLCD_Bmp(99,
             99,
             120,
             45,
             (unsigned char *)&ARM_Ani_16bpp[picIndex * (120 * 45 * 2)]);
#else
    Bad setting
#endif
  }
}