Hi
We are working on the project using angle of line follower of PixyCam.
Follow the line. When 1 is transmitted to Bluetooth, it goes to 1 by using 90 degree angle. When 2 is transmitted by Bluetooth, 0 is being used to go to 2 by angle.
The attached image is similar to our track.
But at the intersection, I do not move by the angle I set. How do I modify the intersection code?
//
// begin license header
//
// This file is part of Pixy CMUcam5 or “Pixy” for short
//
// All Pixy source code is provided under the terms of the
// GNU General Public License v2 (http://www.gnu.org/licenses/gpl-2.0.html).
// Those wishing to use Pixy source code, software and/or
// technologies under different licensing terms should contact us at
// [email protected]. Such licensing terms are available for
// all portions of the Pixy codebase presented here.
//
// end license header
//
#include <SPI.h>
#include <Pixy2.h>
#include <TPixy2.h>
#include <PIDLoop.h>
#include <Pixy2Line.h>
#include <ZumoMotors.h>
#include <SoftwareSerial.h>
// Zumo speeds, maximum allowed is 400
#define ZUMO_FAST 400
#define ZUMO_SLOW 200
#define X_CENTER (pixy.frameWidth/2)
#define ledPin 5
Pixy2 pixy;
ZumoMotors motors;
SoftwareSerial mySerial(2, 3);
PIDLoop headingLoop(5000, 0, 0, false);
void setup()
{
Serial.begin(115200);
mySerial.begin(9600);
Serial.print(“Starting…\n”);
motors.setLeftSpeed(0);
motors.setRightSpeed(0);
pixy.init();
// Turn on both lamps, upper and lower for maximum exposure
pixy.setLamp(1, 1);
// 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”)
pixy.changeProg(“line_tracking”);
// look straight and down
pixy.setServos(500, 1000);
pinMode(ledPin, OUTPUT);
}
void loop()
{
int8_t res;
int32_t error;
int left, right;
char buf[96];
char code;
if(mySerial.available()){
code=(mySerial.read());
if(code == ‘1’ or ‘2’ or ‘3’ or ‘0’) {
digitalWrite(ledPin, HIGH);
delay(3000);
digitalWrite(ledPin,LOW);
}
}
// 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) // 스탑 부분
{
motors.setLeftSpeed(0);
motors.setRightSpeed(0);
Serial.print("stop ");
Serial.println(res);
return;
}
// We found the vector…
if (res&LINE_VECTOR) //
{
// Calculate heading error with respect
// 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.
left = headingLoop.m_command;
right = -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 += ZUMO_SLOW;
right += ZUMO_SLOW;
}
else // otherwise, pedal to the metal!
{
left += ZUMO_FAST;
right += ZUMO_FAST;
}
}
else // If the vector is pointing down, or down-ish, we need to go backwards to follow.
{
left -= ZUMO_SLOW;
right -= ZUMO_SLOW;
}
motors.setLeftSpeed(left);
motors.setRightSpeed(right);
}
// If intersection, do nothing (we’ve already set the turn), but acknowledge with a beep.
if (res&LINE_INTERSECTION)
{
if (code == 3) {pixy.line.setNextTurn(45); pixy.line.intersections->print();} // blue 90 degrees is a left turn
else if (code == 1) {pixy.line.setNextTurn(-45); pixy.line.intersections->print();} // red -90 is a right turn
else if (code == 2) {pixy.line.setNextTurn(0); pixy.line.intersections->print(); } // green 0 is a stat
else if (code == 0) // stop
{
motors.setLeftSpeed(0);
motors.setRightSpeed(0);
Serial.print("stop ");
Serial.println(res);
return;
}
}
pixy.line.intersections->print();
}