#include <Servo.h>
#include <Wire.h>
#include <Adafruit_ADS1015.h>

Servo myservo;  // create servo object to control a servo

Adafruit_ADS1115 ads1115; // construct an ads1115 at default address 0x48
const float multiplier = 0.1875F;

int pos = 0;    // variable to store the servo position

void setup() {
  Serial.begin(9600); // Set baud rate for serial monitor 
  
  myservo.attach(9);  // The servo is attached to digital pin 9

  ads1115.begin();    // Initialize ads1115
  // ads1115.setGain(GAIN_TWOTHIRDS);
  ads1115.setGain(GAIN_ONE);
}

void loop() {

  // ++++++++++++ Create array of angles ++++++++++++++++++++
  int N_array = 50;
  int th_array[N_array];
    
  for(int ii = 0; ii < N_array; ++ii) {
    th_array[ii] = (int)((float)ii)/((float) N_array)*180.0;
  }

  // ++++++++++++ Loop on angles and read values +++++++++++++++++
  // First, go to angle 0
  myservo.write(th_array[0]);
  Serial.print('\n');
  delay(500);

  // Then, loop on other angles in the array
  for(int ii = 0; ii < N_array; ++ii) {

    // Go to that angle
    myservo.write(th_array[ii]);
    delay(100);

    // Read sensors and average values
    int16_t adc0;
    float adc0_ave = 0.0;

    int NAVE = 20; // Average on this many readings
    for(int iii = 0; iii<NAVE; ++iii){

      adc0 = ads1115.readADC_Differential_0_1();
      adc0_ave += (1.0*adc0)/(1.0*NAVE); // Compute double (note: int / NAVE would round off! Multiply by 1.0 as to convert to double!)
      
    }

    // Convert adc signal to mV
    float mV = adc0_ave*4.096/32768*1000; // valid for gain 1
    
    Serial.print(mV);
    Serial.print(' ');

    delay(10);
    
  } // end loop on angles
  
  Serial.print('\n');
  
}