//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);
}