Hi im not very good at programming, basically my project is to do a self avoidance robot w self charging.
Ignoring the self charging part, i am trying to make my robot to go back to "home" once it travels for either x amount of time, or by travelling x distance just to show that it works.
Currently, i am only at the level of making my robot move by 30cm, then reverse back to home in a straight line.
My question is, i am trying to make it to move around while avoiding objects, and then return back to home. Is it doable with my method?
Code as below.
#include "TimerOne.h"
#include <AFMotor.h> //add Adafruit Motor Shield library
#include <NewPing.h> //add Ultrasonic sensor library
#include <HCSR04.h>
// Initialize sensor that uses digital pins 13 and 12 for Ultrasonic sensor
int triggerPin = 12;
int echoPin = 13;
UltraSonicDistanceSensor distanceSensor(triggerPin, echoPin);
//For light encoder for motor
const int Left_IN_A0 = A0; // analog input
const int Left_IN_D2 = 2; // digital input
const byte MOTOR1 = 2; // Motor 1 Interrupt Pin - INT 0
//const byte MOTOR2 = 3; // Motor 2 Interrupt Pin - INT 1
AF_DCMotor leftMotor (2, MOTOR12_1KHZ); //M2 in
AF_DCMotor rightMotor (4, MOTOR12_1KHZ);
// Integers for pulse counters
unsigned int counter1 = 0;
unsigned int counter2 = 0;
// Float for number of slots in encoder disk
float diskslots = 20; // Change to match value of encoder disk
void ISR_count1()
{
counter1++; // increment Motor 1 counter value
}
void ISR_count2()
{
counter2++; // increment Motor 1 counter value
}
/*void ISR_timerone()
{
Timer1.detachInterrupt(); // Stop the timer
Serial.print("Motor Speed 1: ");
float rotation1 = (counter1 / diskslots); // calculate RPS for Motor 1
float distance = (rotation1); //circmference of wheel x rotation
Serial.print(distance);
Serial.println(" cm - ");
//counter1 = 0; // reset counter to zero
Timer1.attachInterrupt( ISR_timerone ); // Enable the timer
}
*/
void setup() {
Serial.begin(9600);
Timer1.initialize(500000); // set timer for 1sec
attachInterrupt(digitalPinToInterrupt (MOTOR1), ISR_count1, RISING); // Increase counter 1 when speed sensor pin goes High
// Timer1.attachInterrupt( ISR_timerone ); // Enable the timer
leftMotor.setSpeed(210);
leftMotor.run(RELEASE);
rightMotor.setSpeed(205);
rightMotor.run(RELEASE);
}
void loop() {
float rotation1 = (counter1 / diskslots); // calculate RPS for Motor 1
float distance1 = (rotation1); //circmference of wheel x rotation
Serial.print(distance1);
Serial.print(" cm - ");
if (distance1 <= 30.00)
{
moveForward();
}
else
goBackToHome();
}
void goBackToHome(){
attachInterrupt(digitalPinToInterrupt (MOTOR1), ISR_count2, RISING);
float rotation2 = (counter2 / diskslots); // calculate RPS for Motor 1
float distance2 = (rotation2); //circmference of wheel x rotation
Serial.print(distance2);
Serial.println(" cm - ");
moveBackward();
if ( distance2 >= 30){
motorStop();
}
}
void turnRight() {
//motorSet = "RIGHT";
leftMotor.run(FORWARD); // turn left motor forward
rightMotor.run(BACKWARD); // turn right motor backward
delay(1200); // run motors this way for 1000
//motorSet = "FORWARD";
leftMotor.run(FORWARD);
rightMotor.run(FORWARD);
}
void moveForward() {
//motorSet = "FORWARD";
leftMotor.run(FORWARD); // turn it on going forward
rightMotor.run(FORWARD); // turn it on going forward
}
void moveBackward() {
//motorSet = "FORWARD";
leftMotor.run(BACKWARD); // turn it on going forward
rightMotor.run(BACKWARD); // turn it on going forward
}
void motorStop() {
leftMotor.run(RELEASE);
rightMotor.run(RELEASE);
}
schematic diagram (POSTER + REPORT).pdf (75.4 KB)
schematic diagram (POSTER + REPORT).pdf (75.4 KB)