View unanswered posts | View active topics It is currently Thu Oct 30, 2014 11:15 am






Reply to topic  [ 1 post ] 
[help] line follower 
Author Message
Rookie

Joined: Tue Apr 22, 2014 1:57 pm
Posts: 1
Post [help] line follower
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?

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


Tue Apr 22, 2014 1:59 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 1 post ] 

Who is online

Users browsing this forum: No registered users and 2 guests


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.