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