diff --git a/main.ino b/main.ino new file mode 100644 index 0000000..ccd8c46 --- /dev/null +++ b/main.ino @@ -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 +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); +} diff --git a/pis-examplev2.01.ino b/pis-examplev2.01.ino new file mode 100644 index 0000000..cbb3f26 --- /dev/null +++ b/pis-examplev2.01.ino @@ -0,0 +1,161 @@ + //Make sure to install the library +#include +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); +} diff --git a/qtr-test.ino b/qtr-test.ino new file mode 100644 index 0000000..6c18af7 --- /dev/null +++ b/qtr-test.ino @@ -0,0 +1,93 @@ +//Made by POLULU + +#include + +// 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); +}