Advertisement
Guest User

Untitled

a guest
Feb 16th, 2015
413
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <QTRSensors.h>
  2.  
  3. #define Kp 0.2 // experiment to determine this, start by something small that just makes your bot follow the line at a slow speed
  4. #define Kd 5 // experiment to determine this, slowly increase the speeds and adjust this value. ( Note: Kp < Kd)
  5. #define rightMaxSpeed 100 // max speed of the robot
  6. #define leftMaxSpeed 100 // max speed of the robot
  7. #define rightBaseSpeed 100 // this is the speed at which the motors should spin when the robot is perfectly on the line
  8. #define leftBaseSpeed 100 // this is the speed at which the motors should spin when the robot is perfectly on the line
  9. #define NUM_SENSORS 6 // number of sensors used
  10. #define TIMEOUT 2500 // waits for 2500 us for sensor outputs to go low
  11. #define EMITTER_PIN 2 // emitter is controlled by digital pin 2
  12.  
  13. #define rightMotor1 3
  14. #define rightMotor2 4
  15. #define rightMotorPWM 5
  16. #define leftMotor1 12
  17. #define leftMotor2 13
  18. #define leftMotorPWM 11
  19. #define motorPower 8
  20.  
  21. QTRSensorsRC qtrrc((unsigned char[]) {14, 15, 16, 17, 18, 19} ,NUM_SENSORS, TIMEOUT, EMITTER_PIN); // sensor connected through analog pins A0 - A5 i.e. digital pins 14-19
  22.  
  23. unsigned int sensorValues[NUM_SENSORS];
  24.  
  25. void setup()
  26. {
  27. pinMode(rightMotor1, OUTPUT);
  28. pinMode(rightMotor2, OUTPUT);
  29. pinMode(rightMotorPWM, OUTPUT);
  30. pinMode(leftMotor1, OUTPUT);
  31. pinMode(leftMotor2, OUTPUT);
  32. pinMode(leftMotorPWM, OUTPUT);
  33. pinMode(motorPower, OUTPUT);
  34.  
  35. int i;
  36. for (int i = 0; i < 100; i++) // calibrate for sometime by sliding the sensors across the line, or you may use auto-calibration instead
  37.  
  38. /* comment this part out for automatic calibration
  39. if ( i < 25 || i >= 75 ) // turn to the left and right to expose the sensors to the brightest and darkest readings that may be encountered
  40. turn_right();
  41. else
  42. turn_left(); */
  43. qtrrc.calibrate();
  44. delay(20);
  45. wait();
  46. delay(2000); // wait for 2s to position the bot before entering the main loop
  47.  
  48. /* comment out for serial printing
  49.  
  50. Serial.begin(9600);
  51. for (int i = 0; i < NUM_SENSORS; i++)
  52. {
  53. Serial.print(qtrrc.calibratedMinimumOn[i]);
  54. Serial.print(' ');
  55. }
  56. Serial.println();
  57.  
  58. for (int i = 0; i < NUM_SENSORS; i++)
  59. {
  60. Serial.print(qtrrc.calibratedMaximumOn[i]);
  61. Serial.print(' ');
  62. }
  63. Serial.println();
  64. Serial.println();
  65. */
  66. }
  67.  
  68. int lastError = 0;
  69.  
  70. void loop()
  71. {
  72. unsigned int sensors[6];
  73. int position = qtrrc.readLine(sensors); // get calibrated readings along with the line position, refer to the QTR Sensors Arduino Library for more details on line position.
  74. int error = position - 2500;
  75.  
  76. int motorSpeed = Kp * error + Kd * (error - lastError);
  77. lastError = error;
  78.  
  79. int rightMotorSpeed = rightBaseSpeed + motorSpeed;
  80. int leftMotorSpeed = leftBaseSpeed - motorSpeed;
  81.  
  82. if (rightMotorSpeed > rightMaxSpeed ) rightMotorSpeed = rightMaxSpeed; // prevent the motor from going beyond max speed
  83. if (leftMotorSpeed > leftMaxSpeed ) leftMotorSpeed = leftMaxSpeed; // prevent the motor from going beyond max speed
  84. if (rightMotorSpeed < 0) rightMotorSpeed = 0; // keep the motor speed positive
  85. if (leftMotorSpeed < 0) leftMotorSpeed = 0; // keep the motor speed positive
  86.  
  87. {
  88. digitalWrite(motorPower, HIGH); // move forward with appropriate speeds
  89. digitalWrite(rightMotor1, HIGH);
  90. digitalWrite(rightMotor2, LOW);
  91. analogWrite(rightMotorPWM, rightMotorSpeed);
  92. digitalWrite(motorPower, HIGH);
  93. digitalWrite(leftMotor1, HIGH);
  94. digitalWrite(leftMotor2, LOW);
  95. analogWrite(leftMotorPWM, leftMotorSpeed);
  96. }
  97. }
  98.  
  99. void wait(){
  100. digitalWrite(motorPower, LOW);
  101. }
Advertisement
RAW Paste Data Copied
Advertisement