Arduino-Controlled, Light-Tracking, 2-Axis Rotating Turret

/*
 
Light Tracking Turret

Circuit and comments: 
See http://www.cesarebrizio.it/Arduino/Light_Tracking_Turret.html
 
 created 27 Sep 2014
 modified ----
 by Cesare Brizio
 
This example code is in the public domain.

This sketch controls a light-tracking turret based on two stepper motors.
The turret can rotate about 90Deg in the vertical and 180Deg in the 
horizontal plane, and so tracking directions include up-down and left-right. 
The turret hasn't rotation limit switches and needs manual centering before 
starting the software, that implements soft rotation limits.
A 4-partition head, each partition containing a photoresistor, is continuously 
aimed at light by minimizing the delta among the four photoresistor, by moving
in the direction of the most illuminated, minimum resistance photoresistor 
(the one that will give the highest reading). 
Sources of information:
Manual start/stop switch: http://arduino.cc/en/Tutorial/Debounce
Speaker: http://arduino.cc/en/Tutorial/Tone
Small stepper control: http://arduino-info.wikispaces.com/SmallSteppers
Photoresistors: https://learn.adafruit.com/photocells/using-a-photocell


*/

/*-----( Import needed libraries )-----*/
#include "pitches.h"
#include <AccelStepper.h>

/*-----( Declare Constants and Pin Numbers )-----*/
/* NEVER PUT ; AFTER A #define statement!!!! */
#define FULLSTEP 4
#define HALFSTEP 8
// motor pins
#define motorPin1  4     // Blue   - 28BYJ48 pin 1
#define motorPin2  5     // Pink   - 28BYJ48 pin 2
#define motorPin3  6     // Yellow - 28BYJ48 pin 3
#define motorPin4  7     // Orange - 28BYJ48 pin 4
                        // Red    - 28BYJ48 pin 5 (VCC)
                        
#define motorPin5  8     // Blue   - 28BYJ48 pin 1
#define motorPin6  9     // Pink   - 28BYJ48 pin 2
#define motorPin7  10    // Yellow - 28BYJ48 pin 3
#define motorPin8  11     // Orange - 28BYJ48 pin 4
                        // Red    - 28BYJ48 pin 5 (VCC)

/*-----( Objects for stepper control )-----*/
// NOTE: The sequence 1-3-2-4 is required for proper sequencing of 28BYJ48
AccelStepper stepper1(HALFSTEP, motorPin1, motorPin3, motorPin2, motorPin4);
AccelStepper stepper2(HALFSTEP, motorPin5, motorPin7, motorPin6, motorPin8);

/*
Constant for azimuth control
------------------------------------
Turret will allow an horizontal rotation of 180Deg 
from 0Deg (leftmost) to 180Deg (rightmost) position,
and a vertical rotation of 90Deg from 0Deg (downmost)
to 90Deg (upmost) position. It will be centered 
manually before program start. 
Tentatively, I assume that the stepper 
will be rotated in 4,5Deg increments (50 steps), so that 180Deg will require 
40 increments (the real number of increments will be empirically 
determined depending from stepper features). Heading 
is expressed by increment number. Thus, the initial neutral 
horizontal heading will correspond to increment 21 (90Deg), while 
the initial neutral vertical heading will correspond to 
increment 11 (45Deg).
*/                        
#define leftLimit  0           // leftmost increment no.
#define neutralHorHeading  21  // mid range increment no., tentatively set to 21 (90Deg)
#define rightLimit  40         // leftmost increment no., tentatively set to 40 (180Deg)
#define downLimit  0           // leftmost increment no.
#define neutralVerHeading  11  // mid range increment no., tentatively set to 11 (45Deg)
#define upLimit  20            // leftmost increment no., tentatively set to 20 (90Deg)

// 4 photoresitors on analog lines 0-3
#define photoResUp A0  
#define photoResDown A1  
#define photoResRight A2  
#define photoResLeft A3  
// 2 LED's on digital lines 2-3
#define ledUp 2  // If moving right or left ...
#define ledDown 2 // ... set Led 2 on
#define ledRight 3 // If moving up or down ...
#define ledLeft 3 // ... set Led 3 on
// a "SLEEP" pin to store last activation state
#define SLEEP 12       // PIN 12 = SLP
#define switchPin 13   //define switch to pin 13
// constants and variables declaration and setup
int threshold = 250; // light threshold derived form experiments
boolean lastButton = LOW;  //part of debounce function
boolean currentButton = LOW;  //part of debounce function
boolean work = LOW;  //low puts driver into sleep mode, high turns it on
boolean firstTimeEver = HIGH;  //used just once to perform a full rotation cycle
// notes in the melody:
int melody[] = {NOTE_C4, NOTE_G3,NOTE_G3, NOTE_A3, NOTE_G3,0, NOTE_B3, NOTE_C4};
// note durations: 4 = quarter note, 8 = eighth note, etc.:
int noteDurations[] = {4,8,8,4,4,4,4,4 };
// variables to define the sequence of execution of the notes
int note = 1;
/* Variables currXxxHeading contain increment number
   and are used to check the rotation limits */
int currHorHeading = 0;
int currVerHeading = 0;
/* Variables currXxxPosition contain step number
   and are used to control the stepper */
int currHorPosition = 0;
int currVerPosition = 0;

/* Not all photoresistors were born equal...
to avoid resistance readings affected by differences
among the photoresistors, the turret head was
put under direct, perpendicular, even illumination 
and one of the photoresistors was chosen as reference.
Thus, a normalization factor was empyrically determined
as the multiplying factor to obtain an even reading by
all the photoresistors */
float normalizeResUp=1.08;
float normalizeResDown=1; // reference for the other photoresistors
float normalizeResRight=1.02;
float normalizeResLeft=0.93; 

/*-----( Allowed delta between opposite photoresistors 
readings - if exceeded, triggers stepper )-----
declared as float just to be safe - it
will be compared with floating point values
*/
float allowedDelta=50;

void play_note(int thisNote) {
         // to calculate the note duration, take one second 
         // divided by the note type.
         //e.g. quarter note = 1000 / 4, eighth note = 1000/8, etc.
         int noteDuration = 1000/noteDurations[thisNote];
         tone(12, melody[thisNote],noteDuration);
         // to distinguish the notes, set a minimum time between them.
         // the note's duration + 30% seems to work well:
         int pauseBetweenNotes = noteDuration * 1.30;
         delay(pauseBetweenNotes);
         // stop the tone playing:
         noTone(12);
  }
  
void play_tune_1() {
      // plays a tune in direct direction
      Serial.println("motivo 1");
             for (note = 0; note < 8; note = note + 1) {
                 play_note(note);
             }
     }
     
void play_tune_2() {
      // plays a tune in reverse direction
      Serial.println("motivo 2");
             for (note = 8; note > 0; note = note - 1) {
                 play_note(note);
             }
     }

boolean debounce(boolean last)  //debounce function for switch
{
  boolean current = digitalRead(switchPin);
  if (last != current)
  {
    delay(5);
    current = digitalRead(switchPin);
  }
  return current;
}


void rotateAzimuth(char direction) {
    
    currHorPosition=stepper1.currentPosition();  // Horizontal (Azimuth) position is managed by Stepper 1
    //Serial.println("Current horizontal position (steps)");
    //Serial.println(currHorPosition, DEC); // currHorPosition is printed in decimal format  
    //Serial.println("Current horizontal heading (4,5Deg increments)");
    //Serial.println(currHorHeading, DEC); // currHorHeading is printed in decimal format  
    
    switch (direction) {
        case 'L':
            //Attempt rotation left
            if(currHorHeading>leftLimit) // check if left rotation limit is reached
            {
              Serial.println("Performing left rotation");
              digitalWrite(ledLeft,HIGH); // put the left led on
              stepper1.moveTo(currHorPosition-50); // 50 microsteps left
              while (stepper1.currentPosition() != currHorPosition-50) 
                  stepper1.runSpeedToPosition();
              currHorHeading--;
              digitalWrite(ledLeft,LOW); // put the left led off
              return;
            }
            else
            {
              Serial.println("Left rotation limit reached!");
              play_note(1); // cannot move further to the left - buzz!
            }            
            break;
        case 'R':
            //Attempt rotation to the right
            if(currHorHeading<rightLimit) // check if right rotation limit is reached
            {
              Serial.println("Performing right rotation");
              digitalWrite(ledRight,HIGH); // put the right led on
              stepper1.moveTo(currHorPosition+50); // 50 microsteps right
              while (stepper1.currentPosition() != currHorPosition+50) 
                  stepper1.runSpeedToPosition();
              currHorHeading++;
              digitalWrite(ledRight,LOW); // put the right led off
            }
            else
            {
              Serial.println("Right rotation limit reached!");
              play_note(2); // cannot move further to the right - buzz!
            }
            break;
    }
}

void rotateZenith(char direction) {
    
    currVerPosition=stepper2.currentPosition(); // Vertical (Zenith) position is managed by Stepper 2
    //Serial.println("Current vertical position (steps)");
    //Serial.println(currVerPosition, DEC); // currVerPosition is printed in decimal format  
    //Serial.println("Current vertical heading (4,5Deg increments)");
    //Serial.println(currVerHeading, DEC); // currVerHeading is printed in decimal format  
    
    switch (direction) {
        case 'U':
            //Attempt rotation up
            if(currVerHeading<upLimit) // check if upper rotation limit is reached
            {
                Serial.println("Performing up rotation");
                digitalWrite(ledUp,HIGH); // put the up led on
                stepper2.moveTo(currVerPosition+50); // 50 microsteps up
                while (stepper2.currentPosition() != currVerPosition+50) 
                    stepper2.runSpeedToPosition();
                currVerHeading++;
                digitalWrite(ledUp,LOW); // put the up led off
            }
            else
            {
                Serial.println("Up rotation limit reached!");
                play_note(3); // cannot move further up - buzz!
            }            
            break;
        case 'D':
            //Attempt rotation down
            if(currVerHeading>downLimit) // check if down rotation limit is reached
            {
                Serial.println("Performing down rotation");
                digitalWrite(ledDown,HIGH); // put the down led on
                stepper2.moveTo(currVerPosition-50); // 50 microsteps down
                while (stepper2.currentPosition() != currVerPosition-50) 
                    stepper2.runSpeedToPosition();
                currVerHeading--;
                digitalWrite(ledDown,LOW); // put the down led off
            }
            else
            {
                Serial.println("Down rotation limit reached!");
                play_note(4); // cannot move further down - buzz!
            }
            break;
    }
}

void operate_turret()
{
        /* Which resistor is getting most light? The one with the HIGHEST READING! The analog line
           reads a voltage, and this voltage INCREASES as resistance DECREASES as light intensity
           INCREASES - so there is a direct relation between light intensity and voltage: I should 
           rotate towards the photoresistance givving the HIGHEST reading! See also 
           https://learn.adafruit.com/photocells/using-a-photocell */

        float valLeft = analogRead(photoResLeft); // valLeft is used to save the reading from photoresistor A0 Left  
    //    Serial.println("photoresistor Left - ");
    //    Serial.println(valLeft, DEC); // valLeft is printed in decimal format  
        valLeft=valLeft*normalizeResLeft;  
    //    Serial.println("Normalized output Left - ");
    //    Serial.println(valLeft, DEC); // normalizeResLeft is printed in decimal format      

        
        float valRight = analogRead(photoResRight);  // valRight is used to save the reading from photoresistor A1 Right 
    //    Serial.println("photoresistor Right - ");
    //    Serial.println(valRight, DEC); // valRight is printed in decimal format  
        valRight=valRight*normalizeResRight;  
   //     Serial.println("Normalized output Right - ");
   //     Serial.println(valRight, DEC); // normalizeResRight is printed in decimal format     
        
        float deltaH = abs(valLeft-valRight);
        
        float valUp = analogRead(photoResUp); // valUp is used to save the reading from photoresistor A2 Up 
    //    Serial.println("photoresistor Up - ");
    //    Serial.println(valUp, DEC); // valUp is printed in decimal format  
        valUp=valUp*normalizeResUp;  
    //    Serial.println("Normalized output Up - ");
    //    Serial.println(valUp, DEC); // normalizeResUp is printed in decimal format     
        
        float valDown = analogRead(photoResDown);  // valDown is used to save the reading from photoresistor A3 Down  
     //   Serial.println("photoresistor Down - ");
     //   Serial.println(valDown, DEC); // valDown is printed in decimal format  
        valDown=valDown*normalizeResDown;  
        Serial.println("Normalized output Down - ");
        Serial.println(valDown, DEC); // normalizeResUp is printed in decimal format     
        
        float deltaV = abs(valUp-valDown);

        /* Check if horizontal rotation is needed */
        if (deltaH <= allowedDelta) // horizontal rotation NOT needed
            {Serial.println("Azimuth correct - no rotation required");}
        /* Check if vertical rotation is needed */
        if (deltaV <= allowedDelta) // vertical rotation NOT needed
            {Serial.println("Zenith correct - no rotation required");}
            
        /* Now I have the two deltas, horizontal and vertical. 
           Due to the rudimentary construction of the 4-photoresistors head,
           it seems to me that heading correction should be applied one axis 
           at a time, choosing the axis where the delta is higher. Otherwise, 
           if movement on both axes is performed in the same cycle, correction 
           on the second axis may adversely affect the correction just performed 
           on the first axis */
        
  

        /* AS LONG AS MOST OPINIONS ADVISE AGAINST THE USE OF NESTED IFs, I CHANGED THE CODE
           THAT NOW USES ITERATED IFs WITH MULTIPLE CONDITIONS IN PLACE OF NESTED IFs*/
         
        // Note: the probability of getting valLeft=valRight or valUp=valDown or deltaV=deltaH is very
        // marginal, and I don't check that condition via <= or >=, the worst possible consequence being
        // one inactive loop cycle (and surely the readings of the next cycle will show some difference
        // between the values...)
        
        // Horizontal rotation is performed if:
        // 1) deltaH is above the threshold
        // 2) deltaV is less than deltaH (otherwise I should correct deltaH first!)
        
        if (deltaH > allowedDelta && deltaH > deltaV && valLeft>valRight) // left rotation required
            {
           // Serial.println("Attempting left rotation");
            rotateAzimuth('L'); // I do not need a string / double quotes but just a char
            }
        if (deltaH > allowedDelta && deltaH > deltaV && valLeft<valRight) // right rotation required
            {
           // Serial.println("Attempting right rotation");
            rotateAzimuth('R'); // I do not need a string / double quotes but just a char
            }
  
        // Vertical rotation is performed if:
        // 1) deltaV is above the threshold
        // 2) deltaH is less than deltaV (otherwise I should correct deltaV first!)
        
        if (deltaV > allowedDelta && deltaV > deltaH && valUp>valDown) // up rotation required
            {
          //  Serial.println("Attempting up rotation");
            rotateZenith('U'); // I do not need a string / double quotes but just a char
            }
        if (deltaV > allowedDelta && deltaV > deltaH && valUp<valDown) // down rotation required
            {
         //   Serial.println("Attempting down rotation");
            rotateZenith('D'); // I do not need a string / double quotes but just a char
            }
        
}

void fullCycle(){
  /*
  On first activation I want to perform a full test swing 
  both horizontal and vertical.
  */
  play_note(6); // buzz!
  play_note(6); // buzz!
  play_note(6); // buzz!
       
  // eleven increments up, so that I am sure 
  // to engage the higher limit
    for (int increm = 1; increm < 12; increm++) { 
     //   Serial.println("Attempting up rotation");
        rotateZenith('U'); 
    }
  // twentyone increments down, so that I am sure 
  // to engage the lower limit
    for (int increm = 1; increm < 22; increm++) { 
     //   Serial.println("Attempting down rotation");
        rotateZenith('D'); 
    }
  // back to starting position
    for (int increm = 1; increm < 11; increm++) { 
     //   Serial.println("Attempting up rotation");
        rotateZenith('U'); 
    }
  // eleven increments left, so that I am sure 
  // to engage the left limit
    for (int increm = 1; increm < 22; increm++) { 
     //   Serial.println("Attempting left rotation");
        rotateAzimuth('L');
    }
  // twentyone increments right, so that I am sure 
  // to engage the right limit
    for (int increm = 1; increm < 42; increm++) { 
    //    Serial.println("Attempting right rotation");
        rotateAzimuth('R');
    }
  // back to starting position
    for (int increm = 1; increm < 21; increm++) { 
     //   Serial.println("Attempting left rotation");
        rotateAzimuth('L');
    } 
}


void setup()  
{  
  /* Pin operation mode setup */
  pinMode(photoResLeft,INPUT);  // Analog input of the photoresistor values 
  pinMode(photoResRight,INPUT);  
  pinMode(photoResUp,INPUT);  
  pinMode(photoResDown,INPUT);  
  pinMode(ledRight,OUTPUT);   // Digital output for turning LED's on 
  pinMode(ledLeft,OUTPUT);   // now there is only one LED for left + right so this line is redundant
  pinMode(ledUp,OUTPUT); 
  pinMode(ledDown,OUTPUT);   // now there is only one LED for up + down so this line is redundant
  pinMode(switchPin, INPUT);  // set pin 13 to input
  pinMode(SLEEP, OUTPUT); // set pin 12 to output
   
  stepper1.setMaxSpeed(1000.0);
  stepper1.setAcceleration(100.0);
  stepper1.setSpeed(1000);

  stepper2.setMaxSpeed(1000.0);
  stepper2.setAcceleration(100.0);
  stepper2.setSpeed(1000);  
  
 /* Initialize serial communications */
 Serial.begin(9600); // Initialize serial communications  
 
 
  /* Initialize turret to neutral staring position */
  /* I enforce a rotation limit based on increment 
     number. The turret is manually set to neutral 
     position, it suffices to declare that the 
     current position at setup is the neutral 
     starting position on both axes */
  currHorHeading = neutralHorHeading;
  currVerHeading = neutralVerHeading;
}  


// Reminder: void loop() cannot be omitted even if
// you put all the code in operate_turret()
// you would need a void function called loop:

void loop(){
  
currentButton = debounce(lastButton);

// I use a button switch to enter a While loop 
// inside the main loop() 

  if (lastButton == LOW && currentButton == HIGH)
  {
    work = !work;  //work is boolean variable for switch on/off
    play_tune_1(); // play a melody every time the switch is pressed
  }
  lastButton = currentButton;
  digitalWrite(SLEEP, work);   //set SLEEP pin to value of work variable


  while(work == HIGH){
    // I must check the button also inside the while loop..
    // otherwise it will be endless  
    currentButton = debounce(lastButton);
    if (lastButton == LOW && currentButton == HIGH)
    {
      work = !work;  //work is boolean variable for switch on/off
      play_tune_2(); // play a melody every time the switch is pressed
    }
    lastButton = currentButton;
    digitalWrite(SLEEP, work);   //set SLEEP pin to value of work variable

    Serial.println("Execution enabled - switch is ON");
    
    /*
    if (firstTimeEver == HIGH)
    {
      firstTimeEver = !firstTimeEver;  //used just once to call fullCycle()
      fullCycle();
    }
    */
    
    operate_turret();
    }
   Serial.println("Execution disabled - Manually Center the turret to neutral position, and then press the switch on the breadboard");
}