Browse Source

Changed main.ino to test.ino, added values to the PID constants.

master
giorgos 1 year ago
parent
commit
11d448226d
4 changed files with 131 additions and 61 deletions
  1. +0
    -40
      main.ino
  2. +16
    -21
      pis-example/pis-example.ino
  3. +0
    -0
      qtr-test/qtr-test.ino
  4. +115
    -0
      test/test.ino

+ 0
- 40
main.ino View File

@@ -1,40 +0,0 @@
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);
}

pis-example.ino → pis-example/pis-example.ino View File

@@ -5,7 +5,7 @@ const uint8_t SensorCount = 8;
uint16_t sensorValues[SensorCount];

float Kp = 0.07; //set up the constants value
float Ki = 0.001;
float Ki = 0.0008;
float Kd = 0.6;
int P;
int I;
@@ -14,7 +14,7 @@ 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
//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;
@@ -22,8 +22,8 @@ const uint8_t basespeedb = 100;

//Set up the drive motor carrier pins
int mode = 8;
int aphase = 6;
int aenbl = 9;
int aphase = 9;
int aenbl = 6;
int bphase = 5;
int benbl = 3;

@@ -35,9 +35,7 @@ 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.setSensorPins((const uint8_t[]){10, 11, 12, 14, 15, 16, 18, 19}, SensorCount);
qtr.setEmitterPin(7);//LEDON PIN

pinMode(mode, OUTPUT);
@@ -52,7 +50,7 @@ void setup() {

boolean Ok = false;
while (Ok == false) { //the loop won't start until the robot is calibrated
if (digitalRead(buttoncalibrate) == HIGH) {
if(digitalRead(buttoncalibrate) == HIGH) {
calibration(); //calibrate the robot for 10 seconds
Ok = true;
}
@@ -70,9 +68,9 @@ void calibration() {
}

void loop() {
if (digitalRead(buttonstart) == HIGH) {
onoff = ! onoff;
if (onoff = true) {
if(digitalRead(buttonstart) == HIGH) {
onoff =! onoff;
if(onoff = true) {
delay(1000);//a delay when the robot starts
}
else {
@@ -81,16 +79,14 @@ void loop() {
}
if (onoff == true) {
PID_control();
Serial.print(">>>>>PID-control");
}
else {
forward_brake(0, 0);
Serial.print(">>>>>forward-brake");
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(aphase, LOW);
digitalWrite(bphase, HIGH);
analogWrite(aenbl, posa);
analogWrite(benbl, posb);
@@ -103,11 +99,11 @@ void PID_control() {
I = I + error;
D = error - lastError;
lastError = error;
int motorspeed = P * Kp + I * Ki + D * Kd;
int motorspeed = P*Kp + I*Ki + D*Kd;
int motorspeeda = basespeeda + motorspeed;
int motorspeedb = basespeedb - motorspeed;
if (motorspeeda > maxspeeda) {
motorspeeda = maxspeeda;
}
@@ -119,8 +115,7 @@ void PID_control() {
}
if (motorspeedb < 0) {
motorspeedb = 0;
}
Serial.print(motorspeeda);Serial.print(" ");Serial.println(motorspeedb);
Serial.print(position);
}
//Serial.print(motorspeeda);Serial.print(" ");Serial.println(motorspeedb);
forward_brake(motorspeeda, motorspeedb);
}

qtr-test.ino → qtr-test/qtr-test.ino View File


+ 115
- 0
test/test.ino View File

@@ -0,0 +1,115 @@

#include <QTRSensors.h>

QTRSensors qtr;

const uint8_t SensorCount = 8;
uint16_t sensorValues[SensorCount];


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();
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
Serial.println("Starting Calibration");

// 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
Serial.println("Calibration finished");

// 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() {
uint16_t position = qtr.readLineBlack(sensorValues);
for (uint8_t i = 0; i < SensorCount; i++)
{
Serial.print(sensorValues[i]);
Serial.print('\t');
}
Serial.println(position);

delay(250);

if (position >= 3000 and position <= 4300) {
left_motor_go(200);
right_motor_go(200);
Serial.print(position);Serial.println("center");
}

if (position < 3000) {
left_motor_go(200);
right_motor_go(0);
Serial.print(position);Serial.println("left");
}

if (position > 4300) {
left_motor_go(0);
right_motor_go(200);
Serial.print(position);Serial.println("right");
}

}
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);
}

Loading…
Cancel
Save