Over My Head

I'm in over my head! i'm pretty new to Arduino and understand some of the basics but I can not seem to get this to work. I have some pre written code for a specific project but I'm trying to add some additional functionality. To give you a idea of what this code does,basically it controls a servo left or right based on signal strength to follow a transmitter to keep a strong signal link. IE,if right antenna has stronger signal then move right,If left antenna is stronger-move left.

What i am trying to add is an additional servo to control tilt (Up and Down) manually via two push buttons(one up and one down)

This code uses an additional library from here Use Version 1.3

/*
  Simple RSSI Antenna Tracker



RELATIVE
*/

#include <Servo.h>
#include <Timer.h>

/* 
 *  For testing different curves
 *  All new curves are more precise
 *  and maintain lock better than old RELATIVE
 *  
 *  Uncomment one 
 */
//#define EXPONENTIAL       // OK
//#define RELATIVE          // old, don't use
#define SIGMOID             // Best
//#define PROPORTIONAL      // twitchy

#define SIGMOID_SLOPE       1
#define SIGMOID_OFFSET      4

#if !defined(EXPONENTIAL) ^ !defined(SIGMOID) ^ !defined(PROPORTIONAL) ^ !defined(RELATIVE)
// all good
#else
  #error "Please define ONE tracking curve: EXPONENTIAL, SIGMOID, PROPORTIONAL, RELATIVE"
#endif


/*
   Pin mapping
   You can change these pins, just make sure that
   RSSI pins are analog inputs
   and servo pin supports PWM
*/
#define LEFT_RSSI_PIN       0     // analog RSSI measurement pin
#define RIGHT_RSSI_PIN      1
#define PAN_SERVO_PIN       5     // Pan servo pin

/*
   There is going to be some difference in receivers,
   one is going to be less sensitive than the other.
   It will result in slight offset when tracker is centered on target

   Find value that evens out RSSI
 */
#define RSSI_OFFSET_RIGHT   0
#define RSSI_OFFSET_LEFT    0

/*
   MIN and MAX RSSI values corresponding to 0% and 100% from receiver
   See serial monitor
*/
#define RSSI_MAX            400
#define RSSI_MIN            120

/*
   Safety limits for servo

   Find values where servo doesn't buzz at full deflection
*/
#define SERVO_MAX           180
#define SERVO_MIN           0

/*
 * Servo 'speed' limits
 * MAX and MIN step in degrees
 */
#define SERVO_MAX_STEP      5
#define SERVO_MIN_STEP      0.09     // prevents windup and servo crawl

/*
   Center deadband value!
   Prevents tracker from oscillating when target is in the center
   When using SIGMOID or EXPONENTIAL you can set this almost to 0
*/
#define DEADBAND            5

/*
   Depending which way around you put your servo
   you may have to change direction

   either 1 or -1
*/
#define SERVO_DIRECTION     1

#define FIR_SIZE            10
#define LAST                FIR_SIZE - 1


uint16_t rssi_left_array[FIR_SIZE];
uint16_t rssi_right_array[FIR_SIZE];

float anglePan = 90;
boolean debug = false;

Timer timer;
Servo servoPan;


void setup() {
  servoPan.attach(PAN_SERVO_PIN);
  servoPan.write(90);

  // wipe array
  for (int i = 0; i < FIR_SIZE; i++) {
    rssi_right_array[i] = 0;
    rssi_left_array[i] = 0;
  }

  Serial.begin(115200);
  while (!debug) {
    delay(3000);
    debug = true;
  }


  timer.every(50, mainLoop);
  timer.every(5, measureRSSI);
}


void loop() {

  timer.update();

}


void mainLoop() {

  uint16_t avgLeft = max(avg(rssi_left_array, FIR_SIZE) + RSSI_OFFSET_LEFT, RSSI_MIN);
  uint16_t avgRight = max(avg(rssi_right_array, FIR_SIZE) + RSSI_OFFSET_RIGHT, RSSI_MIN);

//  If avg RSSI is above 90%, don't move
//  if ((avgRight + avgLeft) / 2 > 360) {
//    return;
//  }

  /*
     the lower total RSSI is, the lower deadband gets
     allows more precise tracking when target is far away
  */
  uint8_t dynamicDeadband = (float(avgRight + avgLeft) / 2 - RSSI_MIN) / (RSSI_MAX - RSSI_MIN) * DEADBAND;

  // if target is in the middle, don't move
  if (abs(avgRight - avgLeft) < dynamicDeadband ) {
    return;
  }

  float ang = 0;

  // move towards stronger signal
  if (avgRight > avgLeft) {
  
  #if defined(EXPONENTIAL)
    float x = float(avgRight - avgLeft);
    x = x * x / 500;
    ang = x * SERVO_DIRECTION * -1;
  #endif

  #if defined(RELATIVE)
    ang = float(avgRight / avgLeft) * (SERVO_DIRECTION * -1);
  #endif

  #if defined(SIGMOID)
    float x = float(avgRight - avgLeft) / 10;
    x = SERVO_MAX_STEP / (1+ exp(-SIGMOID_SLOPE * x + SIGMOID_OFFSET));
    ang = x * SERVO_DIRECTION * -1;
  #endif
    
  #if defined(PROPORTIONAL)
    float x = float(avgRight - avgLeft) / 10;
    ang = x * SERVO_DIRECTION * -1;  
  #endif
  }
  else {

  #if defined(EXPONENTIAL)
    float x = float(avgLeft - avgRight);
    x = x * x / 500;
    ang = x * SERVO_DIRECTION;
  #endif

  #if defined(RELATIVE)
    ang = float(avgLeft / avgRight) * SERVO_DIRECTION;
  #endif

  #if defined(SIGMOID)
    float x = float(avgLeft - avgRight) / 10;
    x = SERVO_MAX_STEP / (1+ exp(-SIGMOID_SLOPE * x + SIGMOID_OFFSET));
    ang = x * SERVO_DIRECTION;
  #endif
    
  #if defined(PROPORTIONAL)
    float x = float(avgLeft - avgRight) / 10;
    ang = x * SERVO_DIRECTION;  
  #endif
  }

  // upper and lower limit for angle step
  ang = (abs(ang) > SERVO_MAX_STEP ? SERVO_MAX_STEP * ang/abs(ang) : ang);
  ang = (abs(ang) < SERVO_MIN_STEP ? 0 : ang);

  // move servo by n degrees
  movePanBy(ang);


  if (debug) {
//    Serial.print("RSSI%: ");
//    Serial.print(map(avgLeft, RSSI_MIN, RSSI_MAX, 0, 100));
//    Serial.print(", ");
//    Serial.print(map(avgRight, RSSI_MIN, RSSI_MAX, 0, 100));

    // raw rssi values, use these for RSSI_MIN and RSSI_MAX
    Serial.print("Calibration - left: ");
    Serial.print(avgLeft);
    Serial.print(" right: ");
    Serial.print(avgRight);

    Serial.print(" servo-angle: ");
    Serial.println(anglePan);
  }
}

void movePanBy(float angle) {

  anglePan += angle;
  anglePan = limit(SERVO_MIN, SERVO_MAX, anglePan);
  servoPan.write(anglePan);
}

void measureRSSI() {

  advanceArray(rssi_left_array, FIR_SIZE);
  advanceArray(rssi_right_array, FIR_SIZE);

  rssi_left_array[LAST] = analogRead(LEFT_RSSI_PIN);
  rssi_right_array[LAST] = analogRead(RIGHT_RSSI_PIN);
}

uint16_t avg(uint16_t samples[], uint8_t n) {

  uint32_t summ = 0;
  for (uint8_t i = 0; i < n; i++) {
    summ += samples[i];
  }

  return uint16_t(summ / n);
}

void advanceArray(uint16_t *samples, uint8_t n) {

  for (uint8_t i = 0; i < n - 1; i++) {
    samples[i] = samples[i + 1];
  }
}


float limit(float lowerLimit, float upperLimit, float var) {

  return min(max(var, lowerLimit), upperLimit);
}

I'm trying to add the functionality of this code

   /*
 Controlling a servo with two Push buttons with Arduino
 when Left  push button is pressed, the servo start moving to the left until it reaches 180( or zero) degrees
 when Right  push button is pressed, the servo start moving to the right until it reaches 180( or zero) degrees
At any instance if the button is released, servo stops

 May 22, 2018 at 01:00 
 Written by Ahmad S. for Robojax.com at Ajax, Ontario, Canada
 Watch video for this code at https://youtu.be/7woqNH_qby4
 This code is taken from http://robojax.com/learn/arduino
*/

#include <Servo.h>

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

int angle =90;    // initial angle  for servo
int angleStep =5;

#define LEFT 12   // pin 12 is connected to left button
#define RIGHT  2  // pin 2 is connected to right button

void setup() {
  // Servo button demo by Robojax.com
  Serial.begin(9600);          //  setup serial
  myservo.attach(9);  // attaches the servo on pin 9 to the servo object
  pinMode(LEFT,INPUT_PULLUP); // assign pin 12 ass input for Left button
  pinMode(RIGHT,INPUT_PULLUP);// assing pin 2 as input for right button
  myservo.write(angle);// send servo to the middle at 90 degrees
 Serial.println("Robojax Servo Button ");
}

void loop() {
  // Servo button demo by Robojax.com
  while(digitalRead(RIGHT) == LOW){

    if (angle > 0 && angle <= 180) {
      angle = angle - angleStep;
       if(angle < 0){
        angle = 0;
       }else{
      myservo.write(angle); // move the servo to desired angle
      Serial.print("Moved to: ");
      Serial.print(angle);   // print the angle
      Serial.println(" degree");
       }
    }
    
  delay(100); // waits for the servo to get there
  }// while
 // Servo button demo by Robojax.com

  while(digitalRead(LEFT) == LOW){

    // Servo button demo by Robojax.com
    if (angle >= 0 && angle <= 180) {
      angle = angle + angleStep;
      if(angle >180){
        angle =180;
       }else{
      myservo.write(angle); // move the servo to desired angle
      Serial.print("Moved to: ");
      Serial.print(angle);   // print the angle
      Serial.println(" degree");
       }
    }
    
  delay(100); // waits for the servo to get there
  }// 

  
}

Please post your best attempt at the combined code and tell us in detail what it actually does and what you want it to do that is different. It will make it much easier to focus on the parts you need help with rather than wasting time on things that you can do.

...R
Simple Merge Demo
Planning and Implementing a Program

This is basically a mash up of both codes, i know it's probably ugly but honestly i don't know what I'm doing.

I'm getting a "'mainLoop' was not declared in this scope"

/*
  Simple RSSI Antenna Tracker



RELATIVE
*/

#include <Servo.h>
#include <Timer.h>

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

int angle =90;    // initial angle  for servo
int angleStep =5;
//TILT SERVO


/* 
 *  For testing different curves
 *  All new curves are more precise
 *  and maintain lock better than old RELATIVE
 *  
 *  Uncomment one 
 */
//#define EXPONENTIAL       // OK
//#define RELATIVE          // old, don't use
#define SIGMOID             // Best
//#define PROPORTIONAL      // twitchy

#define SIGMOID_SLOPE       1
#define SIGMOID_OFFSET      4

#if !defined(EXPONENTIAL) ^ !defined(SIGMOID) ^ !defined(PROPORTIONAL) ^ !defined(RELATIVE)
// all good
#else
  #error "Please define ONE tracking curve: EXPONENTIAL, SIGMOID, PROPORTIONAL, RELATIVE"
#endif


/*
   Pin mapping
   You can change these pins, just make sure that
   RSSI pins are analog inputs
   and servo pin supports PWM
*/
#define LEFT_RSSI_PIN       0     // analog RSSI measurement pin
#define RIGHT_RSSI_PIN      1
#define PAN_SERVO_PIN       5     // Pan servo pin

/*
   There is going to be some difference in receivers,
   one is going to be less sensitive than the other.
   It will result in slight offset when tracker is centered on target

   Find value that evens out RSSI
 */
#define RSSI_OFFSET_RIGHT   0
#define RSSI_OFFSET_LEFT    0

/*
   MIN and MAX RSSI values corresponding to 0% and 100% from receiver
   See serial monitor
*/
#define RSSI_MAX            400
#define RSSI_MIN            120

/*
   Safety limits for servo

   Find values where servo doesn't buzz at full deflection
*/
#define SERVO_MAX           180
#define SERVO_MIN           0

/*
 * Servo 'speed' limits
 * MAX and MIN step in degrees
 */
#define SERVO_MAX_STEP      5
#define SERVO_MIN_STEP      0.09     // prevents windup and servo crawl

/*
   Center deadband value!
   Prevents tracker from oscillating when target is in the center
   When using SIGMOID or EXPONENTIAL you can set this almost to 0
*/
#define DEADBAND            5

/*
   Depending which way around you put your servo
   you may have to change direction

   either 1 or -1
*/
#define SERVO_DIRECTION     1

#define FIR_SIZE            10
#define LAST                FIR_SIZE - 1


uint16_t rssi_left_array[FIR_SIZE];
uint16_t rssi_right_array[FIR_SIZE];

float anglePan = 90;
boolean debug = false;

Timer timer;
Servo servoPan;


void setup() {
  servoPan.attach(PAN_SERVO_PIN);
  servoPan.write(90);

  // wipe array
  for (int i = 0; i < FIR_SIZE; i++) {
    rssi_right_array[i] = 0;
    rssi_left_array[i] = 0;
  }

  Serial.begin(9600);
  while (!debug) {
    delay(3000);
    debug = true;
  }


  timer.every(50, mainLoop);
  timer.every(5, measureRSSI);
}

//TILT SERVO
  // Servo button demo by Robojax.com
  Serial.begin(9600);          //  setup serial
  myservo.attach(9);  // attaches the servo on pin 9 to the servo object
  pinMode(LEFT,INPUT_PULLUP); // assign pin 12 ass input for Left button
  pinMode(RIGHT,INPUT_PULLUP);// assing pin 2 as input for right button
  myservo.write(angle);// send servo to the middle at 90 degrees
 Serial.println("Robojax Servo Button ");
}
  //TILT SERVO
void loop() {

  timer.update();

}

//TILT SERVO
 // Servo button demo by Robojax.com
  while(digitalRead(RIGHT) == LOW){

    if (angle > 0 && angle <= 180) {
      angle = angle - angleStep;
       if(angle < 0){
        angle = 0;
       }else{
      myservo.write(angle); // move the servo to desired angle
      Serial.print("Moved to: ");
      Serial.print(angle);   // print the angle
      Serial.println(" degree");
       }
    }
    
  delay(100); // waits for the servo to get there
  }// while
 // Servo button demo by Robojax.com

  while(digitalRead(LEFT) == LOW){

    // Servo button demo by Robojax.com
    if (angle >= 0 && angle <= 180) {
      angle = angle + angleStep;
      if(angle >180){
        angle =180;
       }else{
      myservo.write(angle); // move the servo to desired angle
      Serial.print("Moved to: ");
      Serial.print(angle);   // print the angle
      Serial.println(" degree");
       }
    }
    
  delay(100); // waits for the servo to get there
  }// 

  
}
  //TILT SERVO

void mainLoop() {

  uint16_t avgLeft = max(avg(rssi_left_array, FIR_SIZE) + RSSI_OFFSET_LEFT, RSSI_MIN);
  uint16_t avgRight = max(avg(rssi_right_array, FIR_SIZE) + RSSI_OFFSET_RIGHT, RSSI_MIN);

//  If avg RSSI is above 90%, don't move
//  if ((avgRight + avgLeft) / 2 > 360) {
//    return;
//  }

  /*
     the lower total RSSI is, the lower deadband gets
     allows more precise tracking when target is far away
  */
  uint8_t dynamicDeadband = (float(avgRight + avgLeft) / 2 - RSSI_MIN) / (RSSI_MAX - RSSI_MIN) * DEADBAND;

  // if target is in the middle, don't move
  if (abs(avgRight - avgLeft) < dynamicDeadband ) {
    return;
  }

  float ang = 0;

  // move towards stronger signal
  if (avgRight > avgLeft) {
  
  #if defined(EXPONENTIAL)
    float x = float(avgRight - avgLeft);
    x = x * x / 500;
    ang = x * SERVO_DIRECTION * -1;
  #endif

  #if defined(RELATIVE)
    ang = float(avgRight / avgLeft) * (SERVO_DIRECTION * -1);
  #endif

  #if defined(SIGMOID)
    float x = float(avgRight - avgLeft) / 10;
    x = SERVO_MAX_STEP / (1+ exp(-SIGMOID_SLOPE * x + SIGMOID_OFFSET));
    ang = x * SERVO_DIRECTION * -1;
  #endif
    
  #if defined(PROPORTIONAL)
    float x = float(avgRight - avgLeft) / 10;
    ang = x * SERVO_DIRECTION * -1;  
  #endif
  }
  else {

  #if defined(EXPONENTIAL)
    float x = float(avgLeft - avgRight);
    x = x * x / 500;
    ang = x * SERVO_DIRECTION;
  #endif

  #if defined(RELATIVE)
    ang = float(avgLeft / avgRight) * SERVO_DIRECTION;
  #endif

  #if defined(SIGMOID)
    float x = float(avgLeft - avgRight) / 10;
    x = SERVO_MAX_STEP / (1+ exp(-SIGMOID_SLOPE * x + SIGMOID_OFFSET));
    ang = x * SERVO_DIRECTION;
  #endif
    
  #if defined(PROPORTIONAL)
    float x = float(avgLeft - avgRight) / 10;
    ang = x * SERVO_DIRECTION;  
  #endif
  }

  // upper and lower limit for angle step
  ang = (abs(ang) > SERVO_MAX_STEP ? SERVO_MAX_STEP * ang/abs(ang) : ang);
  ang = (abs(ang) < SERVO_MIN_STEP ? 0 : ang);

  // move servo by n degrees
  movePanBy(ang);


  if (debug) {
//    Serial.print("RSSI%: ");
//    Serial.print(map(avgLeft, RSSI_MIN, RSSI_MAX, 0, 100));
//    Serial.print(", ");
//    Serial.print(map(avgRight, RSSI_MIN, RSSI_MAX, 0, 100));

    // raw rssi values, use these for RSSI_MIN and RSSI_MAX
    Serial.print("Calibration - left: ");
    Serial.print(avgLeft);
    Serial.print(" right: ");
    Serial.print(avgRight);

    Serial.print(" servo-angle: ");
    Serial.println(anglePan);
  }
}

void movePanBy(float angle) {

  anglePan += angle;
  anglePan = limit(SERVO_MIN, SERVO_MAX, anglePan);
  servoPan.write(anglePan);
}

void measureRSSI() {

  advanceArray(rssi_left_array, FIR_SIZE);
  advanceArray(rssi_right_array, FIR_SIZE);

  rssi_left_array[LAST] = analogRead(LEFT_RSSI_PIN);
  rssi_right_array[LAST] = analogRead(RIGHT_RSSI_PIN);
}

uint16_t avg(uint16_t samples[], uint8_t n) {

  uint32_t summ = 0;
  for (uint8_t i = 0; i < n; i++) {
    summ += samples[i];
  }

  return uint16_t(summ / n);
}

void advanceArray(uint16_t *samples, uint8_t n) {

  for (uint8_t i = 0; i < n - 1; i++) {
    samples[i] = samples[i + 1];
  }
}


float limit(float lowerLimit, float upperLimit, float var) {

  return min(max(var, lowerLimit), upperLimit);
}

RSSI_Track:130:19: error: 'mainLoop' was not declared in this scope

timer.every(50, mainLoop);

^

RSSI_Track:131:18: error: 'measureRSSI' was not declared in this scope

timer.every(5, measureRSSI);

^

C:\Users\Shop\Desktop\ArdProjo\RSSI_Track\RSSI_Track.ino: At global scope:

RSSI_Track:136:3: error: 'Serial' does not name a type

Serial.begin(9600); // setup serial

^

RSSI_Track:137:3: error: 'myservo' does not name a type

myservo.attach(9); // attaches the servo on pin 9 to the servo object

^

RSSI_Track:138:10: error: expected constructor, destructor, or type conversion before '(' token

pinMode(LEFT,INPUT_PULLUP); // assign pin 12 ass input for Left button

^

RSSI_Track:139:10: error: expected constructor, destructor, or type conversion before '(' token

pinMode(RIGHT,INPUT_PULLUP);// assing pin 2 as input for right button

^

RSSI_Track:140:3: error: 'myservo' does not name a type

myservo.write(angle);// send servo to the middle at 90 degrees

^

RSSI_Track:141:2: error: 'Serial' does not name a type

Serial.println("Robojax Servo Button ");

^

RSSI_Track:142:1: error: expected declaration before '}' token

}

^

exit status 1
'mainLoop' was not declared in this scope

You have a whole bunch of code that isn't in any function right after the setup function ends. Also, later on there's the same thing going on here:

//TILT SERVO
// Servo button demo by Robojax.com
while (digitalRead(RIGHT) == LOW)
....

All that stuff needs to be in a function.

Assuming you have tested program #2 by itself and it works, I propose that you first simplify it by making a function that moves the servo like this:

// move servo while button at pin is pressed
void moveServo(int pin, int direction)
{

  while(digitalRead(pin) == LOW){

    // Servo button demo by Robojax.com
    if (angle >= 0 && angle <= 180) {
      angle = angle + (direction * angleStep);
      if(angle >180){
        angle =180;
      }
      if(angle < 0 ) {
        angle = 0;
      }
      else{
        myservo.write(angle); // move the servo to desired angle
        Serial.print("Moved to: ");
        Serial.print(angle);   // print the angle
        Serial.println(" degree");
      }
    }

    delay(100); // waits for the servo to get there
  }//

}

I think that properly replicates the operation of the two blocks of code in loop(). Replace the original contents of loop() with this

void loop() {
  // Servo button demo by Robojax.com
  if (digitalRead(RIGHT) == LOW){

    moveServo(RIGHT, -1);

  } 
  else if (digitalRead(LEFT) == LOW){

    moveServo(LEFT, 1);
  } 

}

and test it. If that works it should make it a bit easier to incorporate into program #1.

I would proceed by adding everything above setup() in #2 to above setup in #1, testing, adding everything form setup() in #2 to setup() in #1 and testing again.

If it is still working I think you could stop panning while tilting by putting this in loo()

  if (digitalRead(RIGHT) == LOW){

    moveServo(RIGHT, -1);

  } 
  else if (digitalRead(LEFT) == LOW){

    moveServo(LEFT, 1);

  } 
  else {

    timer.update();

  }

There may be things I have not thought of, but that would be my strategy.

Both codes work independently,it's when they're combined when the issues pop up. I'll give this a try,thanks

I got the code to compile without errors, looks like it was simply some formating/layout mistakes on my part. I won't be able to test it for a few days but this is the first successful compile. Thanks for the help guys. If i run into any other issues I'll update.

/*
  Simple RSSI Antenna Tracker



RELATIVE
*/

#include <Servo.h>
#include <Timer.h>

/* 
 *  For testing different curves
 *  All new curves are more precise
 *  and maintain lock better than old RELATIVE
 *  
 *  Uncomment one 
 */
//#define EXPONENTIAL       // OK
//#define RELATIVE          // old, don't use
#define SIGMOID             // Best
//#define PROPORTIONAL      // twitchy

#define SIGMOID_SLOPE       1
#define SIGMOID_OFFSET      4

#if !defined(EXPONENTIAL) ^ !defined(SIGMOID) ^ !defined(PROPORTIONAL) ^ !defined(RELATIVE)
// all good
#else
  #error "Please define ONE tracking curve: EXPONENTIAL, SIGMOID, PROPORTIONAL, RELATIVE"
#endif


/*
   Pin mapping
   You can change these pins, just make sure that
   RSSI pins are analog inputs
   and servo pin supports PWM
*/
#define LEFT_RSSI_PIN       0     // analog RSSI measurement pin
#define RIGHT_RSSI_PIN      1
#define PAN_SERVO_PIN       5     // Pan servo pin

/*
   There is going to be some difference in receivers,
   one is going to be less sensitive than the other.
   It will result in slight offset when tracker is centered on target

   Find value that evens out RSSI
 */
#define RSSI_OFFSET_RIGHT   0
#define RSSI_OFFSET_LEFT    0

/*
   MIN and MAX RSSI values corresponding to 0% and 100% from receiver
   See serial monitor
*/
#define RSSI_MAX            400
#define RSSI_MIN            120

/*
   Safety limits for servo

   Find values where servo doesn't buzz at full deflection
*/
#define SERVO_MAX           180
#define SERVO_MIN           0

/*
 * Servo 'speed' limits
 * MAX and MIN step in degrees
 */
#define SERVO_MAX_STEP      5
#define SERVO_MIN_STEP      0.09     // prevents windup and servo crawl

/*
   Center deadband value!
   Prevents tracker from oscillating when target is in the center
   When using SIGMOID or EXPONENTIAL you can set this almost to 0
*/
#define DEADBAND            5

/*
   Depending which way around you put your servo
   you may have to change direction

   either 1 or -1
*/
#define SERVO_DIRECTION     1

#define FIR_SIZE            10
#define LAST                FIR_SIZE - 1


uint16_t rssi_left_array[FIR_SIZE];
uint16_t rssi_right_array[FIR_SIZE];

float anglePan = 90;
boolean debug = false;

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

int angle =90;    // initial angle  for servo
int angleStep =5;

#define LEFT 12   // pin 12 is connected to left button
#define RIGHT  2  // pin 2 is connected to right button
//TILT SERVO

Timer timer;
Servo servoPan;


void setup() {
  //TILT SERVO
myservo.attach(9);  // attaches the servo on pin 9 to the servo object
  pinMode(LEFT,INPUT_PULLUP); // assign pin 12 ass input for Left button
  pinMode(RIGHT,INPUT_PULLUP);// assing pin 2 as input for right button
  myservo.write(angle);// send servo to the middle at 90 degrees
  //TILT SERVO
  
  servoPan.attach(PAN_SERVO_PIN);
  servoPan.write(90);

  // wipe array
  for (int i = 0; i < FIR_SIZE; i++) {
    rssi_right_array[i] = 0;
    rssi_left_array[i] = 0;
  }

  Serial.begin(115200);
  while (!debug) {
    delay(3000);
    debug = true;
    Serial.println("ChimpTracker");
  }


  timer.every(50, mainLoop);
  timer.every(5, measureRSSI);
}



void loop() {
  //TILT SERVO
   while(digitalRead(RIGHT) == LOW){

    if (angle > 0 && angle <= 180) {
      angle = angle - angleStep;
       if(angle < 0){
        angle = 0;
       }else{
      myservo.write(angle); // move the servo to desired angle
      Serial.print("Moved to: ");
      Serial.print(angle);   // print the angle
      Serial.println(" degree");
       }
    }
    
  delay(100); // waits for the servo to get there
  }// while
 // Servo button demo by Robojax.com

  while(digitalRead(LEFT) == LOW){

    // Servo button demo by Robojax.com
    if (angle >= 0 && angle <= 180) {
      angle = angle + angleStep;
      if(angle >180){
        angle =180;
       }else{
      myservo.write(angle); // move the servo to desired angle
      Serial.print("Moved to: ");
      Serial.print(angle);   // print the angle
      Serial.println(" degree");
       }
    }
    
  delay(100); // waits for the servo to get there
  }// 
//TILT SERVO

  timer.update();

}


void mainLoop() {

  uint16_t avgLeft = max(avg(rssi_left_array, FIR_SIZE) + RSSI_OFFSET_LEFT, RSSI_MIN);
  uint16_t avgRight = max(avg(rssi_right_array, FIR_SIZE) + RSSI_OFFSET_RIGHT, RSSI_MIN);

//  If avg RSSI is above 90%, don't move
//  if ((avgRight + avgLeft) / 2 > 360) {
//    return;
//  }

  /*
     the lower total RSSI is, the lower deadband gets
     allows more precise tracking when target is far away
  */
  uint8_t dynamicDeadband = (float(avgRight + avgLeft) / 2 - RSSI_MIN) / (RSSI_MAX - RSSI_MIN) * DEADBAND;

  // if target is in the middle, don't move
  if (abs(avgRight - avgLeft) < dynamicDeadband ) {
    return;
  }

  float ang = 0;

  // move towards stronger signal
  if (avgRight > avgLeft) {
  
  #if defined(EXPONENTIAL)
    float x = float(avgRight - avgLeft);
    x = x * x / 500;
    ang = x * SERVO_DIRECTION * -1;
  #endif

  #if defined(RELATIVE)
    ang = float(avgRight / avgLeft) * (SERVO_DIRECTION * -1);
  #endif

  #if defined(SIGMOID)
    float x = float(avgRight - avgLeft) / 10;
    x = SERVO_MAX_STEP / (1+ exp(-SIGMOID_SLOPE * x + SIGMOID_OFFSET));
    ang = x * SERVO_DIRECTION * -1;
  #endif
    
  #if defined(PROPORTIONAL)
    float x = float(avgRight - avgLeft) / 10;
    ang = x * SERVO_DIRECTION * -1;  
  #endif
  }
  else {

  #if defined(EXPONENTIAL)
    float x = float(avgLeft - avgRight);
    x = x * x / 500;
    ang = x * SERVO_DIRECTION;
  #endif

  #if defined(RELATIVE)
    ang = float(avgLeft / avgRight) * SERVO_DIRECTION;
  #endif

  #if defined(SIGMOID)
    float x = float(avgLeft - avgRight) / 10;
    x = SERVO_MAX_STEP / (1+ exp(-SIGMOID_SLOPE * x + SIGMOID_OFFSET));
    ang = x * SERVO_DIRECTION;
  #endif
    
  #if defined(PROPORTIONAL)
    float x = float(avgLeft - avgRight) / 10;
    ang = x * SERVO_DIRECTION;  
  #endif
  }

  // upper and lower limit for angle step
  ang = (abs(ang) > SERVO_MAX_STEP ? SERVO_MAX_STEP * ang/abs(ang) : ang);
  ang = (abs(ang) < SERVO_MIN_STEP ? 0 : ang);

  // move servo by n degrees
  movePanBy(ang);


  if (debug) {
//    Serial.print("RSSI%: ");
//    Serial.print(map(avgLeft, RSSI_MIN, RSSI_MAX, 0, 100));
//    Serial.print(", ");
//    Serial.print(map(avgRight, RSSI_MIN, RSSI_MAX, 0, 100));

    // raw rssi values, use these for RSSI_MIN and RSSI_MAX
    Serial.print("Calibration - left: ");
    Serial.print(avgLeft);
    Serial.print(" right: ");
    Serial.print(avgRight);

    Serial.print(" servo-angle: ");
    Serial.println(anglePan);
  }
}

void movePanBy(float angle) {

  anglePan += angle;
  anglePan = limit(SERVO_MIN, SERVO_MAX, anglePan);
  servoPan.write(anglePan);
}

void measureRSSI() {

  advanceArray(rssi_left_array, FIR_SIZE);
  advanceArray(rssi_right_array, FIR_SIZE);

  rssi_left_array[LAST] = analogRead(LEFT_RSSI_PIN);
  rssi_right_array[LAST] = analogRead(RIGHT_RSSI_PIN);
}

uint16_t avg(uint16_t samples[], uint8_t n) {

  uint32_t summ = 0;
  for (uint8_t i = 0; i < n; i++) {
    summ += samples[i];
  }

  return uint16_t(summ / n);
}

void advanceArray(uint16_t *samples, uint8_t n) {

  for (uint8_t i = 0; i < n - 1; i++) {
    samples[i] = samples[i + 1];
  }
}


float limit(float lowerLimit, float upperLimit, float var) {

  return min(max(var, lowerLimit), upperLimit);
}