Power Sunflower

#include <Stepper.h>
#include <limits.h> 
#define fotoresistenza4 A9  
#define fotoresistenza3 A8  
#define fotoresistenza2 A5  
#define fotoresistenza1 A4 
#define dolong(x)     ((x)>=0?(long)((x)+0.5):(long)((x)-0.5))
#define dolongint(x) (int)dolong(x)


int PinDirezione_1 = 3;            
int PinStepper_1 = 2;        //definisce stepper 1 e 2 
int PinDirezione_2 = 7;
int PinStepper_2 = 6;
int _step = 0;               // valore iniziale per le funzioni che guidano motori passo-passo
int counter_steps_1 = 0;
int counter_steps_2 = 0;
float LR;
float UD;



float km1 = 0.65;
float km2 = 0.45;

int SUM;
int AV;
int FP1;
int FP2;
int FP3;
int FP4;
signed counter_diff_passi;
int num_steps_1 ; 
int num_steps_2 ;            //per efettuare una rivoluzione, num_ steps = 1600 
boolean dirOrario = true;       //rotazione verso dx
boolean dirAntiOrario = false; // rotazione verso sx

void setup()
{
  Serial.begin(9600); //Comunicazione seriale al computer
 pinMode(A4,INPUT);
 pinMode(A5,INPUT); 
 pinMode(A8,INPUT);
 pinMode(A9,INPUT);        
 pinMode(PinDirezione_1, OUTPUT);  //definisce la firezioen nella quale lo stepper 1 ruota 
 pinMode(PinStepper_1, OUTPUT);
 pinMode(PinDirezione_2, OUTPUT);  //definisce la direzione nella quale lo stepper 2 ruota 
 pinMode(PinStepper_2, OUTPUT);
}

 float diff_1(int FP1, int FP2) {
  float res1;
  res1 = FP1 - FP2;
  return res1;
}     
int sum(int FP1, int FP2, int FP3, int FP4) {
  int res3;
  res3 = FP1 + FP2 + FP3 + FP4;
  return res3;
}

  int avarage(int SUM) {
  int res4;
  res4 = SUM/4;
  return res4;
}
                                            //queste due funzioni(diff_1 e diff_2) calcolano la differenza 
                                            //tra le intensit delle luci sull'asse orizontale 
                                            //e su quello verticale dei due fotoresistori
float diff_2(int FP3, int FP4) { 
  float res2;
  res2 = FP3 + 100  - FP4;
  return res2;
}

int steps (float X, float k) {
   int passi;
    if( X < 0.0){
      passi = dolongint ( - k * X);
        }else{ 
         passi = dolongint ( k * X);
        }
         return passi;
       } 
//
// int steps_1(float X) {
//   int passi_1;
//    if( X < 0.0){
//      passi_1 = dolongint ( -km1 * X);
//        }else{ 
//         passi_1 = dolongint (km1 * X);
//        }
//         return passi_1;
//       } 
//  
//  
//  int steps_2(float Y ) {
//    int passi_2;
//     if(Y < 0.0){
//       passi_2 = dolongint ( -km2 * Y);
//       }else{ 
//       passi_2 = dolongint ( km2 * Y);
//         }
//         return passi_2;
//       } 
//     
 
 
void loop()
{  
  /*digitalWrite(PinStepper_1, LOW);
  digitalWrite(PinStepper_2, LOW);*/
  
  int FP1 = (analogRead(fotoresistenza1));
  int FP2 = (analogRead(fotoresistenza2));
  int FP3 = (analogRead(fotoresistenza3));
  int FP4 = (analogRead(fotoresistenza4));

Serial.print ("Valore luminosit_0=");
Serial.println(FP1); 
//delay(100);
Serial.print ("Valore luminosit_1=");
Serial.println(FP2);// Scrivo il valore della fotoresistenza
//delay(100);
Serial.print ("Valore luminosit_2=");
Serial.println(FP3); 
//delay(100);
Serial.print ("Valore luminosit_3=");
Serial.println(FP4);//
delay(100);


  
  /*int FP1 = 100;                 // test con valori settati da me 
  int FP2 = 0;
  int FP3 = 0;
  int FP4 = 0;*/
  
  LR = diff_1(FP1, FP2);
  Serial.print ("Valore_LR=");
  Serial.println(LR);
  //delay(1000);
  UD = diff_2(FP3, FP4);
  Serial.print ("Valore_UD=");
  Serial.println(UD);
  SUM = sum(FP1, FP2, FP3, FP3);
  AV = avarage(SUM);
  Serial.print ("Valore_AV=");
  Serial.println(AV);
  
  num_steps_1 = steps(LR, km1);
  
 //num_steps_1 = steps_1(LR);
  Serial.print ("Numero passi motore 1 =");
  Serial.println(num_steps_1);
   
  num_steps_2 = steps(UD, km2);
   //num_steps_2 = steps_2(UD);
  Serial.print ("Numero passi motore 2 =");
  Serial.println(num_steps_2);
  //delay(10000);
 
  
  if (AV > 0 && AV < 1023)
  {
    
    findStability();
        // delay(500);
  }
  else
  {
       //delay(900000);
    delay(100);
  }

}

void findStability() {
if (LR <= -35) {
  
  counter_steps_1 += num_steps_1;
  
  if (counter_steps_1 <=810){
    if (UD > -45 && UD < 45)
      step(false, true, num_steps_1, 0); 
    else if (UD >= 45)
      step(false, false, num_steps_1, num_steps_2);
    else if (UD <= -45)
      step(false, true, num_steps_1, num_steps_2);
  }
  else{
    counter_steps_1 = -805;
    step(true, true, 1610, 0);
  }
} 

else if (LR >= 35) {
    
    counter_steps_1 -= num_steps_1;
    
    if (counter_steps_1 >=-810){
      if (UD > -45 && UD < 45)
        step(true, true, num_steps_1, 0); 
      else if (UD >= 45)
        step(true, false, num_steps_1, num_steps_2);
      else if (UD <= -45)
        step(true, true, num_steps_1, num_steps_2);
   }
 else{
    counter_steps_1 = 805;
    step(false, true, 1610, 0);
 }
} 
   
else if (LR > -35 && LR < 35){
     if (UD > -45 && UD < 45)
      step(true, true, 0, 0); 
    else if (UD >= 45)
      step(true, false, 0, num_steps_2);
    else if (UD <= -45)
      step(true, true, 0, num_steps_2);
     }
}


void step(boolean dir1, boolean dir2,int steps1, int steps2){
  digitalWrite(PinDirezione_1,dir1);
  digitalWrite(PinDirezione_2,dir2);
  delay(100);              // codice implementato da noi 
  
  int max_steps = 0;
  int max_PinStepper = 0;
  
  int min_steps = 0;
  int min_PinStepper = 0;
  
  if (steps1 >= steps2) {
    max_steps = steps1;
    max_PinStepper = PinStepper_1;
    
    min_steps = steps2;
    min_PinStepper = PinStepper_2;
   } else {
     max_steps = steps2;
     max_PinStepper = PinStepper_2;
     
     min_steps = steps1;
     min_PinStepper = PinStepper_1;
   }
  
  for(int i=0; i < max_steps; i++){
    digitalWrite(max_PinStepper, HIGH);
    
    if (i < min_steps)
     digitalWrite(min_PinStepper, HIGH);
     
     delayMicroseconds(3500);
     
     digitalWrite(max_PinStepper,LOW);
     
    if (i < min_steps)
    digitalWrite(min_PinStepper,LOW);
    
    delayMicroseconds(3500); 
    
  }
}