示例#1
0
//------------------------------------------------------------------------------
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));

}