mirror of
https://github.com/KevinMidboe/Arduino.git
synced 2025-10-29 09:30:12 +00:00
108 lines
3.1 KiB
C++
108 lines
3.1 KiB
C++
#include <PLab_ZumoMotors.h>
|
|
#include <Pushbutton.h>
|
|
#include <QTRSensors.h>
|
|
#include <ZumoBuzzer.h>
|
|
#include <ZumoMotors.h>
|
|
#include <ZumoReflectanceSensorArray.h>
|
|
|
|
|
|
/*
|
|
* Demo line-following code for the Pololu Zumo Robot
|
|
*
|
|
* This code will follow a black line on a white background, using a
|
|
* PID-based algorithm. It works decently on courses with smooth, 6"
|
|
* radius curves and has been tested with Zumos using 30:1 HP and
|
|
* 75:1 HP motors. Modifications might be required for it to work
|
|
* well on different courses or with different motors.
|
|
*
|
|
* http://www.pololu.com/catalog/product/2506
|
|
* http://www.pololu.com
|
|
* http://forum.pololu.com
|
|
*/
|
|
|
|
|
|
ZumoReflectanceSensorArray reflectanceSensors;
|
|
ZumoMotors motors;
|
|
Pushbutton button(ZUMO_BUTTON);
|
|
int lastError = 0;
|
|
|
|
// This is the maximum speed the motors will be allowed to turn.
|
|
// (400 lets the motors go at top speed; decrease to impose a speed limit)
|
|
const int MAX_SPEED = 400;
|
|
// Set constants for PID control
|
|
const float KP = 0.5; // Proportional constant
|
|
const float KD = 6; // Derivative constant
|
|
const int SV = 2500; // Set-value for position (in the middle of sensors)
|
|
|
|
|
|
void setup()
|
|
{
|
|
|
|
reflectanceSensors.init();
|
|
button.waitForButton(); // wait to start calibration
|
|
|
|
// Turn on LED to indicate we are in calibration mode
|
|
pinMode(13, OUTPUT);
|
|
digitalWrite(13, HIGH);
|
|
|
|
// Wait 1 second and then begin automatic sensor calibration
|
|
// by rotating in place to sweep the sensors over the line
|
|
delay(1000);
|
|
int i;
|
|
for(i = 0; i < 80; i++)
|
|
{
|
|
if ((i > 10 && i <= 30) || (i > 50 && i <= 70))
|
|
motors.setSpeeds(-200, 200);
|
|
else
|
|
motors.setSpeeds(200, -200);
|
|
|
|
reflectanceSensors.calibrate();
|
|
|
|
// Since our counter runs to 80, the total delay will be
|
|
// 80*20 = 1600 ms.
|
|
delay(20);
|
|
}
|
|
motors.setSpeeds(0,0);
|
|
|
|
// Turn off LED to indicate we are through with calibration
|
|
digitalWrite(13, LOW);
|
|
|
|
// Wait for the user button to be pressed and released
|
|
button.waitForButton();
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
unsigned int sensors[6];
|
|
|
|
int pv = reflectanceSensors.readLine(sensors);
|
|
// Our "error" is how far we are away from the center of the line, which
|
|
// corresponds to position 2500.
|
|
int error = pv - SV;
|
|
// do PD computation ( Integral is not used)
|
|
int speedDifference = KP*error + KD * (error - lastError);
|
|
|
|
lastError = error;
|
|
|
|
// Get individual motor speeds. The sign of speedDifference
|
|
// determines if the robot turns left or right.
|
|
int m1Speed = MAX_SPEED + speedDifference;
|
|
int m2Speed = MAX_SPEED - speedDifference;
|
|
|
|
// Here we constrain our motor speeds to be between 0 and MAX_SPEED.
|
|
// Generally speaking, one motor will always be turning at MAX_SPEED
|
|
// and the other will be at MAX_SPEED-|speedDifference| if that is positive,
|
|
// else it will be stationary. For some applications, you might want to
|
|
// allow the motor speed to go negative so that it can spin in reverse.
|
|
if (m1Speed < 0)
|
|
m1Speed = 0;
|
|
if (m2Speed < 0)
|
|
m2Speed = 0;
|
|
if (m1Speed > MAX_SPEED)
|
|
m1Speed = MAX_SPEED;
|
|
if (m2Speed > MAX_SPEED)
|
|
m2Speed = MAX_SPEED;
|
|
|
|
motors.setSpeeds(m1Speed, m2Speed);
|
|
}
|