Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- //the right motor will be controlled by the motor A pins on the motor driver
- const int AIN1 = 13; //control pin 1 on the motor driver for the right motor
- const int AIN2 = 12; //control pin 2 on the motor driver for the right motor
- const int PWMA = 11; //speed control pin on the motor driver for the right motor
- //the left motor will be controlled by the motor B pins on the motor driver
- const int PWMB = 10; //speed control pin on the motor driver for the left motor
- const int BIN2 = 9; //control pin 2 on the motor driver for the left motor
- const int BIN1 = 8; //control pin 1 on the motor driver for the left motor
- int switchPin = 6; //switch to turn the robot on and off
- //robot behaviour variables
- int backupTime = 300; //amount of time that the robot will back up when it senses an object
- int turnTime = 650; //amount that the robot will turn once it has backed up
- volatile unsigned int countL = 0;
- volatile unsigned int countR = 0;
- /********************************************************************************/
- void setup()
- {
- pinMode(switchPin, INPUT_PULLUP); //set this as a pullup to sense whether the switch is flipped
- //set the motor contro pins as outputs
- pinMode(AIN1, OUTPUT);
- pinMode(AIN2, OUTPUT);
- pinMode(PWMA, OUTPUT);
- pinMode(BIN1, OUTPUT);
- pinMode(BIN2, OUTPUT);
- pinMode(PWMB, OUTPUT);
- pinMode(2, INPUT_PULLUP);
- pinMode(3, INPUT_PULLUP);
- Serial.begin(9600); //begin serial communication with the computer
- attachInterrupt(digitalPinToInterrupt(2),lMotorCount,CHANGE);
- attachInterrupt(digitalPinToInterrupt(3),rMotorCount,CHANGE);
- Serial.print("To infinity and beyond!"); //test the serial connection
- }
- /********************************************************************************/
- void loop(){
- if(digitalRead(switchPin) == HIGH){
- moveF(587);
- turnR(170);
- moveF(587);
- turnR(170);
- moveF(587);
- turnR(170);
- moveF(587);
- turnR(170);
- }
- }
- void moveF(volatile unsigned int countR){
- countR = 0;
- for(int i - 0; i < countR; i++){
- leftMotor(255);
- rightMotor(255);
- }
- leftMotor(0);
- rightMotor(0);
- delay(500);
- }
- void turnL(volatile unsigned int countR){
- countR = 0;
- for(int i - 0; i < countR; i++){
- leftMotor(-255);
- rightMotor(255);
- }
- leftMotor(0);
- rightMotor(0);
- delay(500);
- }
- void turnR(volatile unsigned int countR){
- countR = 0;
- for(int i - 0; i < countR; i++){
- leftMotor(255);
- rightMotor(-255);
- }
- leftMotor(0);
- rightMotor(0);
- delay(500);
- }
- void rMotorCount(){
- countR++;
- }
- void lMotorCount(){
- countL++;
- }
- /********************************************************************************/
- void rightMotor(int motorSpeed) //function for driving the right motor
- {
- if (motorSpeed > 0) //if the motor should drive forward (positive speed)
- {
- digitalWrite(AIN1, HIGH); //set pin 1 to high
- digitalWrite(AIN2, LOW); //set pin 2 to low
- }
- else if (motorSpeed < 0) //if the motor should drive backwar (negative speed)
- {
- digitalWrite(AIN1, LOW); //set pin 1 to low
- digitalWrite(AIN2, HIGH); //set pin 2 to high
- }
- else //if the motor should stop
- {
- digitalWrite(AIN1, LOW); //set pin 1 to low
- digitalWrite(AIN2, LOW); //set pin 2 to low
- }
- analogWrite(PWMA, abs(motorSpeed)); //now that the motor direction is set, drive it at the entered speed
- }
- /********************************************************************************/
- void leftMotor(int motorSpeed) //function for driving the left motor
- {
- if (motorSpeed > 0) //if the motor should drive forward (positive speed)
- {
- digitalWrite(BIN1, HIGH); //set pin 1 to high
- digitalWrite(BIN2, LOW); //set pin 2 to low
- }
- else if (motorSpeed < 0) //if the motor should drive backwar (negative speed)
- {
- digitalWrite(BIN1, LOW); //set pin 1 to low
- digitalWrite(BIN2, HIGH); //set pin 2 to high
- }
- else //if the motor should stop
- {
- digitalWrite(BIN1, LOW); //set pin 1 to low
- digitalWrite(BIN2, LOW); //set pin 2 to low
- }
- analogWrite(PWMB, abs(motorSpeed)); //now that the motor direction is set, drive it at the entered speed
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement