/*
    Name:       FindACone.ino
    Created:	8/29/2018 12:57:16 PM
    Author:     DELLGARAGE\Ron


	1300 = CW   Full Speed
	1500 = Stop
	1700 = CCW  Full Speed
*/

#include <Servo.h>      // Include servo library
#include <SPI.h>			//be sure to add this!
#include <Pixy2.h>


// This is the main myPixy object 
Pixy2 myPixy;

Servo LServo;                        // Declare left servo signal Pin 5
Servo RServo;                       // Declare right servo signal Pin 6

//---PWM settings
int const CW = 1300;
int const CCW = 1700;
int const HALT = 1500;


void setup()
{
	LServo.attach(5);                // Attach left signal to pin 5, not 13   Interferes with ICSP bus the Pixy Uses
	RServo.attach(6);                // Attach right signal to pin 6, not 12 
	
	delay(2000);
	myPixy.init();
	Forward();
}


void loop()
{
	
	myPixy.ccc.getBlocks();

	// If there are detected blocks
	if (myPixy.ccc.numBlocks)
	{

		for (int i = 0; i < myPixy.ccc.numBlocks; i++)
		{
			if (myPixy.ccc.blocks[i].m_height >= 110)
			{
				Stop();
				BackWards();
				LeftTurn();
				Forward();

			}
		}
	}
	

}

void Forward()
{
	//---forward
	LServo.writeMicroseconds(CCW);
	RServo.writeMicroseconds(CW);
	delay(2000);
}

void BackWards()
{
	//---back
	LServo.writeMicroseconds(CW);
	RServo.writeMicroseconds(CCW);
	delay(2000);}

void LeftTurn()
{
	//---left
	LServo.writeMicroseconds(CCW);
	RServo.writeMicroseconds(CCW);
	delay(1000);
}

void RightTurn()
{
	//---right
	LServo.writeMicroseconds(CW);
	RServo.writeMicroseconds(CW);
	delay(1000);
}

void Stop()
{
	//---stop
	LServo.writeMicroseconds(HALT);     
	RServo.writeMicroseconds(HALT);
	delay(2000);
}