My loop is at the bottom of this code
#include <Adafruit_GPS.h>
#include <SoftwareSerial.h>
const int pwPin1 = 7;
const int pwPin2 = 6;
const int pwPin3 = 4; // Pins for sending PWM to Sonars
//motor A connected between A01 and A02
//motor B connected between B01 and B02
int STBY = 10; //standby
//Motor A
int PWMA = 13 ; //Speed control
int AIN1 = 9; //Direction
int AIN2 = 8; //Direction
//Motor B
int PWMB = 5; //Speed control
int BIN1 = 11; //Direction
int BIN2 = 12; //Direction
long pulse, inches, duration;
String stringOne;
// If you're using a GPS module:
// Connect the GPS Power pin to 5V
// Connect the GPS Ground pin to ground
// Connect the GPS TX (transmit) pin to Digital 3
// Connect the GPS RX (receive) pin to Digital 2
// (you can change the pin numbers to match your wiring):
SoftwareSerial mySerial(3, 2);
// If using hardware serial (e.g. Arduino Mega), comment out the
// above SoftwareSerial line, and enable this line instead
// (you can change the Serial number to match your wiring):
//HardwareSerial mySerial = Serial1;
Adafruit_GPS GPS(&mySerial);
// Set GPSECHO to 'false' to turn off echoing the GPS data to the Serial console
// Set to 'true' if you want to debug and listen to the raw GPS sentences.
#define GPSECHO true
// this keeps track of whether we're using the interrupt
// off by default!
boolean usingInterrupt = false;
void useInterrupt(boolean); // Func prototype keeps Arduino 0023 happy
//////////////////////////////////////////////////////////GPS Tracker Begin/////////////////////////////////////////////////////////
// Interrupt is called once a millisecond, looks for any new GPS data, and stores it
SIGNAL(TIMER0_COMPA_vect) {
char c = GPS.read();
// if you want to debug, this is a good time to do it!
#ifdef UDR0
if (GPSECHO)
if (c) UDR0 = c;
// writing direct to UDR0 is much much faster than Serial.print
// but only one character can be written at a time.
#endif
}
void useInterrupt(boolean v) {
if (v) {
// Timer0 is already used for millis() - we'll just interrupt somewhere
// in the middle and call the "Compare A" function above
OCR0A = 0xAF;
TIMSK0 |= _BV(OCIE0A);
usingInterrupt = true;
} else {
// do not call the interrupt function COMPA anymore
TIMSK0 &= ~_BV(OCIE0A);
usingInterrupt = false;
}
}
void setup()
{ stringOne = String("Sonar 3");
//////////////////////////////////////////////////TB6612FNG////////////////////////////////////////////////////////////////////
Serial.begin(115200);// connect at 115200 so we can read the GPS fast enough and echo without dropping chars
pinMode(STBY, OUTPUT);
pinMode(PWMA, OUTPUT);
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
pinMode(PWMB, OUTPUT);
pinMode(BIN1, OUTPUT);
pinMode(BIN2, OUTPUT);
//stringOne = String("Sonar 3");
// 9600 NMEA is the default baud rate for Adafruit MTK GPS's- some use 4800
GPS.begin(9600);
// uncomment this line to turn on RMC (recommended minimum) and GGA (fix data) including altitude
GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
// uncomment this line to turn on only the "minimum recommended" data
//GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
// For parsing data, we don't suggest using anything but either RMC only or RMC+GGA since
// the parser doesn't care about other sentences at this time
// Set the update rate
GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ); // 1 Hz update rate
// For the parsing code to work nicely and have time to sort thru the data, and
// print it out we don't suggest using anything higher than 1 Hz
// Request updates on antenna status, comment out to keep quiet
GPS.sendCommand(PGCMD_ANTENNA);
// the nice thing about this code is you can have a timer0 interrupt go off
// every 1 millisecond, and read data from the GPS for you. that makes the
// loop code a heck of a lot easier!
useInterrupt(true);
delay(1000);
// Ask for firmware version
mySerial.println(PMTK_Q_RELEASE);
}
//////////////////////////////////////////////////GPS END//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////TB6612FNG END INTRUCTIONS///////////////////////////////////////////////////////
void move(int motor, int speed, int direction){
//Move specific motor at speed and direction
//motor: 0 for B 1 for A
//speed: 0 is off, and 255 is full speed
//direction: 0 clockwise, 1 counter-clockwise
digitalWrite(STBY, HIGH); //disable standby
boolean inPin1 = LOW;
boolean inPin2 = HIGH;
if(direction == 1){
inPin1 = HIGH;
inPin2 = LOW;
}
if(motor == 1){
digitalWrite(AIN1, inPin1);
digitalWrite(AIN2, inPin2);
analogWrite(PWMA, speed);
}else{
digitalWrite(BIN1, inPin1);
digitalWrite(BIN2, inPin2);
analogWrite(PWMB, speed);
}
}
void stop(){
//enable standby
digitalWrite(STBY, LOW);
}
////////////////////////////////////////////////////END TB1266FNG INSTRUCTIONS//////////////////////////////////////////////////////////////////////////////
uint32_t timer = millis();
void loop (){
pinMode (pwPin1, INPUT);
pulse = pulseIn(pwPin1, HIGH);
inches = pulse / 147; //147uS per inch
if (inches < 15){
move (1, 150, 1);//motor 1, full left
move (2, 0, 1);//motor 2, full left
delay(100);
stop();}
//delay(250);
Serial.print("Sonar 1,");
Serial.print(inches);
Serial.print("in ");
Serial.println();
pinMode (pwPin2, INPUT);
pulse = pulseIn(pwPin2, HIGH);
inches = pulse / 147; //147uS per inch
duration = pulseIn(pwPin2,HIGH);
if (inches < 15) {
move (1, 0, 1);//motor 1, full left
move (2, 150, 1);//motor 2, full left
delay(100);
stop();}
//delay(250);
Serial.print("Sonar 2,");
Serial.print(inches);
Serial.print("in ");
Serial.println();
pinMode (pwPin3, INPUT);
pulse = pulseIn(pwPin3, HIGH);
inches = pulse/147; //147uS per inch
if (inches < 15)
{ move (1, 150, 1);//motor 1, full left
move (2, 150, 1);//motor 2, full left
delay(100);
stop();
delay(2000);}
Serial.print("Sonar 3,");
Serial.print(inches);
Serial.print("in");
Serial.println();
if (! usingInterrupt) {
// read data from the GPS in the 'main loop'
char c = GPS.read();
// if you want to debug, this is a good time to do it!
if (GPSECHO)
if (c) Serial.print(c);
}
// if a sentence is received, we can check the checksum, parse it...
if (GPS.newNMEAreceived()) {
//Serial.println(GPS.lastNMEA()); // this also sets the newNMEAreceived() flag to false
if (!GPS.parse(GPS.lastNMEA())) // this also sets the newNMEAreceived() flag to false
return; // we can fail to parse a sentence in which case we should just wait for another
}
//if millis() or timer wraps around, we'll just reset it
if(timer > millis())
timer = millis();
// approximately every 2 seconds or so, print out the current stats
if (millis() - timer > 2000) {
timer = millis(); // reset the timer
Serial.print("\nTime: ");
Serial.print(GPS.hour, DEC); Serial.print(':');
Serial.print(GPS.minute, DEC); Serial.print(':');
Serial.print(GPS.seconds, DEC); Serial.print('.');
Serial.println(GPS.milliseconds);
Serial.print("Date: ");
Serial.print(GPS.day, DEC); Serial.print('/');
Serial.print(GPS.month, DEC); Serial.print("/20");
Serial.println(GPS.year, DEC);
Serial.print("Fix: "); Serial.print((int)GPS.fix);
Serial.print(" quality: "); Serial.println((int)GPS.fixquality);
if (GPS.fix) {
Serial.print("Location: ");
Serial.print(GPS.latitude, 4); Serial.print(GPS.lat);
Serial.print(", ");
Serial.print(GPS.longitude, 4); Serial.println(GPS.lon);
Serial.print("Location (in degrees, works with Google Maps): ");
Serial.print(GPS.latitudeDegrees, 4);
Serial.print(", ");
Serial.println(GPS.longitudeDegrees, 4);
Serial.print("Speed (knots): "); Serial.println(GPS.speed);
Serial.print("Angle: "); Serial.println(GPS.angle);
Serial.print("Altitude: "); Serial.println(GPS.altitude);
Serial.print("Satellites: "); Serial.println((int)GPS.satellites);
}
}
}