CCS C Software and Maintenance Offers
FAQFAQ   FAQForum Help   FAQOfficial CCS Support   SearchSearch  RegisterRegister 

ProfileProfile   Log in to check your private messagesLog in to check your private messages   Log inLog in 

CCS does not monitor this forum on a regular basis.

Please do not post bug reports on this forum. Send them to support@ccsinfo.com

Pixy2 line follower

 
Post new topic   Reply to topic    CCS Forum Index -> Code Library
View previous topic :: View next topic  
Author Message
PrinceNai



Joined: 31 Oct 2016
Posts: 452
Location: Montenegro

View user's profile Send private message

Pixy2 line follower
PostPosted: Sun Oct 23, 2022 6:13 pm     Reply with quote

Update 25.10.2022
Much improved in terms it doesn't just fold after a glitch on a comm port or if the line is lost for a short period.

Below is the code that talks with a Pixy2 camera over serial. At this point it correctly asks and gets answer for the start and end coordinates of the vector it sees inside GetMainFeatures() function. In this iteration it simply stops if it doesn't get the answer from Pixy2 (like when no vector was actually seen). It doesn't actually drive servos, it just sends out data on serial about the action it believes would need to be taken. It uses 3 vertical dividing lines over x coordinates. Left, center and right. Based on the x position of the head of the vector with respect to those lines it makes some decisions that are not tested in real life. It doesn't handle a situation if the vector changes direction.

Pixy2 is able and does talk over USB at the same time it speaks over a serial. So you can visually check if the coordinates reported are correct. Too bad it doesn't do wireless. It would be nice to compare what it sees with what the program does in action, when the car is on it's way.

On 18f46k22 115 kbauds are achievable, but only with a crystal. So lower the speed if you experience problems with communication.

Code:

#include <18F46K22.h>
#device ADC=10

#FUSES NOWDT                     // No Watch Dog Timer
#FUSES NOPUT                     // debugging setting

#device ICD=TRUE

#use delay(crystal=20000000)
#use rs232(baud=115200,parity=N,xmit=PIN_C6,rcv=PIN_C7,bits=8,stream = PIXY_COMM,errors)
#use rs232(baud=19200,parity=N,xmit=PIN_D6,rcv=PIN_D7,bits=8,stream = DEBUG,errors)
#use i2c(Master,Fast,sda=PIN_C4,scl=PIN_C3,force_hw)

// definition of commands to Pixy
#define PixySincLow 0xAE                                       // first byte of sync out
#define PixySincHigh 0xC1                                      // second byte of sync out
#define GetMainPacketType 0x30                                 // command
#define GetMainPayloadLength 0x02
#define GetMainRequestMainFeatures   0                         //
//#define GetMainRequestAllFeatures   1
#define GetMainFeaturesBitmap 0x01                             // get only vector data

// incoming data on UART1
char PixyData = 0;                                             // variable to hold data from Pixy
// buffer is not needed. It is sed for debugging purposes.
#define PIXY_BUFFER_SIZE 64                                    // Pixy data buffer
char PixyBuffer[PIXY_BUFFER_SIZE];
int8 NextBuffer = 0;
int8 VectorDataReady = 0;                                      // signal that vector data is ready
int8 Decode = 0;                                               // state machine variable
int8 ByteCounter = 0;                                          // how many bytes to skip while decoding (CRC bytes)

// outgoing data on UART1
char Tmp = 0;                                                  // used to transmit data to Pixy2

// vector related
#define CENTER_LINE 40                                         // Pixy has 79 lines on x axis, this is in the middle
#define CENTER_LINE_STIL_OK 5
#define LEFT_LINE 20                                           // Pixy has 79 lines on x axis, this is marking 1/3 of the x axis
#define RIGHT_LINE 60                                          // Pixy has 79 lines on x axis, this is marking 2/3 of the x axis
#define LINE_OFFSET 5                                          // +/- this still counts as being dead center on line

// vector variables
int8 x_tail;
int8 y_tail;
int8 x_head;                                                   // holds x position of the head of the vector
int8 y_head;

// servo related


// work cycle related
int8 Tick = 0;
int8 Next_Cycle = 0;
#define TIME_INTERVAL 4                                        // TIME_INTERVAL x 25 is one work cycle

// check for errors if no response from Pixy
int8 No_Response_Counter = 0;                                  // increment every ms while waiting for response from Pixy
int8 ErrorCount = 0;                                           // counts errors caused by vector not being seen

int8 PassCounter = 0;                                          // how many times command was repeated



// ****************************************************************************
//    FUNCTIONS
// ****************************************************************************

// ***************************************************************************

// GetMainFeatures() sends a complete command  0xAE 0xC1 0x30 0x02 0x00 0x01
// to record a vector, waits for new data to appear in the respective variable.
// Response from Pixy2 to this function is like this:

// 0xAF 0xC1 0x31 0x08 0x76 0x01 0x01 0x06 0x24 0x1C 0x2E 0x03 0xFE 0x00

// 0xAF 0xC1  generic response to all commands
// 0x31  correct response to command 0x30
// 0x08 length of data AFTER checksum 0x76 0x01
// 0x01  vector info comes after this!!!
// 0x06h length of vector info data that follows
// 0x24 x position of the tail of the vector
// 0x1C y position of the tail
// 0x2E x position of the head. THIS IS THE DATA WE NEED TO EXTRACT!!!
// 0x03 y position of the head
// 0xFE unknown
// 0x00 it seems like a standard ending
// At 115.200 baudrate the whole exchange takes less than 2ms
// now wait for the data to be ready on PIXY stream
// it is decoded inside INT_RDA, VectorDataReady flag is set and x position of the "head" of the vector is in x_head variable
// possible values are between 0 and 79

void GetMainFeatures(void){

   VectorDataReady = 0;
   ErrorCount = 0;
 
   while((ErrorCount < 5) && (!VectorDataReady)){                 // repeat command until 5 failures to get a response from Pixy2
   
      PassCounter++;
      DELAY_MS(10);
   
      Tmp = PixySincLow;
      fputc(Tmp, PIXY_COMM);                                      // send the first byte of sync
      Tmp = PixySincHigh;
      fputc(Tmp, PIXY_COMM);                                      // send the second byte of sync
      Tmp = GetMainPacketType;
      fputc(Tmp, PIXY_COMM);                                      // send the actual command
      Tmp = GetMainPayloadLength;
      fputc(Tmp, PIXY_COMM);                                      // send the length of the payload
      Tmp = GetMainRequestMainFeatures;
      fputc(Tmp, PIXY_COMM);                                      // enter which features you want. 0 = main features, 1 = all features
      Tmp = GetMainFeaturesBitmap;
      fputc(Tmp, PIXY_COMM);                                      // send the request to record vectors,intersections, barcodes   Only vectors?

     
      while(!VectorDataReady){
         delay_ms(1);
         No_Response_Counter++;
         if(No_Response_Counter >=10){
            No_Response_Counter = 0;
            ErrorCount++;
            break;
         }     
      }                       // while (!VectorDataReady)     
   }                          // while (ErrorCount...

      if(ErrorCount > 4){
         fputs("ERROR, LINE NOT RECOGNISED OR COMM FAILURE", DEBUG);
         delay_ms(5);
         while(1);            // stop on errors
      }
}                             // function end
                               
// ************************************************************
// empty so far
void PixyInit (void){
}

// ****************************************************************************
//    INTERRUPTS
// ****************************************************************************
#INT_TIMER0                                                    // 25 ms overflow at 20MHz
void  TIMER0_isr(void)
{
   Tick++;
   if(Tick >= TIME_INTERVAL){                                  // check time between two work cycles
      Tick = 0;
      Next_Cycle = 1;                                          // start new cycle after cca.TIME_INTERVAL x 25 in ms
   }
}
// ****************************************************************************
//    RECEIVE DATA FROM PIXY2
// ****************************************************************************
#INT_RDA
void  RDA_isr(void)
{
   PixyData = fgetc(PIXY_COMM);                                // get received char from PIXY_COMM and clear interrupt flag   

   PixyBuffer[NextBuffer] =  PixyData;                         // get the data in the buffer
   NextBuffer++;                                               // increment IN pointer
   if(NextBuffer == PIXY_BUFFER_SIZE) {                        // if the buffer is full, go back to 0         
      NextBuffer = 0;                           
   }
   
// decode answer from Pixy2 
// ************************************************************
// ************************************************************
      switch (Decode)
      {
// ------------ DECODE ADDRESS -----------------             
         case 0:                                           
         {
            if(PixyData == 0xAF){
               Decode = 1;
            }
            break;
         }
// ---------------------------------------         
         case 1:                                               
         {
            if(PixyData == 0xC1){
               Decode = 2;
            }
            else{
               Decode = 0;
            }
            break;           
         }
// ------- CHECK IF THE RESPONSE TYPE IS CORRECT FOR THE COMMAND ISSUED         
         case 2:                                             
         {
            if(PixyData == 0x31){                              // it is
               Decode =3;
            }
            else{
               Decode = 0;
            }
            break;
         }
         
// ----- CHECK THE LENGTH OF THE PACKET ----------         
         case 3:                                             
         {
            if(PixyData > 0x00){                               // length indicator > 0 indicates vector is present. 0x00 means it is not.
               Decode = 4;
            }
            else{
               Decode = 0;
            }
            break;           

         }                 
// ----- SKIP 2 BYTES OF CHECKSUM ----------         
         case 4:                                             
         {
            ByteCounter++;
            if(ByteCounter == 2){
               Decode = 5;
               ByteCounter = 0;
            }
            break;
         }
// ------ NEXT BYTE MUST BE 0x01, FOLLOWING BY THE LENGTH OF DATA DESCRIBING VECTOR
         case 5:                                             
         {
            if(PixyData == 0x01){
               Decode = 6;
            }
            else{
               Decode = 0;
            }
            break;
         }
// ------ IGNORE THE LENGTH OF DATA PACKAGE BYTE --------------       
         case 6:                                             
         {
            Decode = 7;
            break;
         }                   
// ------ X COORDINATE OF TAIL -----------------------
         case 7:                                             
         {
            x_tail = PixyData;
            Decode = 8;
            break;
         }         
// ------ Y COORDINATE OF TAIL -----------------------         
         case 8:                                             
         {
            y_tail = PixyData;
            Decode = 9;
            break;
         }         
// ------ X COORDINATE OF HEAD -----------------------         
         case 9:                                             
         {
            x_head = PixyData;
            Decode = 10;
            break;
         }
// ------ Y COORDINATE OF HEAD -----------------------         
         case 10:                                             
         {
            y_head = PixyData;
            Decode = 0;
            VectorDataReady = 1;                               // we got new data from Pixy, no further data is needed. Go to beginning           
            break;
         }         
// ---------------------------------------------------       
      }              // end switch
}                    // end isr

// ****************************************************************************
//    MAIN
// ****************************************************************************

void main()
{
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_2);                    //25 ms overflow

   PixyInit();                                                 // empty function so far

   enable_interrupts(INT_TIMER0);
   enable_interrupts(INT_RDA);
   enable_interrupts(GLOBAL);   

   while(TRUE)
   {
      Next_Cycle = 0;                                          // each if statement is executed until Next_Cycle is TRUE again
      GetMainFeatures();                                       // ask for vector coordinates. Relevant data is in x_head variable
      PassCounter = 0;                                         // used to check how many times command was sent to Pixy2 in one cycle
     
// sssssssssssssssteeeeeeeeeeeerrrrrrrrrriiiiiiiiiinnnnnggggggg     
// steering
// this solution might lead to a "drunken drive" effect. Not tested, since I don't have a robot


// set motors for HARD RIGHT until the next cycle
      if(x_head >= RIGHT_LINE){                                // steer hard right. Vector is way RIGHT, so you are way LEFT off the line. Steer RIGHT, hard. Counter intuitive, but correct.
         fputs("GO HARD RIGHT", DEBUG);
     
         while(!Next_Cycle){                                   // spin motors hard right until next cycle         
         }       
      }

// steer right. Vetor head is on the right side. Steer RIGHT.
      else if(x_head > CENTER_LINE + LINE_OFFSET){
         fputs("GO RIGHT", DEBUG);
           
         while(!Next_Cycle);                                   // spin motors until next cycle
      }

// set motors for HARD LEFT until the next cycle
      else if(x_head <= LEFT_LINE){                            // steer hard left
         fputs("GO HARD LEFT", DEBUG);   
     
         while(!Next_Cycle);                                   // spin motors until next cycle         
      }

// set motors for LEFT until the next cycle
      else if (x_head < CENTER_LINE - LINE_OFFSET){            // steer left
         fputs("GO LEFT", DEBUG);
           
         while(!Next_Cycle);                                   // spin motors until next cycle
      }

// vector head is within the limits. Set motors for a straight line until the next cycle
      else{                                                    // go straight
         fputs("GO STRAIGHT", DEBUG);
     
         while(!Next_Cycle);                                   // spin motors until next cycle
      }
// end steering

   }        // while(true)

}           // main
Display posts from previous:   
Post new topic   Reply to topic    CCS Forum Index -> Code Library All times are GMT - 6 Hours
Page 1 of 1

 
Jump to:  
You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot vote in polls in this forum


Powered by phpBB © 2001, 2005 phpBB Group