Advertisement
Guest User

Untitled

a guest
Jan 20th, 2019
72
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 4.82 KB | None | 0 0
  1. //the right motor will be controlled by the motor A pins on the motor driver
  2. const int AIN1 = 13;           //control pin 1 on the motor driver for the right motor
  3. const int AIN2 = 12;            //control pin 2 on the motor driver for the right motor
  4. const int PWMA = 11;            //speed control pin on the motor driver for the right motor
  5.  
  6. //the left motor will be controlled by the motor B pins on the motor driver
  7. const int PWMB = 10;           //speed control pin on the motor driver for the left motor
  8. const int BIN2 = 9;           //control pin 2 on the motor driver for the left motor
  9. const int BIN1 = 8;           //control pin 1 on the motor driver for the left motor
  10.  
  11. int switchPin = 6;             //switch to turn the robot on and off
  12.  
  13. //robot behaviour variables
  14. int backupTime = 300;           //amount of time that the robot will back up when it senses an object
  15. int turnTime = 650;             //amount that the robot will turn once it has backed up
  16.  
  17. volatile unsigned int countL = 0;
  18. volatile unsigned int countR = 0;
  19.  
  20. /********************************************************************************/
  21. void setup()
  22. {
  23.  
  24.   pinMode(switchPin, INPUT_PULLUP);   //set this as a pullup to sense whether the switch is flipped
  25.  
  26.  
  27.   //set the motor contro pins as outputs
  28.   pinMode(AIN1, OUTPUT);
  29.   pinMode(AIN2, OUTPUT);
  30.   pinMode(PWMA, OUTPUT);
  31.  
  32.   pinMode(BIN1, OUTPUT);
  33.   pinMode(BIN2, OUTPUT);
  34.   pinMode(PWMB, OUTPUT);
  35.  
  36.   pinMode(2, INPUT_PULLUP);
  37.   pinMode(3, INPUT_PULLUP);
  38.  
  39.   Serial.begin(9600);     //begin serial communication with the computer
  40.  
  41.   attachInterrupt(digitalPinToInterrupt(2),lMotorCount,CHANGE);  
  42.   attachInterrupt(digitalPinToInterrupt(3),rMotorCount,CHANGE);  
  43.  
  44.   Serial.print("To infinity and beyond!");  //test the serial connection
  45. }
  46.  
  47. /********************************************************************************/
  48.  
  49. void loop(){
  50.  
  51.   if(digitalRead(switchPin) == HIGH){
  52.     moveF(587);
  53.     turnR(170);
  54.     moveF(587);
  55.     turnR(170);
  56.     moveF(587);
  57.     turnR(170);
  58.     moveF(587);
  59.     turnR(170);  
  60.   }
  61. }
  62.  
  63. void moveF(volatile unsigned int countR){
  64.   ticks = 0;
  65.   for(int i - 0; i < countR; i++){
  66.     leftMotor(255);
  67.     rightMotor(255);
  68.   }
  69.  leftMotor(0);
  70.  rightMotor(0);
  71.  delay(500)
  72. }
  73.  
  74. void turnL(volatile unsigned int countR){
  75.   ticks = 0;
  76.   for(int i - 0; i < countR; i++){
  77.     leftMotor(-255);
  78.     rightMotor(255);
  79.   }
  80.  leftMotor(0);
  81.  rightMotor(0);
  82.  delay(500)
  83. }
  84.  
  85. void turnR(volatile unsigned int countR){
  86.   ticks = 0;
  87.   for(int i - 0; i < countR; i++){
  88.     leftMotor(255);
  89.     rightMotor(-255);
  90.   }
  91.  leftMotor(0);
  92.  rightMotor(0);
  93.  delay(500)
  94. }
  95.  
  96. void rMotorCount(){
  97.   countR++;
  98. }
  99. void lMotorCount(){
  100.   countL++;
  101. }
  102. /********************************************************************************/
  103. void rightMotor(int motorSpeed)                       //function for driving the right motor
  104. {
  105.   if (motorSpeed > 0)                                 //if the motor should drive forward (positive speed)
  106.   {
  107.     digitalWrite(AIN1, HIGH);                         //set pin 1 to high
  108.     digitalWrite(AIN2, LOW);                          //set pin 2 to low
  109.   }
  110.   else if (motorSpeed < 0)                            //if the motor should drive backwar (negative speed)
  111.   {
  112.     digitalWrite(AIN1, LOW);                          //set pin 1 to low
  113.     digitalWrite(AIN2, HIGH);                         //set pin 2 to high
  114.   }
  115.   else                                                //if the motor should stop
  116.   {
  117.     digitalWrite(AIN1, LOW);                          //set pin 1 to low
  118.     digitalWrite(AIN2, LOW);                          //set pin 2 to low
  119.   }
  120.   analogWrite(PWMA, abs(motorSpeed));                 //now that the motor direction is set, drive it at the entered speed
  121. }
  122.  
  123. /********************************************************************************/
  124. void leftMotor(int motorSpeed)                        //function for driving the left motor
  125. {
  126.   if (motorSpeed > 0)                                 //if the motor should drive forward (positive speed)
  127.   {
  128.     digitalWrite(BIN1, HIGH);                         //set pin 1 to high
  129.     digitalWrite(BIN2, LOW);                          //set pin 2 to low
  130.   }
  131.   else if (motorSpeed < 0)                            //if the motor should drive backwar (negative speed)
  132.   {
  133.     digitalWrite(BIN1, LOW);                          //set pin 1 to low
  134.     digitalWrite(BIN2, HIGH);                         //set pin 2 to high
  135.   }
  136.   else                                                //if the motor should stop
  137.   {
  138.     digitalWrite(BIN1, LOW);                          //set pin 1 to low
  139.     digitalWrite(BIN2, LOW);                          //set pin 2 to low
  140.   }
  141.   analogWrite(PWMB, abs(motorSpeed));                 //now that the motor direction is set, drive it at the entered speed
  142. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement