//Cyclops5: Hori and Vert, also: bring back the motors and indicators...
#include <AFMotor.h> // Adafruit MotorShield
#include <VarSpeedServo.h> // VarSpeedServo library gives more options, but variable speed isnt used yet
const int PR = A1; // connect PR to A1
int Array[125]; // array to store sweep values
VarSpeedServo Hori;
int HoriPos = 90; // centre is 90. hardware modififed to prevent clashes. 30 (left) to 150 (right)
int CWHoriDir = 0;
int CCWHoriDir = 0;
VarSpeedServo Vert;
int VertPos = 90; // 130 = level, 0 is back on itself, 30 is straight up
int UpVertDir = 0;
int DownVertDir = 0;
AF_DCMotor motorL(3, MOTOR12_1KHZ);
AF_DCMotor motorR(2, MOTOR12_1KHZ);
const int RLED = 13; // indicator LED: 13 and 2 are the only properly unused digi pins on the motorshield.
const int LLED = 2; // indicator LED
int VertDelay; // integer used to calculate "distance" to travel. Shallow angle on Vert indicates distance to light is greater,and increases time to run motors.
int HoriDelay; // Steep (vertical) angle indicates closer to light and therefore moves in small steps to help prevent over run
// front sensors and integers for calibration
const int LeftPR = A2; // pin that the sensor is attached to
const int CentrePR = A3; // pin that the sensor is attached to
const int RightPR = A4; // pin that the sensor is attached to
int LeftValue = 0; // the sensor value
int LeftMin = 1023; // minimum sensor value
int LeftMax = 0; // maximum sensor value
int CentreValue = 0; // the sensor value
int CentreMin = 1023; // minimum sensor value
int CentreMax = 0; // maximum sensor value
int RightValue = 0; // the sensor value
int RightMin = 1023; // minimum sensor value
int RightMax = 0; // maximum sensor value
unsigned long previous = 0;
unsigned long current = 0;
unsigned long interval = 1000;
void setup() { // Setup*******************
Serial.begin(9600); // Serial at 9600 bps
Hori.attach(9); // 9 is "Servo2"
Vert.attach(10); // 10 is "Servo1"
motorL.setSpeed(250); // 0 is stop, 255 is full speed:
motorR.setSpeed(250); //
pinMode (RLED,OUTPUT); // indicator LEDS. HIGH is on, LOW is off
pinMode (LLED,OUTPUT);
// **************calibrate ************
{
while (millis() < 5000) {
//Spin on the spot
motorL.run(FORWARD);
motorR.run(BACKWARD);
digitalWrite(LLED, LOW);
digitalWrite(RLED, HIGH);
//Calibrate
LeftValue = analogRead(LeftPR);
// record the maximum sensor value
if (LeftValue > LeftMax) {
LeftMax = LeftValue;
}
// record the minimum sensor value
if (LeftValue < LeftMin) {
LeftMin = LeftValue;
}
CentreValue = analogRead(CentrePR);
// record the maximum sensor value
if (CentreValue > CentreMax) {
CentreMax = CentreValue;
}
// record the minimum sensor value
if (CentreValue < CentreMin) {
CentreMin = CentreValue;
}
RightValue = analogRead(RightPR);
// record the maximum sensor value
if (RightValue > RightMax) {
RightMax = RightValue;
}
// record the minimum sensor value
if (RightValue < RightMin) {
RightMin = RightValue;
}
}
motorL.run(RELEASE);
motorR.run(RELEASE);
digitalWrite(LLED, LOW);
digitalWrite(RLED, LOW);
// ***************end of the calibration******************
BlindSpot(); //BlindSpot Check
unsigned long current = millis();
}
}
void BlindSpot (){
for(VertPos = 100; VertPos >= 1; VertPos -= 1)
{
Array[VertPos-1] = analogRead(PR);
Vert.write(VertPos);
if ((Array[VertPos-1] < Array[VertPos]) && (VertPos<100)) {
UpVertDir = VertPos;
}
delay (10) ;
}
for(VertPos = 1; VertPos < 100; VertPos += 1)
{
Array[VertPos-1] = analogRead(PR);
Vert.write(VertPos);
if ((Array[VertPos-1] < Array[VertPos+1]) && (VertPos>1)) {
DownVertDir = VertPos;
}
delay (10) ;
}
VertPos = ((UpVertDir+DownVertDir)/2); // Average the results to get the centre
Vert.write(VertPos); // Go to the position and print the results
Serial.print("UpSweep: ");
Serial.print(UpVertDir);
Serial.print(", DownSweep: ");
Serial.print(DownVertDir);
Serial.print(", VertPos: ");
Serial.println(VertPos);
delay (100);
Vert.write(100);
if(VertPos < 50){
motorL.run(FORWARD);
motorR.run(BACKWARD);
digitalWrite(LLED, LOW);
digitalWrite(RLED, HIGH);
delay (1750);
motorL.run(RELEASE);
motorR.run(RELEASE);
digitalWrite(LLED, LOW);
digitalWrite(RLED, LOW);
}
}
void Decisions(){
if((HoriPos > 110) && (HoriPos <= 150)){// single wheel moving
HoriDelay = map (HoriPos, 110, 150, 100,1000);
if (current - previous < interval){
motorL.run(FORWARD);
motorR.run(BACKWARD);
digitalWrite(LLED, LOW);
digitalWrite(RLED, HIGH);
}
else{
motorL.run(RELEASE);
motorR.run(RELEASE);
digitalWrite(LLED, LOW);
digitalWrite(RLED, LOW);
}
}
//LEFT
if((HoriPos < 70) && (HoriPos >= 30)){ // Shallow angle results in single wheel moving
HoriDelay = map (HoriPos, 70, 30, 100, 1000);
if (current - previous < interval){
motorL.run(BACKWARD);
motorR.run(FORWARD);
digitalWrite(LLED, HIGH);
digitalWrite(RLED, LOW);
}
else{
motorL.run(RELEASE);
motorR.run(RELEASE);
digitalWrite(LLED, LOW);
digitalWrite(RLED, LOW);
}
}
// FORWARD run forward: Window of 20 degrees in the centre results in running forward
if((HoriPos >= 70) && (HoriPos <= 110)){
VertDelay = map (VertPos, 130, 30, 10000, 500);
if (current - previous < interval){
motorL.run(FORWARD);
motorR.run(FORWARD);
digitalWrite(LLED, HIGH);
digitalWrite(RLED, HIGH);
}
else{
motorL.run(RELEASE);
motorR.run(RELEASE);
digitalWrite(LLED, LOW);
digitalWrite(RLED, LOW);
}
}
}
void HoriSweep (){ // Horizontal Sweep********
for(HoriPos = 30; HoriPos < 150; HoriPos += 1)
{
Array[HoriPos-30] = analogRead(PR);
Hori.write(HoriPos);
if ((Array[HoriPos-30] < Array[HoriPos-31]) && (HoriPos>30)) {
CWHoriDir = HoriPos;
}
delay (10) ;
}
for(HoriPos = 150; HoriPos >= 30; HoriPos -= 1)
{
Array[HoriPos-30] = analogRead(PR);
Hori.write(HoriPos);
if ((Array[HoriPos-30] < Array[HoriPos-29]) && (HoriPos<150)) {
CCWHoriDir = HoriPos;
}
delay (10) ;
}
HoriPos = ((CWHoriDir+CCWHoriDir)/2); // Average the two value to get the centre
Hori.write(HoriPos); // Go to the position, and print the results to serial
Serial.print("CWSweep: ");
Serial.print(CWHoriDir);
Serial.print(", CCWSweep: ");
Serial.print(CCWHoriDir);
Serial.print(", HoriPos: ");
Serial.println(HoriPos);
delay (500);
}
void VertSweep (){ // Vertical Sweep*************
for(VertPos = 130; VertPos >= 30; VertPos -= 1)
{
Array[VertPos-30] = analogRead(PR);
Vert.write(VertPos);
if ((Array[VertPos-30] < Array[VertPos-29]) && (VertPos<130)) {
UpVertDir = VertPos;
}
delay (10) ;
}
for(VertPos = 30; VertPos < 130; VertPos += 1)
{
Array[VertPos-30] = analogRead(PR);
Vert.write(VertPos);
if ((Array[VertPos-30] < Array[VertPos-31]) && (VertPos>30)) {
DownVertDir = VertPos;
}
delay (10) ;
}
VertPos = ((UpVertDir+DownVertDir)/2); // Average the results to get the centre
Vert.write(VertPos); // Go to the position and print the results
Serial.print("UpSweep: ");
Serial.print(UpVertDir);
Serial.print(", DownSweep: ");
Serial.print(DownVertDir);
Serial.print(", VertPos: ");
Serial.println(VertPos);
delay (100);
}
void loop() {
HoriPos = constrain (HoriPos, 30, 150); // constrain servos, Just in case.
VertPos = constrain (VertPos, 30, 130);
HoriSweep(); //Horizontal Scan
VertSweep(); // Vertical Scan
//********************************
previous = current;
//********************************
//Stop under light
if(VertPos < 40){
motorL.run(RELEASE);
motorR.run(RELEASE);
digitalWrite(LLED, LOW);
digitalWrite(RLED, LOW);
delay (99999999); // lazy mans ending
}
else {
Decisions(); // Decisions Decisions
}
}