The Dandelion Hunter

//GPS not yet implemented
#include <Adafruit_GPS.h>
#include <PWMServo.h>
#include <Pixy2I2C.h>
//define pins
#define WEAPON 2
#define SERVO_PAN 23
#define M1_1 38
#define M1_2 37
#define M2_1 35
#define M2_2 36
#define CENTER_POS 158
#define DEADZONE 10
#define ANGLE_DELAY 10 //number of ms per servo angle
#define TARGET_WIDTH 100 //from 1-316
#define TARGET_HEIGHT 60 //from 1-208
#define GPSSERIAL Serial1
#define GPSECHO false
//create objects
Pixy2I2C pixy;
PWMServo pan_servo;
Adafruit_GPS GPS(&GPSSERIAL);
IntervalTimer gpsTimer;
int largest_size = 0, currently_tracked = 0, servo_pos = 90, servo_flag = -1;
double home_lat, home_lon;
void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  init_pins();
  pixy.init();
  init_GPS();
}
void loop() {
  // put your main code here, to run repeatedly:
  pixy.ccc.getBlocks();
  if(pixy.ccc.numBlocks){
      pixy.ccc.blocks[0].print();
      if(servo_pos != 90){
        if(servo_pos < 90){
          set_motors(20, -20);
          delay((90-servo_pos)*ANGLE_DELAY);
          set_motors(0, 0);
        }
        else if(servo_pos > 90){
          set_motors(-20, 20);
          delay((servo_pos-90)*ANGLE_DELAY);
          set_motors(0, 0);
        }
        servo_pos = 90;
        pan_servo.write(90);
      }
      while(pixy.ccc.blocks[0].m_width * pixy.ccc.blocks[0].m_height < TARGET_HEIGHT * TARGET_WIDTH){
        pixy.ccc.getBlocks();
        if(!pixy.ccc.numBlocks){
          set_motors(0, 0);
          break;
        }
        else{
          pixy.ccc.blocks[0].print();
        if(pixy.ccc.blocks[0].m_x < CENTER_POS-DEADZONE){
          set_motors(-20, 20);
          Serial.println("Moving left");
          delay(50);
          set_motors(0, 0);
        }
        else if((pixy.ccc.blocks[0].m_x >= CENTER_POS-DEADZONE) && (pixy.ccc.blocks[0].m_x <= CENTER_POS+DEADZONE)){
          set_motors(60, 60);
          Serial.println("Moving forward");
          delay(50);
          set_motors(0, 0);
        }
        else if(pixy.ccc.blocks[0].m_x > CENTER_POS+DEADZONE){
          set_motors(20, -20);
          Serial.println("Moving right");
          delay(50);
          set_motors(0, 0);
        }
      }
      }
      if(pixy.ccc.blocks[0].m_width * pixy.ccc.blocks[0].m_height>=TARGET_HEIGHT * TARGET_WIDTH){
        kill_flower();
      }
  }
  else{
    set_motors(0, 0);
    if(!servo_pos){
      servo_flag = 1;
    }
    else if(servo_pos >= 180){
      servo_flag = -1;
    }
    servo_pos += servo_flag;
    pan_servo.write(servo_pos);
  }
}
void kill_flower(){
  set_motors(0, 0);
  set_motors(-60, -60);
  delay(700);
  analogWrite(WEAPON, 255);
  set_motors(80, 80);
  delay(1200);
  set_motors(0, 0);
  analogWrite(WEAPON, 0);
  largest_size = 0;
  currently_tracked = 0;
}
void set_motors(uint8_t m1_val, uint8_t m2_val){
   if(m1_val<0){
      analogWrite(M1_1, 0);
      analogWrite(M1_2, m1_val*-1);
   }
   else{
      analogWrite(M1_1, m1_val);
      analogWrite(M1_2, 0);
   }
   if(m2_val<0){
      analogWrite(M2_1, 0);
      analogWrite(M2_2, m2_val*-1);
   }
   else{
      analogWrite(M2_1, m2_val);
      analogWrite(M2_2, 0);
   }
}
void init_pins(){
  pinMode(M1_1, OUTPUT);
  pinMode(M1_2, OUTPUT);
  pinMode(M2_1, OUTPUT);
  pinMode(M2_2, OUTPUT);
  analogWrite(M1_1, 0);
  analogWrite(M1_2, 0);
  analogWrite(M2_1, 0);
  analogWrite(M2_2, 0);
  pinMode(WEAPON, OUTPUT);
  analogWrite(WEAPON, 0);
  pan_servo.attach(SERVO_PAN);
  pan_servo.write(90);
}
void print_GPS(){
      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/100, 5); Serial.print(GPS.lat);
      Serial.print(", ");
      Serial.print(GPS.longitude/100, 5); Serial.println(GPS.lon);
      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);
    }
}
void init_GPS(){
  GPS.begin(9600);
  GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
  GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
  delay(1000);
  GPSSERIAL.println(PMTK_Q_RELEASE);
  char c;
  while(!GPS.newNMEAreceived()){
    c = GPS.read();
  }
  if(GPS.newNMEAreceived()){
    print_GPS();
  }
  //gpsTimer.begin(print_GPS, 3000000);
}