|
@@ -5,7 +5,7 @@ const uint8_t SensorCount = 8; |
|
|
uint16_t sensorValues[SensorCount]; |
|
|
uint16_t sensorValues[SensorCount]; |
|
|
|
|
|
|
|
|
float Kp = 0.07; //set up the constants value |
|
|
float Kp = 0.07; //set up the constants value |
|
|
float Ki = 0.001; |
|
|
|
|
|
|
|
|
float Ki = 0.0008; |
|
|
float Kd = 0.6; |
|
|
float Kd = 0.6; |
|
|
int P; |
|
|
int P; |
|
|
int I; |
|
|
int I; |
|
@@ -14,7 +14,7 @@ int D; |
|
|
int lastError = 0; |
|
|
int lastError = 0; |
|
|
boolean onoff = false; |
|
|
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 maxspeeda = 150; |
|
|
const uint8_t maxspeedb = 150; |
|
|
const uint8_t maxspeedb = 150; |
|
|
const uint8_t basespeeda = 100; |
|
|
const uint8_t basespeeda = 100; |
|
@@ -22,8 +22,8 @@ const uint8_t basespeedb = 100; |
|
|
|
|
|
|
|
|
//Set up the drive motor carrier pins |
|
|
//Set up the drive motor carrier pins |
|
|
int mode = 8; |
|
|
int mode = 8; |
|
|
int aphase = 6; |
|
|
|
|
|
int aenbl = 9; |
|
|
|
|
|
|
|
|
int aphase = 9; |
|
|
|
|
|
int aenbl = 6; |
|
|
int bphase = 5; |
|
|
int bphase = 5; |
|
|
int benbl = 3; |
|
|
int benbl = 3; |
|
|
|
|
|
|
|
@@ -35,9 +35,7 @@ void setup() { |
|
|
Serial.begin(9600); |
|
|
Serial.begin(9600); |
|
|
qtr.setTypeRC(); |
|
|
qtr.setTypeRC(); |
|
|
//Set up the sensor array pins |
|
|
//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 |
|
|
qtr.setEmitterPin(7);//LEDON PIN |
|
|
|
|
|
|
|
|
pinMode(mode, OUTPUT); |
|
|
pinMode(mode, OUTPUT); |
|
@@ -52,7 +50,7 @@ void setup() { |
|
|
|
|
|
|
|
|
boolean Ok = false; |
|
|
boolean Ok = false; |
|
|
while (Ok == false) { //the loop won't start until the robot is calibrated |
|
|
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 |
|
|
calibration(); //calibrate the robot for 10 seconds |
|
|
Ok = true; |
|
|
Ok = true; |
|
|
} |
|
|
} |
|
@@ -70,9 +68,9 @@ void calibration() { |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
void loop() { |
|
|
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 |
|
|
delay(1000);//a delay when the robot starts |
|
|
} |
|
|
} |
|
|
else { |
|
|
else { |
|
@@ -81,16 +79,14 @@ void loop() { |
|
|
} |
|
|
} |
|
|
if (onoff == true) { |
|
|
if (onoff == true) { |
|
|
PID_control(); |
|
|
PID_control(); |
|
|
Serial.print(">>>>>PID-control"); |
|
|
|
|
|
} |
|
|
} |
|
|
else { |
|
|
else { |
|
|
forward_brake(0, 0); |
|
|
|
|
|
Serial.print(">>>>>forward-brake"); |
|
|
|
|
|
|
|
|
forward_brake(0,0); |
|
|
} |
|
|
} |
|
|
} |
|
|
} |
|
|
void forward_brake(int posa, int posb) { |
|
|
void forward_brake(int posa, int posb) { |
|
|
//set the appropriate values for aphase and bphase so that the robot goes straight |
|
|
//set the appropriate values for aphase and bphase so that the robot goes straight |
|
|
digitalWrite(aphase, HIGH); |
|
|
|
|
|
|
|
|
digitalWrite(aphase, LOW); |
|
|
digitalWrite(bphase, HIGH); |
|
|
digitalWrite(bphase, HIGH); |
|
|
analogWrite(aenbl, posa); |
|
|
analogWrite(aenbl, posa); |
|
|
analogWrite(benbl, posb); |
|
|
analogWrite(benbl, posb); |
|
@@ -103,11 +99,11 @@ void PID_control() { |
|
|
I = I + error; |
|
|
I = I + error; |
|
|
D = error - lastError; |
|
|
D = error - lastError; |
|
|
lastError = error; |
|
|
lastError = error; |
|
|
int motorspeed = P * Kp + I * Ki + D * Kd; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
int motorspeed = P*Kp + I*Ki + D*Kd; |
|
|
|
|
|
|
|
|
int motorspeeda = basespeeda + motorspeed; |
|
|
int motorspeeda = basespeeda + motorspeed; |
|
|
int motorspeedb = basespeedb - motorspeed; |
|
|
int motorspeedb = basespeedb - motorspeed; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if (motorspeeda > maxspeeda) { |
|
|
if (motorspeeda > maxspeeda) { |
|
|
motorspeeda = maxspeeda; |
|
|
motorspeeda = maxspeeda; |
|
|
} |
|
|
} |
|
@@ -119,8 +115,7 @@ void PID_control() { |
|
|
} |
|
|
} |
|
|
if (motorspeedb < 0) { |
|
|
if (motorspeedb < 0) { |
|
|
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); |
|
|
forward_brake(motorspeeda, motorspeedb); |
|
|
} |
|
|
} |