Пример #1
0
// Flex sensor task
void flexTask(void* p) {
	Serial.println("IN FLEX TASK");
	while (1) {
		
		if (deviceRdy) {
			int readRaw = analogRead(flexpin);
			//Serial.println(readRaw);
			int frameLen = 0;
			unsigned char outFrame[Q_SIZE];
			unsigned char outMsg[2];
			memcpy(outMsg, "4", 1);
		
			if (readRaw < 490 && handStatus == open) {
				if (counterForFlex[0] >= 5) {
					// send data to rpi to inform that hand has been closed for 2 cycles
					handStatus = close;
					memcpy(&outMsg[1], "1", 1);
					frameLen = xbee.Send(outMsg, 2, outFrame, RPI_ADDR);
					Serial1.write(outFrame, frameLen);
					Serial.println("ON");
					counterForFlex[0] = 0; //reset counter	
				}
				else {
					counterForFlex[0]++;	//increment counter
					counterForFlex[1] = 0;
				}
				
			} else if (readRaw >= 500 && handStatus == close) {
				if (counterForFlex[1] >= 5) {
					// send data to rpi to inform that hand has been opened for 2 cycles;
					handStatus = open;
					memcpy(&outMsg[1], "0", 1);
					frameLen = xbee.Send(outMsg, 2, outFrame, RPI_ADDR);
					Serial1.write(outFrame, frameLen);
					Serial.println("OFF");
					counterForFlex[1] = 0; //reset counter	
				}
				else {
					counterForFlex[1]++;	//reset counters
					counterForFlex[0] = 0;
				}
			}
			else {
				counterForFlex[0] = 0;
				counterForFlex[1] = 0;
			}
		}
		vTaskDelay(200);
	}
}
Пример #2
0
// Task to receive data from RPi
void xbeeTask(void* p) {
	Serial.println("IN XBEE TASK");
	while (1) {

		int queueLen = 0;
		int delPos = 0;
		
		while (Serial1.available() > 0) {
			unsigned char in = (unsigned char)Serial1.read();
			Serial.println(in, HEX);
			if (!RxQ.Enqueue(in)) {
				break;
			}
		}

		queueLen = RxQ.Size();
		for (int i=0;i<queueLen;i++) {
			if (RxQ.Peek(i) == 0x7E) {
				unsigned char checkBuff[Q_SIZE];
				unsigned char msgBuff[Q_SIZE];
				int checkLen = 0;
				int msgLen = 0;
				checkLen = RxQ.Copy(checkBuff, i);
				msgLen = xbee.Receive(checkBuff, checkLen, msgBuff);
			
				if (msgLen > 0) {
					unsigned char outMsg[MAX_RPI_MSG_SIZE];
					unsigned char outFrame[Q_SIZE];
					int frameLen = 0;
					int packageID = (char)msgBuff[PKG_INDEX] - '0';

					int ack_len = 0;
					
					switch(packageID) {
						case DEVICE_READY:
							memcpy(outMsg, "ACK", 3);
							deviceRdy = ack;
							ack_len = 3;
							break;
						case NAVI_READY:
							memcpy(outMsg, "ACK", 3);
							naviRdy = true;
							ack_len = 3;
							break;
						case NAVI_END:
							memcpy(outMsg, "ACK", 3);
							naviRdy = false;
							deviceRdy = not_ack;
							ack_len = 3;
							break;
						case OBSTACLE_DETECTED:
							memcpy(outMsg, "ACK", 3);
							// activate servos
							if(msgBuff[9] == 'L')
								Serial.println("left");
							else
								Serial.println("right");
							// str = msgBuff[10];
							//writeToActuator(left,right);
							ack_len = 3;
							break;
						default:
							// raise error ?
							break;
					}
					
					
					frameLen = xbee.Send(outMsg, ack_len, outFrame, RPI_ADDR);
					
					Serial1.write(outFrame, frameLen);
					i += msgLen;
					delPos = i;
				}
				
				else {
					if (i > 0) {
						delPos = i-1;
					}
				}
			}
		}

		RxQ.Clear(delPos);
		vTaskDelay(200);
	}
}