// 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);
}
}
// 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);
}
}