MPPT Vehicle

// Servo library used to make panel tilt algorithm
//this code wil track the maximum input power of the sun for a given angle
//by way of Perturb and Observe (loosely based on the popular P&O algorithm)



#include <Servo.h> 
 
Servo myservo;                                //create servo object to control a servo 
                                              // a maximum of eight servo objects can be created 
 
int pos = 0;                                  // variable to store the servo position 
float Pdiff = 0;
double R = 2000000;                               //resistance in ohms of the current sampling resistor (2Mohm)
int pos_start = 45;                           //dynamic value for the starting position of the servo controlling the panel angle

void setup() 
{ 
  Serial.begin(115200);                        //intiialize serial communication for printing values//debug basically
  myservo.attach(9);                         // attaches the servo on pin 9 to the servo object 
  
} 

 
void loop() 
{ 
  //Initalize measurements
  float Vmeas = analogRead(2);                  //positive side panel voltage on pin2
  float Vmeas_I = analogRead(5);                //negative side panel voltage reading (across 2.2 ohm resistor) on pin 5
  
  float V_1 = 0;                             //might convert to int in the interest of saving memory
  float I_1 = 0;
  float P_1 = 0;
  float V_2 = 0;
  float I_2 = 0;
  float P_2 = 0;
  float Vc = (3.3/1023);                     //voltage correction factor for the ADC (note: may be more than 10 bit ADC on 432)
  //=========================================================================================================================
  //Track in the positive direction....
  //=========================================================================================================================
  for(pos = pos_start; pos < 135; pos += 1)  // goes from  a min of 45 degrees to  max of 180 degrees //in steps of one degree
  {  
    
    Vmeas = analogRead(2);                  //positive side panel voltage on pin2
    Vmeas_I = analogRead(5);                //negative side panel voltage reading (across 2.2 ohm resistor) on pin 5
    
    V_1 = 2*Vmeas*Vc;                          //measure voltage at past time stamp
    I_1 = ((Vmeas-Vmeas_I)*Vc)/R;            //measure current at past time stamp
    P_1 = V_1*I_1;                           //calculate power 
    myservo.write(pos);                      // tell servo to go to position in variable 'pos'// Perterb!!
    Serial.print("\n\nP_1\n");
    Serial.println(P_1);                     //optional, only to view the voltages we are getting
    delay(1000);                              // waits 50ms for the servo to reach the position 
    
    Vmeas = analogRead(2);                  //positive side panel voltage on pin2
    Vmeas_I = analogRead(5);                //negative side panel voltage reading (across 2.2 ohm resistor) on pin 5
    
    V_2 = 2*Vc*Vmeas;                             //Repeat all measurements again (Observe!!)
    I_2 = ((Vmeas-Vmeas_I)*Vc)/R;
    P_2 = V_2*I_2;
    Serial.print("\n\nP_2\n");
 
    Pdiff = P_2 - P_1;
    if (Pdiff < 0)                             //if the power is dropping, go the other direction, if it is not dropping keep climbing the hill!
      {
        //Serial.println(Pdiff);            //debug line
        pos_start = pos;                     //update the start position so that the controller does not move when entering the next loop
        break;                               //break into the next for loop
      }
    
  } 
  pos_start = pos;
  
  //=============================================================================================================================
  //Track in the negative direction.... Constantly Updatting and tracking the maximum powerpoint
  //=============================================================================================================================
  for(pos = pos_start; pos >= 45; pos -= 1)  // goes from  a max of 180 degrees to  min of 45 degrees //in steps of one degree
  {  
    
    Vmeas = analogRead(2);                  //positive side panel voltage on pin2
    Vmeas_I = analogRead(5);                //negative side panel voltage reading (across 2.2 ohm resistor) on pin 5
    
    V_1 = 2*Vc*Vmeas;                             //measure voltage at past time stamp
    I_1 = (Vmeas-Vmeas_I)/R;                 //measure current at past time stamp
    P_1 = V_1*I_1;                           //calculate power 
    myservo.write(pos);                      // tell servo to go to position in variable 'pos'// (Perterb!!)
    
    //Serial.println(-(V_1));                  //debug line
    
    delay(1000);                              // waits 50ms for the servo to reach the position 
    Vmeas = analogRead(2);                  //positive side panel voltage on pin2
    Vmeas_I = analogRead(5);                //negative side panel voltage reading (across 2 Mohm resistor) on pin 5
    
    
    V_2 = 2*Vc*Vmeas;                             //Repeat all measurements again (Observe!!)
    I_2 = (Vmeas-Vmeas_I)/R;
    P_2 = V_2*I_2;
    Pdiff = P_2 - P_1;
    //Serial.println(-(V_2));                  //debug line
    if (Pdiff< 0)                             //if the power is dropping, go the other direction, if it is not dropping keep climbing the hill!
      {
        pos_start = pos;                     //update the start position so that the controller does not move when entering the next loop
        break;                               //break into the next for loop
      }
  
  } 
  //=============================================================================================================================
  //Enter the postive direction loop again....
  //==============================================================================================================================
pos_start = pos;
//Serial.print("Loop Switch"); //debug line
}