I have the pololy qtr 8 and using a Arduino Mega.
I use 2 motors with an l298.
I have this code
#include <QTRSensors.h>
int motorRechtsSpeed = 6;
int motorlinksSpeed = 8;
int moterRechts = 7;
int moterLinks = 9;
int maxspeedrechts = 100;
int maxspeedlinks = 100;
int defaultspeedrechts = 50;
int defaultspeedlinks = 50;
// Change the values below to suit your robot's motors, weight, wheel type, etc.
#define KP .2
#define KD 5
//#define M1_DEFAULT_SPEED 50
//#define M2_DEFAULT_SPEED 50
//#define M1_MAX_SPEED 200
//#define M2_MAX_SPEED 200
#define MIDDLE_SENSOR 3
#define NUM_SENSORS 5 // number of sensors used
#define TIMEOUT 2500 // waits for 2500 us for sensor outputs to go low
#define EMITTER_PIN 2 // emitter is controlled by digital pin 2
#define DEBUG 0 // set to 1 if serial debug output needed
QTRSensorsRC qtrrc((unsigned char[]) { 21,20,19,18,17,16,15,14} ,NUM_SENSORS, TIMEOUT, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];
void setup()
{
pinMode(moterRechts, OUTPUT);
pinMode(moterLinks, OUTPUT);
pinMode(motorRechtsSpeed, OUTPUT);
pinMode(motorlinksSpeed, OUTPUT);
digitalWrite(moterRechts, HIGH);
digitalWrite(moterLinks, HIGH);
delay(1000);
manual_calibration();
set_motors(0,0);
}
int lastError = 0;
int last_proportional = 0;
int integral = 0;
int line_position=0;
void loop()
{
unsigned int sensors[5];
int position = qtrrc.readLine(sensors);
int error = position - 2000;
int motorSpeed = KP * error + KD * (error - lastError);
lastError = error;
int leftMotorSpeed = defaultspeedrechts + motorSpeed;
int rightMotorSpeed = defaultspeedlinks - motorSpeed;
// set motor speeds using the two motor speed variables above
set_motors(leftMotorSpeed, rightMotorSpeed);
}
void set_motors(int motor1speed, int motor2speed)
{
if (motor1speed > maxspeedrechts ) motor1speed = maxspeedrechts ; // limit top speed
if (motor2speed > maxspeedlinks ) motor2speed = maxspeedlinks; // limit top speed
if (motor1speed < 0){ motor1speed = 0; // keep motor above 0
if (motor2speed < 0){ motor2speed = 0; // keep motor speed above 0
analogWrite(motorRechtsSpeed, motor1speed); // set motor speed
analogWrite(motorlinksSpeed, motor2speed);
}
void manual_calibration() {
int i;
for (i = 0; i < 250; i++) // the calibration will take a few seconds
{
qtrrc.calibrate(QTR_EMITTERS_ON);
delay(20);
}
if (DEBUG) { // if true, generate sensor dats via serial output
Serial.begin(9600);
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMinimumOn[i]);
Serial.print(' ');
}
Serial.println();
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMaximumOn[i]);
Serial.print(' ');
}
Serial.println();
Serial.println();
}
}
But when my robot is make curves one wheel stops and one wheel turns so he makes the curve.
But i need that one wheel turns forward and one wheel backwards so he can maken 90 degree curves good.
Sorry for my bad english
How sharp is your 90 degree curve? If it is a right angle, you might look at the 3pi maze solving example code to see how the 3pi detects and makes 90 degree turns by driving one of the wheels backwards. You can find more information in the “Example Project #2: Maze Solving” section of the 3pi user’s guide.
Hi Jeremy
I can’t get it working this is my code i made from your site…
#include <QTRSensors.h>
char path[100] = "";
unsigned char path_length = 0;
int motorRechtsSpeed = 6;
int motorlinksSpeed = 8;
int moterRechts = 7;
int moterLinks = 9;
int i;
// Change the values below to suit your robot's motors, weight, wheel type, etc.
#define KP .2
#define KD 5
#define MIDDLE_SENSOR 3
#define NUM_SENSORS 5 // number of sensors used
#define TIMEOUT 2500 // waits for 2500 us for sensor outputs to go low
#define EMITTER_PIN 2 // emitter is controlled by digital pin 2
#define DEBUG 0 // set to 1 if serial debug output needed
QTRSensorsRC qtrrc((unsigned char[]) { 18,17,16,15,14} ,NUM_SENSORS, TIMEOUT, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];
void setup()
{
pinMode(moterRechts, OUTPUT);
pinMode(moterLinks, OUTPUT);
pinMode(motorRechtsSpeed, OUTPUT);
pinMode(motorlinksSpeed, OUTPUT);
digitalWrite(moterRechts, HIGH);
digitalWrite(moterLinks, HIGH);
delay(1000);
manual_calibration();
//set_motors(0,0);
}
int lastError = 0;
int last_proportional = 0;
int integral = 0;
int line_position=0;
void loop()
{
// FIRST MAIN LOOP BODY
follow_segment();
// Drive straight a bit. This helps us in case we entered the
// intersection at an angle.
// Note that we are slowing down - this prevents the robot
// from tipping forward too much.
analogWrite(motorRechtsSpeed, 100);
analogWrite(motorlinksSpeed, 100);
delay(50);
// These variables record whether the robot has seen a line to the
// left, straight ahead, and right, whil examining the current
// intersection.
unsigned char found_left=0;
unsigned char found_straight=0;
unsigned char found_right=0;
// Now read the sensors and check the intersection type.
unsigned int sensors[5];
qtrrc.readLine(sensors);
// Check for left and right exits.
if(sensors[0] > 100)
found_left = 1;
if(sensors[4] > 100)
found_right = 1;
// Drive straight a bit more - this is enough to line up our
// wheels with the intersection.
analogWrite(motorRechtsSpeed, 70);
analogWrite(motorlinksSpeed, 70);
delay(200);
// Check for a straight exit.
qtrrc.readLine(sensors);
if(sensors[1] > 200 || sensors[2] > 200 || sensors[3] > 200)
found_straight = 1;
// Check for the ending spot.
// If all three middle sensors are on dark black, we have
// solved the maze.
if(sensors[1] > 600 && sensors[2] > 600 && sensors[3] > 600)
{
// break;
}
// Intersection identification is complete.
// If the maze has been solved, we can follow the existing
// path. Otherwise, we need to learn the solution.
unsigned char dir = select_turn(found_left, found_straight, found_right);
// Make the turn indicated by the path.
turn(dir);
// Store the intersection in the path variable.
path[path_length] = dir;
path_length ++;
// You should check to make sure that the path_length does not
// exceed the bounds of the array. We'll ignore that in this
// example.
// Simplify the learned path.
simplify_path();
// Display the path on the LCD.
//display_path();
// SECOND MAIN LOOP BODY
follow_segment();
// Drive straight while slowing down, as before.
analogWrite(motorRechtsSpeed, 100);
analogWrite(motorlinksSpeed, 100);
delay(50);
analogWrite(motorRechtsSpeed, 70);
analogWrite(motorlinksSpeed, 70);
delay(200);
// Make a turn according to the instruction stored in
// path[i].
turn(path[i]);
}
void simplify_path()
{
// only simplify the path if the second-to-last turn was a 'B'
if(path_length < 3 || path[path_length-2] != 'B')
return;
int total_angle = 0;
for(i=1;i<=3;i++)
{
switch(path[path_length-i])
{
case 'R':
total_angle += 90;
break;
case 'L':
total_angle += 270;
break;
case 'B':
total_angle += 180;
break;
}
}
// Get the angle as a number between 0 and 360 degrees.
total_angle = total_angle % 360;
// Replace all of those turns with a single one.
switch(total_angle)
{
case 0:
path[path_length - 3] = 'S';
break;
case 90:
path[path_length - 3] = 'R';
break;
case 180:
path[path_length - 3] = 'B';
break;
case 270:
path[path_length - 3] = 'L';
break;
}
// The path is now two steps shorter.
path_length -= 2;
}
void turn(char dir)
{
switch(dir)
{
case 'L':
//Turn left
digitalWrite(moterRechts, HIGH);
digitalWrite(moterLinks, LOW);
analogWrite(motorRechtsSpeed, 100);
analogWrite(motorlinksSpeed, 50);
delay(200);
break;
case 'R':
//Turn right
digitalWrite(moterRechts, LOW);
digitalWrite(moterLinks, HIGH);
analogWrite(motorRechtsSpeed, 50);
analogWrite(motorlinksSpeed, 100);
delay(200);
break;
case 'B' :
// Turn arround
digitalWrite(moterRechts, LOW);
digitalWrite(moterLinks, HIGH);
analogWrite(motorRechtsSpeed, 50);
analogWrite(motorlinksSpeed, 100);
delay(400);
case 'S':
// Don't do anything!
break;
}
}
void maze_solve()
{
while(1)
{
// FIRST MAIN LOOP BODY
// (when we find the goal, we use break; to get out of this)
}
// Now enter an infinite loop - we can re-run the maze as many
// times as we want to.
while(1)
{
// Beep to show that we finished the maze.
// Wait for the user to press a button...
int i;
for(i=0;i<path_length;i++)
{
// SECOND MAIN LOOP BODY
}
// Follow the last segment up to the finish.
follow_segment();
// Now we should be at the finish! Restart the loop.
}
}
char select_turn(unsigned char found_left, unsigned char found_straight,
unsigned char found_right)
{
// Make a decision about how to turn. The following code
// implements a left-hand-on-the-wall strategy, where we always
// turn as far to the left as possible.
if(found_left)
return 'L';
else if(found_straight)
return 'S';
else if(found_right)
return 'R';
else
return 'B';
}
void follow_segment()
{
int last_proportional = 0;
long integral=0;
while(1)
{
// Normally, we will be following a line. The code below is
// similar to the 3pi-linefollower-pid example, but the maximum
// speed is turned down to 60 for reliability.
// Get the position of the line.
unsigned int sensors[5];
int position = qtrrc.readLine(sensors);
// The "proportional" term should be 0 when we are on the line.
int proportional = (position - 2000);
// Compute the derivative (change) and integral (sum) of the
// position.
int derivative = proportional - last_proportional;
integral += proportional;
// Remember the last position.
last_proportional = proportional;
// Compute the difference between the two motor power settings,
// m1 - m2. If this is a positive number the robot will turn
// to the left. If it is a negative number, the robot will
// turn to the right, and the magnitude of the number determines
// the sharpness of the turn.
int power_difference = proportional/20 + integral/10000 + derivative*3/2;
// Compute the actual motor settings. We never set either motor
// to a negative value.
const int max = 200; // the maximum speed
if(power_difference > max){
power_difference = max;
digitalWrite(moterRechts, HIGH);
digitalWrite(moterLinks, HIGH);
}
if(power_difference < -max){
power_difference = max;
digitalWrite(moterRechts, LOW);
digitalWrite(moterLinks, LOW);
}
if(power_difference < 0)
{
analogWrite(motorRechtsSpeed, max);
analogWrite(motorlinksSpeed, max+power_difference);
}
else
{
analogWrite(motorRechtsSpeed, max);
analogWrite(motorlinksSpeed, max-power_difference);
}
// We use the inner three sensors (1, 2, and 3) for
// determining whether there is a line straight ahead, and the
// sensors 0 and 4 for detecting lines going to the left and
// right.
if(sensors[1] < 100 && sensors[2] < 100 && sensors[3] < 100)
{
// There is no line visible ahead, and we didn't see any
// intersection. Must be a dead end.
return;
}
else if(sensors[0] > 200 || sensors[4] > 200)
{
// Found an intersection.
return;
}
}
}
void manual_calibration() {
int i;
for (i = 0; i < 250; i++) // the calibration will take a few seconds
{
qtrrc.calibrate(QTR_EMITTERS_ON);
delay(20);
}
if (DEBUG) { // if true, generate sensor dats via serial output
Serial.begin(9600);
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMinimumOn[i]);
Serial.print(' ');
}
Serial.println();
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtrrc.calibratedMaximumOn[i]);
Serial.print(' ');
}
Serial.println();
Serial.println();
}
}
