#include "servo.h"

#include <Servo.h>

/* *****************************************************************************
 *    Servo functions
 * *****************************************************************************/

Servo servo1;
float servo1_angle = 0.0;
float servo1_direction = 0.0;


/* *****************************************************************************
 *  FUNCTION:       servoLoop
 *  DESCRIPTION:    to be called every 100ms
 *                  change servo1 to new position by advancing SERVO_STEP_SIZE degrees
 */
void servoLoop(int param) {
  servo1_angle += servo1_direction;
  if (servo1_direction>0) {
    if (servo1_angle>180) {
      servo1_direction = 0 - .1;
    }
  } else if (servo1_direction<0) {
    if (servo1_angle<0) {
      servo1_direction = 0 + .1;
    }
  }
  servo1.write(servo1_angle);
}

/* *****************************************************************************
 *  FUNCTION:       initServo
 */
void initServo() {
  // setup servo(s)
  servo1.attach(SERVO_1_PIN, 400, 2600); // SG90 type servo Minimum and maximum pulse width (in µs) to go from 0° to 180°.
  servo1_angle = 0;
  servo1_direction = 0+1;
  servo1.write(servo1_angle);
  addLoopScanner(10,servoLoop,0,"servoLoop");
  SERIAL_PRINTF("Servo RBPXPLR initialized\n");
}