George_Andreou/Line_Following-2022
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

add arduino code

Browse source
This commit is contained in:
George_Andreou 2022-06-01 12:50:06 +03:00
parent eb673befcb
commit 7777dd3a54
4 changed files with 420 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

40
main.ino Normal file
View file

@ -0,0 +1,40 @@
int MODE = 8;
int left_motor_in = 6;
int left_motor_out = 9;
int right_motor_in = 3;
int right_motor_out = 5;
void setup() {
Serial.begin(9600);
Serial.println("Start");
init_motor();
}
void loop() {
left_motor_go(250);
right_motor_go(250);
}
void init_motor(){
pinMode(left_motor_in, OUTPUT);
pinMode(left_motor_out, OUTPUT);
pinMode(right_motor_in, OUTPUT);
pinMode(right_motor_out, OUTPUT);
pinMode(MODE, OUTPUT);
digitalWrite(MODE, HIGH);
}
//range:0<value<255
void left_motor_go(int motor_speed){
analogWrite(left_motor_in, motor_speed);
digitalWrite(left_motor_out, LOW);
}
//range:0<value<255
void right_motor_go(int motor_speed){
analogWrite(right_motor_in, motor_speed);
digitalWrite(right_motor_out, LOW);
}

126
pis-example.ino Normal file
View file

@ -0,0 +1,126 @@
//Make sure to install the library
#include <QTRSensors.h>
QTRSensors qtr;
const uint8_t SensorCount = 8;
uint16_t sensorValues[SensorCount];
float Kp = 0.07; //set up the constants value
float Ki = 0.001;
float Kd = 0.6;
int P;
int I;
int D;
int lastError = 0;
boolean onoff = false;
//Increasing the maxspeed can damage the motors - at a value of 255 the 6V motors will receive 7,4 V
const uint8_t maxspeeda = 150;
const uint8_t maxspeedb = 150;
const uint8_t basespeeda = 100;
const uint8_t basespeedb = 100;
//Set up the drive motor carrier pins
int mode = 8;
int aphase = 6;
int aenbl = 9;
int bphase = 5;
int benbl = 3;
//Set up the buttons pins
int buttoncalibrate = 17; //pin A3
int buttonstart = 2;
void setup() {
Serial.begin(9600);
qtr.setTypeRC();
//Set up the sensor array pins
qtr.setSensorPins((const uint8_t[]) {
10, 11, 12, 14, 15, 16, 18, 19
}, SensorCount);
qtr.setEmitterPin(7);//LEDON PIN
pinMode(mode, OUTPUT);
pinMode(aphase, OUTPUT);
pinMode(aenbl, OUTPUT);
pinMode(bphase, OUTPUT);
pinMode(benbl, OUTPUT);
digitalWrite(mode, HIGH);
delay(500);
pinMode(LED_BUILTIN, OUTPUT);
boolean Ok = false;
while (Ok == false) { //the loop won't start until the robot is calibrated
if (digitalRead(buttoncalibrate) == HIGH) {
calibration(); //calibrate the robot for 10 seconds
Ok = true;
}
}
forward_brake(0, 0);
}
void calibration() {
digitalWrite(LED_BUILTIN, HIGH);
for (uint16_t i = 0; i < 400; i++)
{
qtr.calibrate();
}
digitalWrite(LED_BUILTIN, LOW);
}
void loop() {
if (digitalRead(buttonstart) == HIGH) {
onoff = ! onoff;
if (onoff = true) {
delay(1000);//a delay when the robot starts
}
else {
delay(50);
}
}
if (onoff == true) {
PID_control();
Serial.print(">>>>>PID-control");
}
else {
forward_brake(0, 0);
Serial.print(">>>>>forward-brake");
}
}
void forward_brake(int posa, int posb) {
//set the appropriate values for aphase and bphase so that the robot goes straight
digitalWrite(aphase, HIGH);
digitalWrite(bphase, HIGH);
analogWrite(aenbl, posa);
analogWrite(benbl, posb);
}
void PID_control() {
uint16_t position = qtr.readLineBlack(sensorValues);
int error = 3500 - position;
P = error;
I = I + error;
D = error - lastError;
lastError = error;
int motorspeed = P * Kp + I * Ki + D * Kd;
int motorspeeda = basespeeda + motorspeed;
int motorspeedb = basespeedb - motorspeed;
if (motorspeeda > maxspeeda) {
motorspeeda = maxspeeda;
}
if (motorspeedb > maxspeedb) {
motorspeedb = maxspeedb;
}
if (motorspeeda < 0) {
motorspeeda = 0;
}
if (motorspeedb < 0) {
motorspeedb = 0;
}
Serial.print(motorspeeda);Serial.print(" ");Serial.println(motorspeedb);
Serial.print(position);
forward_brake(motorspeeda, motorspeedb);
}

161
pis-examplev2.01.ino Normal file
View file

@ -0,0 +1,161 @@
//Make sure to install the library
#include <QTRSensors.h>
QTRSensors qtr;
const uint8_t SensorCount = 8;
uint16_t sensorValues[SensorCount];
float Kp = 0.07; //set up the constants value
float Ki = 0.001;
float Kd = 0.6;
int P;
int I;
int D;
int lastError = 0;
boolean onoff = false;
//Increasing the maxspeed can damage the motors - at a value of 255 the 6V motors will receive 7,4 V
const uint8_t maxspeeda = 120;
const uint8_t maxspeedb = 120;
const uint8_t basespeeda = 100;
const uint8_t basespeedb = 100;
//Set up the drive motor carrier pins
int mode = 8;
/*int aphase = 9;
int aenbl = 6;
int bphase = 5;
int benbl = 3;*/
int aphase = 6;
int aenbl = 9;
int bphase = 3;
int benbl = 5;
//Set up the buttons pins
int buttoncalibrate = 17; //pin A3
int buttonstart = 2;
void setup() {
Serial.begin(9600);
qtr.setTypeRC();
//Set up the sensor array pins
qtr.setSensorPins((const uint8_t[]) {
10, 11, 12, 14, 15, 16, 18, 19
}, SensorCount);
qtr.setEmitterPin(7);//LEDON PIN
pinMode(mode, OUTPUT);
pinMode(aphase, OUTPUT);
pinMode(aenbl, OUTPUT);
pinMode(bphase, OUTPUT);
pinMode(benbl, OUTPUT);
digitalWrite(mode, HIGH);
delay(500);
pinMode(LED_BUILTIN, OUTPUT);
//Stephanos code - LED will blink for 3 seconds and then
/* calibration will begin running for 10 seconds;
this happens when our system boots. Giorgo i removed
the functionality to calibrate through the button keypress.*/
for (int counter=0; counter < 3;counter++){
digitalWrite(LED_BUILTIN, HIGH);
delay(500);
digitalWrite(LED_BUILTIN, LOW);
delay(500);
}
calibration();
//When calibration is compelted we understand that by blinking the LED for
//5 times in a period of 2 seconds, faster than getting ready for calibration
for (int counter=0; counter < 3;counter++){
digitalWrite(LED_BUILTIN, HIGH);
delay(200);
digitalWrite(LED_BUILTIN, LOW);
delay(200);
}
//modified by Stephanos - commented out
/*boolean Ok = false;
while (Ok == false) { //the loop won't start until the robot is calibrated
if (digitalRead(buttoncalibrate) == HIGH) {
calibration(); //calibrate the robot for 10 seconds
Ok = true;
}
}*/
forward_brake(0, 0);
}
void calibration() {
digitalWrite(LED_BUILTIN, HIGH);
for (uint16_t i = 0; i < 400; i++)
{
qtr.calibrate();
}
digitalWrite(LED_BUILTIN, LOW);
Serial.print("Calibration COMPLETED!!!");
}
void loop() {
/*if (digitalRead(buttonstart) == HIGH) {
onoff = ! onoff;
if (onoff = true) {
delay(1000);//a delay when the robot starts
}
else {
delay(50);
}
}*/
delay(3000);// a small 3 seconds delay before starting our rebot
PID_control();
/*if (onoff == true) {
PID_control();
}
else {
forward_brake(0, 0);
}*/
}
void forward_brake(int posa, int posb) {
//set the appropriate values for aphase and bphase so that the robot goes straight
digitalWrite(aphase, HIGH);
digitalWrite(bphase, HIGH);
analogWrite(aenbl, posa);
analogWrite(benbl, posb);
}
void PID_control() {
uint16_t position = qtr.readLineBlack(sensorValues);
int error = 3500 - position;
P = error;
I = I + error;
D = error - lastError;
lastError = error;
int motorspeed = P * Kp + I * Ki + D * Kd;
int motorspeeda = basespeeda + motorspeed;
int motorspeedb = basespeedb - motorspeed;
if (motorspeeda > maxspeeda) {
motorspeeda = maxspeeda;
}
if (motorspeedb > maxspeedb) {
motorspeedb = maxspeedb;
}
if (motorspeeda < 0) {
motorspeeda = 0;
}
if (motorspeedb < 0) {
motorspeedb = 0;
}
Serial.print(motorspeeda);Serial.print(" ");Serial.println(motorspeedb);
forward_brake(motorspeeda, motorspeedb);
}

93
qtr-test.ino Normal file
View file

@ -0,0 +1,93 @@
//Made by POLULU
#include <QTRSensors.h>
// This example is designed for use with eight RC QTR sensors. These
// reflectance sensors should be connected to digital pins 3 to 10. The
// sensors' emitter control pin (CTRL or LEDON) can optionally be connected to
// digital pin 2, or you can leave it disconnected and remove the call to
// setEmitterPin().
//
// The setup phase of this example calibrates the sensors for ten seconds and
// turns on the Arduino's LED (usually on pin 13) while calibration is going
// on. During this phase, you should expose each reflectance sensor to the
// lightest and darkest readings they will encounter. For example, if you are
// making a line follower, you should slide the sensors across the line during
// the calibration phase so that each sensor can get a reading of how dark the
// line is and how light the ground is. Improper calibration will result in
// poor readings.
//
// The main loop of the example reads the calibrated sensor values and uses
// them to estimate the position of a line. You can test this by taping a piece
// of 3/4" black electrical tape to a piece of white paper and sliding the
// sensor across it. It prints the sensor values to the serial monitor as
// numbers from 0 (maximum reflectance) to 1000 (minimum reflectance) followed
// by the estimated location of the line as a number from 0 to 5000. 1000 means
// the line is directly under sensor 1, 2000 means directly under sensor 2,
// etc. 0 means the line is directly under sensor 0 or was last seen by sensor
// 0 before being lost. 5000 means the line is directly under sensor 5 or was
// last seen by sensor 5 before being lost.
QTRSensors qtr;
const uint8_t SensorCount = 8;
uint16_t sensorValues[SensorCount];
void setup()
{
// configure the sensors
qtr.setTypeRC();
qtr.setSensorPins((const uint8_t[]){10, 11, 12, 14, 15, 16, 18, 19}, SensorCount);
qtr.setEmitterPin(7);
delay(500);
pinMode(LED_BUILTIN, OUTPUT);
digitalWrite(LED_BUILTIN, HIGH); // turn on Arduino's LED to indicate we are in calibration mode
// 2.5 ms RC read timeout (default) * 10 reads per calibrate() call
// = ~25 ms per calibrate() call.
// Call calibrate() 400 times to make calibration take about 10 seconds.
for (uint16_t i = 0; i < 400; i++)
{
qtr.calibrate();
}
digitalWrite(LED_BUILTIN, LOW); // turn off Arduino's LED to indicate we are through with calibration
// print the calibration minimum values measured when emitters were on
Serial.begin(9600);
for (uint8_t i = 0; i < SensorCount; i++)
{
Serial.print(qtr.calibrationOn.minimum[i]);
Serial.print(' ');
}
Serial.println();
// print the calibration maximum values measured when emitters were on
for (uint8_t i = 0; i < SensorCount; i++)
{
Serial.print(qtr.calibrationOn.maximum[i]);
Serial.print(' ');
}
Serial.println();
Serial.println();
delay(1000);
}
void loop()
{
// read calibrated sensor values and obtain a measure of the line position
// from 0 to 5000 (for a white line, use readLineWhite() instead)
uint16_t position = qtr.readLineBlack(sensorValues);
// print the sensor values as numbers from 0 to 1000, where 0 means maximum
// reflectance and 1000 means minimum reflectance, followed by the line
// position
for (uint8_t i = 0; i < SensorCount; i++)
{
Serial.print(sensorValues[i]);
Serial.print('\t');
}
Serial.println(position);
delay(250);
}