/**********************************************
   e-Gizmo PBot Uno Controller - SERVO_SWEEP.ino

   This is for SERVO sample sketch
   to sweeps the shaft back and forth
   across 180 degrees.

   SERVO Control
   ex. PBot.SERVO(whichSERVO,pulsewidth);
   where:
   whichservo = 1 to 4 ,ignore other values
   pulsewidth = 0 to 180 (degrees)
   - value less than 500 stops the SERVO PWM generator
   - the pulsewidth converted to degrees from 0 to 180 (default)

   Codes by
   e-Gizmo Mechatronix Central
   http://www.e-gizmo.com
   March 17,2020
 ************************************************/
#include "eGizmo_PBot_Uno.h"
#include <Wire.h>

eGizmo_PBot_Uno PBot; // Create PBot object to control a SERVO
int MinPos = 10;
int MaxPos = 170;
int RunPos = 1;
int delaymsec = 15;
int Pos = 0; //servo start position

void setup() {

  PBot.BEGIN();
}
void loop() {

  for (Pos = MinPos; Pos < MaxPos; Pos += RunPos) { // goes from 00 degrees to 170 degrees
    // in steps of 1 degree
    PBot.SERVO(1, Pos);               // tell servo to go to position in variable 'pos'
    PBot.SERVO(2, Pos);
    PBot.SERVO(3, Pos);
    PBot.SERVO(4, Pos);
    delay(delaymsec);
    // waits 15ms for the servo to reach the position
  }
  for (Pos = MaxPos; Pos >= MinPos; Pos -= RunPos) { // goes from 170 degrees to 10 degrees

    PBot.SERVO(1, Pos);               // tell servo to go to position in variable 'pos'
    PBot.SERVO(2, Pos);
    PBot.SERVO(3, Pos);
    PBot.SERVO(4, Pos);
    delay(delaymsec);                       // waits 15ms for the servo to reach the position
  }

}