//------------------------------------------------------------------------------
void PingerDriver::ProcessData()
{
int i;
U32 numBytesToRead = mBuffer.GetNumBytesInBuffer(); // let's see what we 've got...
do {
if (remainingBytes==0) {
if (numBytesToRead>=3) { // nead at least 3 bytes to determine the length of the packet
// get the first 3 bytes
// reading first 3 bytes in bufhead
mBuffer.ReadBytes(bufhead, 3);
if (bufhead[0]!=(U8)'*') flushSerialBuffer();
else remainingBytes = bufhead[1] + bufhead[2] * 256 -2; // get the length of the rest of the message
}
} else if (numBytesToRead >= remainingBytes) { // it's time to get the remaining packet(s)
int totalbytes = remainingBytes + 3; // calculate total length
U8 buffer[totalbytes];
U8 rembuffer[remainingBytes];
// now reading remaining bytes as estimated using the packet header
// going altogether
mBuffer.ReadBytes(rembuffer, remainingBytes);
// tailoring bufhead and rembuffer into buffer
for (i=0; i<totalbytes; i++)
if (i<3) buffer[i] = bufhead[i];
else buffer[i] = rembuffer[i-3];
// now handling the message
// handling packet
PICPacket* pack = PICComm::convertBytes2Packet(buffer);
// clearing remaining bytes
remainingBytes = 0;
// done
PingerDriver::transitionAction(pack); // change internal state and act
// disposing the packet
PICComm::disposePacket(pack);
}
numBytesToRead = mBuffer.GetNumBytesInBuffer();
} while ((numBytesToRead >= remainingBytes)&&(remainingBytes>0));
}
int main() {
int i=0;
int rx_i=0;
device.baud(57600);
// Setup a serial interrupt function to receive data
device.attach(&Rx_interrupt, Serial::RxIrq);
// Setup a serial interrupt function to transmit data
device.attach(&Tx_interrupt, Serial::TxIrq);
device.printf("SlaveHands_FingerServoControl\n\r");
device.printf("Created:\t06/12/2015\n\r");
device.printf("Updated:\t06/12/2015\n\r");
device.printf("Version:\t0.1\n\r");
device.printf("Authors:\tIrvin Steve Cardenas (kPatch)\n\r");
device.printf("\t\t[other_authors]\n\r");
Servo5.Enable(1500,20000);
Servo6.Enable(1500,20000);
Servo7.Enable(1500,20000);
Servo8.Enable(1500,20000);
Servo9.Enable(1500,20000);
Servo10.Enable(1500,20000);
Servo21.Enable(1500,20000);
Servo22.Enable(1500,20000);
Servo23.Enable(1500,20000);
Servo24.Enable(1500,20000);
Servo25.Enable(1500,20000);
Servo26.Enable(1500,20000);
flushSerialBuffer();
while(1) {
// Read a line from the large rx buffer from rx interrupt routine
read_line();
// Read ASCII number from rx line buffer
//sscanf(rx_line,"%x",&rx_i);
// Parse Command
device.printf("%s\n\r", rx_line);
char * pch; //reference to each character in the arrray param
//int order[6]={0,0,0,0,0,0}; //Clear the array,
int order[12]={0,0,0,0,0,0,0,0,0,0,0,0}; //Clear the array,
int i = 0; //counter for looping
pch = strtok (rx_line," ");
//while ((pch != NULL) && ( i <= 6 )){
while ((pch != NULL) && ( i <= 12 )){
order[i] = atoi (pch); //refere to element in the array.(atoi - convert string to integer)
pch = strtok (NULL, " "); //strok string tokenzier.
i++; //
}
//printf ("servo5=%d servo6=%d servo7=%d servo8=%d servo9=%d servo10=%d\n\r",order[0],order[1],order[2],order[3],order[4],order[5]);
// string buf; // Have a buffer string
// stringstream ss(rx_line); // Insert the string into a stream
// vector<string> tokens; // Create vector to hold our words
// while (ss >> buf) {
// tokens.push_back(buf);
// }
Servo5.SetPosition(order[0]);
Servo6.SetPosition(order[1]);
Servo7.SetPosition(order[2]);
Servo8.SetPosition(order[3]);
Servo9.SetPosition(order[4]);
Servo10.SetPosition(order[5]);
// Servo21.SetPosition(order[0]);
// Servo22.SetPosition(order[1]);
// Servo23.SetPosition(order[2]);
// Servo24.SetPosition(order[3]);
// Servo25.SetPosition(order[4]);
// Servo26.SetPosition(order[5]);
Servo21.SetPosition(order[6]);
Servo22.SetPosition(order[7]);
Servo23.SetPosition(order[8]);
Servo24.SetPosition(order[9]);
Servo25.SetPosition(order[10]);
Servo26.SetPosition(order[11]);
// Print the pwm servo information
// printf ("servo5=%d servo6=%d servo7=%d servo8=%d servo9=%d servo10=%d\n\r",order[0],order[1],order[2],order[3],order[4],order[5]);
// for (int pos = 1000; pos < 2000; pos += 25) {
// Servo5.SetPosition(pos);
// wait_ms(20);
// Servo6.SetPosition(pos);
// wait_ms(20);
// Servo7.SetPosition(pos);
// wait_ms(20);
// Servo8.SetPosition(pos);
// wait_ms(20);
// Servo9.SetPosition(pos);
// wait_ms(20);
// Servo10.SetPosition(pos);
// wait_ms(20);
// }
//
// for (int pos = 2000; pos > 1000; pos -= 25) {
// Servo5.SetPosition(pos);
// //wait_ms(20);
// wait_ms(20);
// Servo6.SetPosition(pos);
// wait_ms(20);
// Servo7.SetPosition(pos);
// wait_ms(20);
// Servo8.SetPosition(pos);
// wait_ms(20);
// Servo9.SetPosition(pos);
// wait_ms(20);
// Servo10.SetPosition(pos);
// wait_ms(20);
// }
}
}
// processData. processData assembles the frames and calls to transitionAction.
void EZRoboNetDevice::processData() {
int i;
uint16_t numBytesToRead = CommPort->available(); // let's see what we 've got...
do {
if (remainingBytes==0) { // previous frame was fully read
if (numBytesToRead>=6) { // nead at least 6 bytes to read frame header
// get the first 6 bytes (starter(1)+sender-receiverid(2)+frame type(1)+payload length (2))
// reading first 6 bytes into FrameHead
for (i=0; i<6; i++)
FrameHead[i] = CommPort->read();
// now cheking for framestarter character mismatch
if (FrameHead[0]!=framestarter) {
flushSerialBuffer();
}
else {// reamaining bytes should be the payload length plus the terminator and two CRC bytes
remainingBytes = FrameHead[4] + FrameHead[5] * 256 +3;
}
}
} else if (numBytesToRead >= remainingBytes) { // it's time to get the remaining frame(s)
int totalBytes = remainingBytes + 6; // calculate total length
byte buffer[totalBytes];
byte remBuffer[remainingBytes];
// now reading remaining bytes as estimated using the frame header
// going altogether
for (i=0; i<remainingBytes; i++)
remBuffer[i] = CommPort->read();
// tailoring bufhead and rembuffer into buffer
for (i=0; i<totalBytes; i++)
if (i<6) buffer[i] = FrameHead[i];
else buffer[i] = remBuffer[i-6];
// now handling the message...
// checking terminator and CRC
uint16_t CRC = buffer[totalBytes-2]+256*buffer[totalBytes-1];
if ((buffer[totalBytes-3]==frameterminator)&&(checkCRC(buffer, totalBytes-2, CRC))) {
Serial.println("Frame received!");
EZFrame* frame = bytes2Frame(buffer);
// assembled the frame
if (frame->ReceiverID == NodeID) // frame was addressed to this node
transitionAction(frame); // change internal state and act
else // frame was not addressed to this node and being disposed of
disposeFrame(frame);
} else {
Serial.print("Error. Frame CRC :");
Serial.println(CRC, DEC);
Serial.print("Frame terminator :");
Serial.println(buffer[totalBytes-3], DEC);
}
// clearing remaining bytes
remainingBytes = 0;
}
numBytesToRead = CommPort->available();
} while ((numBytesToRead >= remainingBytes)&&(remainingBytes>0));
}