Im working on a project with a line following robot which uses the pixy2 cam. The course is mapped out with lines and has 4 intersections. The robot is using IR sensors to detect an obstacle or "passenger" oboard, it then needs to drive to a certain location on the mapped out course with that passenger. The robot needs to be capable of following a direct route on the course, or use an alternative route if there is conditions such as "road closure" or "no left turn". These conditions will be implemented using the pixyCam barcodes.
The 3 scenarios would be as follows:
Robot follows line as normal
Passenger is onboard, follow direct route to destination
Passenger is onboard, follow alternative route (if barcode is detected)
I would like some guidance on how I can write the code for this project. I am unsure if I need to use IF statements or WHILE loops for the different scenarios. The code below is based off the pixy2 library downloads for line zumo sketch. As of now, the robot is only following the line as normal.
Any guidance would really be appreciated.
#include <Pixy2.h>
#include <PIDLoop.h>
// speeds, maximum allowed is 255
#define FAST 80
#define SLOW 40
#define STOP 35
#define X_CENTER (pixy.frameWidth/2)
Pixy2 pixy;
int S3 = 5; ///<Motor3 Speed
int S4 = 6; ///<Motor4 Speed
int M3 = 8; ///<Motor3 Direction
int M4 = 7; ///<Motor4 Direction
PIDLoop headingLoop(325, 0, 25, false);
void setup() {
Serial.begin(115200);
Serial.print("Starting...\n");
M3_forward(0);
M4_forward(0);
pinMode(S3,OUTPUT);
pinMode(M3,OUTPUT);
pinMode(S4,OUTPUT);
pinMode(M4,OUTPUT);
pixy.init();
pixy.setLamp(1, 1); // Turn on both Pixy LEDS, upper and lower for maximum exposure
pixy.changeProg("line"); // change to the line_tracking program. Note, changeProg can use partial strings, so for example,
// you can change to the line_tracking program by calling changeProg("line") instead of the whole
// string changeProg("line_tracking")
// look straight and down
pixy.setServos(590, 900);
}
void loop()
{
int8_t res;
int32_t error;
int left, right, stop;
char buf[96];
// Get latest data from Pixy, including main vector, new intersections and new barcodes.
res = pixy.line.getMainFeatures();
// If error or nothing detected, stop motors
if (res <= 0)
{
M3_forward(0);
M4_forward(0);
Serial.print("stop ");
Serial.println(res);
return;
}
// We found the vector...
if (res&LINE_VECTOR)
{
// Calculate heading error with respect to m_x1, which is the far-end of the vector,
// the part of the vector we're heading toward.
error = (int32_t)pixy.line.vectors->m_x1 - (int32_t)X_CENTER;
pixy.line.vectors->print();
// Perform PID calcs on heading error.
headingLoop.update(error);
// separate heading into left and right wheel velocities.
right = -headingLoop.m_command;
left = headingLoop.m_command;
// If vector is heading away from us (arrow pointing up), things are normal.
if (pixy.line.vectors->m_y0 > pixy.line.vectors->m_y1)
{
// ... but slow down a little if intersection is present, so we don't miss it.
if (pixy.line.vectors->m_flags&LINE_FLAG_INTERSECTION_PRESENT)
{
left += STOP;
right += STOP;
}
else // otherwise, pedal to the metal!
{
left += FAST;
right += FAST;
}
}
else // If the vector is pointing down, or down-ish, we need to go backwards to follow.
{
left -= SLOW;
right -= SLOW;
}
M3_forward(left);
M4_forward(right);
if (pixy.line.vectors->m_x1 > 75)//centre line for pixy is 40
{
M3_forward(left);
M4_reverse(0);
}
if (pixy.line.vectors->m_x1 < 5)
{
M3_reverse(0);
M4_forward(right);
}
}
// If intersection, do nothing (we've already set the turn), but acknowledge with a beep.
if (res&LINE_INTERSECTION)
{
pixy.line.intersections->print();
}
// If barcode set left or right turn accordingly.
// When calling setNextTurn(), Pixy will "execute" the turn upon the next intersection,
// making the left or right branch in the intersection the new main vector, depending on
// the angle passed to setNextTurn(). The robot will then follow the branch.
// If the turn is not set, Pixy will choose the straight(est) path by default, but
// the default turn can be changed too by calling setDefaultTurn(). The default turn
// is normally 0 (straight).
if (res&LINE_BARCODE)
{
pixy.line.barcodes->print();
// code==0 is our left-turn sign
if (pixy.line.barcodes->m_code==0)
pixy.line.setNextTurn(90); // 90 degrees is a left turn
// code==5 is our right-turn sign
else if (pixy.line.barcodes->m_code==5)
pixy.line.setNextTurn(-90); // -90 is a right turn
}
}
void M3_forward(char Speed) { ///<Motor3 Forward
digitalWrite(M3,LOW);
analogWrite(S3,Speed);
}
void M4_forward(char Speed) { ///<Motor4 Forward
digitalWrite(M4,LOW);
analogWrite(S4,Speed);
}
void M3_reverse(char Speed) { ///<Motor3 Reverse
digitalWrite(M3,HIGH);
analogWrite(S3,Speed);
}
void M4_reverse(char Speed) { ///<Motor4 Reverse
digitalWrite(M4,HIGH);
analogWrite(S4,Speed);
}