Hi everybody,
My name is léo, i’m 18 yo, i’m french and i live in Paris, so sorry if my english is not so good
I made a robot line follower, it’s my project for my graduation
So i use a Pololu Zumo kit, with a QTR-8A sensor , and an arduino Uno
here some photo :
For the code i use the PID control :
#include <Pushbutton.h>
#include <ZumoMotors.h>
#include <QTRSensors.h>
Pushbutton button(ZUMO_BUTTON);
ZumoMotors motors;
#define KP 1
#define KD 0
#define ML_DEFAULT_SPEED 350
#define MR_DEFAULT_SPEED 200
#define ML_MAX_SPEED 400
#define MR_MAX_SPEED 400
#define MIDDLE_SENSOR 3
#define NUM_SENSORS 6
#define TIMEOUT 2500
#define NO_EMITTER_PIN 2
#define DEBUG 0
#define FORWARD 400
QTRSensorsAnalog qtra((unsigned char[]){A0, A1, A2, A3, A4, A5}, 6);
unsigned int sensorValues[NUM_SENSORS];
void setup()
{
delay(1000);
manual_calibration();
set_motors(0,0);
stop();
}
int lastError = 0;
int last_proportional = 0;
int integral = 0;
void loop()
{
unsigned int sensors[6];
int position = qtra.readLine(sensors);
int error = position - 3800;
int motorSpeed = KP * error + KD * (error - lastError);
lastError = error;
int leftMotorSpeed = ML_DEFAULT_SPEED + motorSpeed;
int rightMotorSpeed = MR_DEFAULT_SPEED - motorSpeed;
motors.setSpeeds(leftMotorSpeed, rightMotorSpeed);
stop();
}
void set_motors(int motor1speed, int motor2speed)
{
if (motor1speed > ML_MAX_SPEED ) motor1speed = ML_MAX_SPEED;
if (motor2speed > MR_MAX_SPEED ) motor2speed = MR_MAX_SPEED;
if (motor1speed < 0) motor1speed = 0;
if (motor2speed < 0) motor2speed = 0;
motors.setLeftSpeed(motor1speed);
motors.setRightSpeed(motor2speed);
}
void manual_calibration() {
button.waitForButton();
delay(500);
int i;
pinMode(13, OUTPUT);
digitalWrite(13, HIGH);
for (i = 0; i < 400; i++)
{
qtra.calibrate();
}
}
void stop(){
if(button.isPressed()){
motors.setSpeeds(0,0);
}
}
Video :
here an other code without the PID control :
#include <Pushbutton.h>
#include <ZumoMotors.h>
#include <QTRSensors.h>
#define NUM_SENSORS 6
#define NUM_SAMPLES_PER_SENSOR 4
#define NO_EMITTER_PIN 2
#define QTR_A_THRESHOLD 900
#define TURN_SPEED 300
#define TURN_SPEED2 150
#define FORWARD_SPEED 400
Pushbutton button(ZUMO_BUTTON);
ZumoMotors motors;
QTRSensorsAnalog qtra((unsigned char[]){A0, A1, A2, A3, A4, A5}, 6);
unsigned int sensorValues[NUM_SENSORS];
void setup()
{
button.waitForButton();
delay(500);
int i;
pinMode(13, OUTPUT);
digitalWrite(13, HIGH);
for (i = 0; i < 400; i++)
{
qtra.calibrate();
}
digitalWrite(13, LOW);
}
void emittersOff(){
}
void loop()
{
unsigned int position = qtra.readLine(sensorValues);
unsigned char i;
if ( sensorValues[4] > QTR_A_THRESHOLD || sensorValues[1] > QTR_A_THRESHOLD){
motors.setSpeeds(FORWARD_SPEED,FORWARD_SPEED);
digitalWrite(13, LOW);
}
if ( sensorValues[3] > QTR_A_THRESHOLD && sensorValues[2] > QTR_A_THRESHOLD){
motors.setSpeeds(FORWARD_SPEED,FORWARD_SPEED);
digitalWrite(13, HIGH);
}
if ( sensorValues[5] > QTR_A_THRESHOLD ){
motors.setSpeeds(TURN_SPEED,-TURN_SPEED2);
delay(200);
digitalWrite(13, LOW);
}
if ( sensorValues[0] > QTR_A_THRESHOLD ){
motors.setSpeeds(-TURN_SPEED2,TURN_SPEED);
delay(200);
digitalWrite(13, LOW);
}
}
video :
you see that he isn’t really fast… i’m a newbie in coding.
[b]So if you have idea for code optimization or hardware optimization tell me !
thanks ! [/b]