line_robo_demo 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 <Pixy2.h>
#include <PIDLoop.h>
//Includes required to use Roboclaw library
#include <SoftwareSerial.h>
#include “RoboClaw.h”
//See limitations of Arduino SoftwareSerial
SoftwareSerial serial(10,11);
RoboClaw roboclaw(&serial,10000);
#define address 0x80
//#include <ZumoMotors.h>…previous zumo
//#include <ZumoBuzzer.h>…previous zumo
// Robo speeds, maximum allowed is 128
#define ROBO_FAST 64
#define ROBO_SLOW 24
#define X_CENTER (pixy.frameWidth/2)
Pixy2 pixy;
//RoboMotors roboclaw;
//ZumoMotors motors;…previous zumo
//ZumoBuzzer buzzer;…previous zumo
PIDLoop headingLoop(5000, 0, 0, false);
void setup()
{
//Open roboclaw serial ports
roboclaw.begin(38400);
Serial.begin(38400);
roboclaw.ForwardMixed(address,0);
roboclaw.TurnRightMixed(address,0);
Serial.print(“Starting…\n”);
//motors.setLeftSpeed(0);…previous zumo
//motors.setRightSpeed(0);…previous zumo
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”);
// look straight and down
pixy.setServos(500, 1000);
}
void loop()
{
int8_t res;
int32_t error;
int left, right;
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)
{
roboclaw.ForwardMixed(address,0);
roboclaw.TurnRightMixed(address,0);
//motors.setLeftSpeed(0);
//motors.setRightSpeed(0);
//buzzer.playFrequency(500, 50, 15);
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.
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 += ROBO_SLOW;
right += ROBO_SLOW;
}
else // otherwise, pedal to the metal!
{
left += ROBO_FAST;
right += ROBO_FAST;
}
}
else // If the vector is pointing down, or down-ish, we need to go backwards to follow.
{
left -= ROBO_SLOW;
right -= ROBO_SLOW;
}
roboclaw.FowrwardMixed(address,left);
roboclaw.TurnRightMixed(address,right);
}
// If intersection, do nothing (we’ve already set the turn), but acknowledge with a beep.
if (res&LINE_INTERSECTION)
//{
// buzzer.playFrequency(1000, 100, 15);
// pixy.line.intersections->print();
// }
// If barcode, acknowledge with beep, and 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)
//{
// buzzer.playFrequency(2000, 100, 15);
// 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
}
}
