hei guys, i recently made a robot.(line follower) using: arduino uno, ardumoto,polu qrt8a (analog sensor), motors and batteries.
probably my error occurs in code...just one motor work, i don't know why. i tested both motors. they works, but not in this code.
here is my code..any suggestion?
#include <QTRSensors.h>
#define NUM_SENSORS 6 // number of sensors used
#define NUM_SAMPLES_PER_SENSOR 4 // average 4 analog samples per sensor reading
#define EMITTER_PIN QTR_NO_EMITTER_PIN
QTRSensorsAnalog qtra((unsigned char[]) {0, 1, 2, 3, 4, 5},
NUM_SENSORS, NUM_SAMPLES_PER_SENSOR, EMITTER_PIN);
unsigned int sensorValues[NUM_SENSORS];
int PWMA = 3;
int PWMB = 11;
int max_speed = 200;
int proportional, Setpoint, integral, derivative, last_proportional, error_value, Kp, Ki, Kd, right_speed, left_speed;
double pozitie;
int MOTOR1_PIN1 = 12;
int MOTOR1_PIN2 = 3;
int MOTOR2_PIN1 = 13;
int MOTOR2_PIN2 = 11;
// int led = 13;
void setup() {
Setpoint = 128;
/* pid tuning parameters */
Kp = 2.2; // 2.5
Ki = 0.9; // 0.9
Kd = 6; // 6
pinMode(MOTOR1_PIN1, OUTPUT);
pinMode(MOTOR1_PIN2, OUTPUT);
pinMode(MOTOR2_PIN1, OUTPUT);
pinMode(MOTOR2_PIN2, OUTPUT);
// pinMode(led, OUTPUT);
pinMode(PWMA, OUTPUT);
pinMode(PWMB, OUTPUT);
delay(500);
// digitalWrite(led, HIGH); // turn on Arduino's LED to indicate we are in calibration mode
for (int i = 0; i < 400; i++) // make the calibration take about 10 seconds
{
qtra.calibrate(); // reads all sensors 10 times at 2500 us per read (i.e. ~25 ms per call)
}
// digitalWrite(led, 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 (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtra.calibratedMinimumOn[i]);
Serial.print(' ');
}
Serial.println();
// print the calibration maximum values measured when emitters were on
for (int i = 0; i < NUM_SENSORS; i++)
{
Serial.print(qtra.calibratedMaximumOn[i]);
Serial.print(' ');
}
Serial.println();
Serial.println();
delay(1000);
}
void loop() {
/*if (irrecv.decode(&results)) {
switch (results.value)
{
case 0x506A9BC7 :
ir=0; //stop
break;
case 0xDBCB3A41 :
ir=1; //start
break;
}
irrecv.resume(); // Receive the next value
}*/
//if (ir) {
digitalWrite(MOTOR1_PIN1, HIGH);
digitalWrite(MOTOR1_PIN2, LOW);
digitalWrite(MOTOR2_PIN1, LOW);
digitalWrite(MOTOR2_PIN2, HIGH);
pid_calc();
calc_turn();
motor_drive(right_speed, left_speed);
//}
/* else {
digitalWrite(MOTOR1_PIN1, LOW);
digitalWrite(MOTOR1_PIN2, LOW);
digitalWrite(MOTOR2_PIN1, LOW);
digitalWrite(MOTOR2_PIN2, LOW);
}*/
}
void pid_calc() {
pozitie = qtra.readLine(sensorValues);
pozitie = map(pozitie,0,5000,0,255);
proportional = pozitie - Setpoint;
integral = integral + proportional;
derivative = proportional - last_proportional;
last_proportional = proportional;
error_value = int(proportional * Kp + integral * Ki + derivative * Kd);
}
void calc_turn() { //Restricting the error value between +256.
if (error_value < -max_speed) {
error_value = -max_speed;
}
if (error_value > max_speed) {
error_value = max_speed;
}
// If error_value is less than zero calculate right turn speed values
if (error_value < 0) {
right_speed = max_speed + error_value;
left_speed = max_speed;
}
// If error_value is greater than zero calculate left turn values
else {
right_speed = max_speed;
left_speed = max_speed - error_value;
}
}
void motor_drive(int right_speed, int left_speed) { // Drive motors according to the calculated values for a turn
analogWrite(PWMA, right_speed);
analogWrite(PWMB, left_speed);
}