PrinceNai
Joined: 31 Oct 2016
Posts: 479
Location: Montenegro
|
| Pixy2 line follower |
 Posted: Sun Oct 23, 2022 6:13 pm |
 |
|
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
|
|
|
FAQ 
Search
Register
Profile 
Log in to check your private messages 
Log in
