daily pastebin goal
6%
SHARE
TWEET

Untitled

a guest Mar 26th, 2019 58 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /*
  2. Drive forward and turn left or right when border is detected
  3.   - Only reflectionsensor 0 and 5 are used.
  4. */
  5. #include <ZumoMotors.h>
  6. #include <Pushbutton.h>
  7. #include <QTRSensors.h>
  8. #include <ZumoReflectanceSensorArray.h>
  9. #include <NewPing.h>
  10.  
  11. #define LED 13
  12.  
  13. // this might need to be tuned for different lighting conditions, surfaces, etc.
  14. #define QTR_THRESHOLD  500//1800 //
  15.  
  16. // these might need to be tuned for different motor types
  17. #define REVERSE_SPEED     200 // 0 is stopped, 400 is full speed
  18. #define TURN_SPEED        300
  19. #define FORWARD_SPEED     390 * speedModifier
  20. #define REVERSE_DURATION  200 // ms
  21. #define TURN_DURATION     420 // ms
  22.  
  23. ZumoMotors motors;
  24. Pushbutton button(ZUMO_BUTTON); // pushbutton on pin 12
  25.  
  26. #define NUM_SENSORS 6
  27. unsigned int sensor_values[NUM_SENSORS];
  28.  
  29. ZumoReflectanceSensorArray sensors;
  30.  
  31.  
  32. //______________SONAR_____________\\
  33.  
  34. const int echoPin = A4;
  35. const int triggerPin = A4;
  36. const int maxDistance = 50;
  37. int speedModifier = 1.0;
  38.  
  39. NewPing sonar(triggerPin, echoPin, maxDistance);
  40.  
  41.  
  42. void setup()
  43. {
  44.    sensors.init();
  45.    button.waitForButton();
  46.  
  47.    Serial.begin(9600);
  48. }
  49.  
  50.  
  51. void loop()
  52. {
  53.   //tornado();
  54.   //torpedo();
  55.   attackMode();
  56.   //findCenter();
  57.   //findOpponent();
  58. //  sensors.read(sensor_values);
  59. //
  60. //  if (sensor_values[0] > QTR_THRESHOLD)
  61. //  {
  62. //    // if leftmost sensor detects line, reverse and turn to the right
  63. //    motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
  64. //    delay(REVERSE_DURATION);
  65. //    motors.setSpeeds(TURN_SPEED, -TURN_SPEED);
  66. //    delay(TURN_DURATION);
  67. //    motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED+5);
  68. //  }
  69. //  else if (sensor_values[5] > QTR_THRESHOLD)
  70. //  {
  71. //    // if rightmost sensor detects line, reverse and turn to the left
  72. //    motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
  73. //    delay(REVERSE_DURATION);
  74. //    motors.setSpeeds(-TURN_SPEED, TURN_SPEED);
  75. //    delay(TURN_DURATION);
  76. //    motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED+5);
  77. //  }
  78. //  else
  79. //  {
  80. //    // otherwise, go straight
  81. //    motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED+5);
  82. //  }
  83. }
  84.  
  85. //___________________________METHODS______________________________________\\
  86.  
  87. int findCenter() {
  88.   goToEdge();
  89.   reverse();
  90.   turn180();
  91.  
  92.   long startTimer = millis();
  93.   goToEdge();
  94.   long endTimer = millis();
  95.   int centerTime = (endTimer - startTimer)/2;
  96.   reverse();
  97.   turn180();
  98.  
  99.   //Finds center and places
  100.   goForward(centerTime);
  101.   return centerTime;
  102. }
  103.  
  104. void findOpponent() {
  105.   motors.setSpeeds(0.5*TURN_SPEED,-TURN_SPEED*0.5); //Roterer
  106.  
  107.   unsigned int time = sonar.ping();
  108.   float distance = sonar.convert_cm(time);
  109.   Serial.println(distance);
  110.  
  111.   if (distance == 0.0) { // sonar gives zero when outside range
  112.    }
  113.    else {
  114.       goToEdge();
  115.       reverse();
  116.    }
  117. }
  118.  
  119. void goToEdge(){ // Goes straight forward until it reaches the edge
  120.   sensors.read(sensor_values);
  121.   while (sensor_values[0] > QTR_THRESHOLD && sensor_values[5] > QTR_THRESHOLD) {
  122.     motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED+5);
  123.     sensors.read(sensor_values);
  124.    }
  125.   motors.setSpeeds(0,0);
  126. }
  127.  
  128. void turn180(){ // Turns around 180 degrees
  129.   motors.setSpeeds(TURN_SPEED,-TURN_SPEED);
  130.   delay(TURN_DURATION); // HVOR LENGE SKAL DEN VÆRE FOR Å SNU 180 GRADER?
  131.   motors.setSpeeds(0,0);
  132. }
  133.  
  134. void reverse(){
  135.   motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
  136.   delay(REVERSE_DURATION);
  137.   motors.setSpeeds(0,0);
  138. }
  139.  
  140. void goForward(int ms){
  141.   motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED + 5);
  142.   delay(ms);
  143.   motors.setSpeeds(0,0);
  144. }
  145.  
  146. void goBackward(int ms){
  147.   motors.setSpeeds(-FORWARD_SPEED, -FORWARD_SPEED -5 );
  148.   delay(ms);
  149.   motors.setSpeeds(0,0);
  150. }
  151.  
  152. void attackMode(){
  153.  motors.setSpeeds(0.5*TURN_SPEED,-TURN_SPEED*0.5); //Roterer
  154.  
  155.   unsigned int time = sonar.ping();
  156.   float distance = sonar.convert_cm(time);
  157.   Serial.println(distance);
  158.  
  159.   if (distance == 0.0) { // sonar gives zero when outside range
  160.    }
  161.    else {
  162.       if (sensor_values[0] > QTR_THRESHOLD && sensor_values[5] > QTR_THRESHOLD){
  163.         motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED+5);
  164.       }
  165.    }
  166. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top