Code: #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); } |