#include <Servo.h>

/* Set these to +/- 1 for half-stepping, +/- 2 for full-stepping */
#define FORWARD    1
#define BACKWARD  -1
#define TURN       90          /* steps per rotation,
                                   depends on stepping & motor */

      /* These parameters will depend on your motor, what it's driving */
#define MAX_DELAY    255               /* determines min startup speed */
#define MIN_DELAY     10                /* determines max cruise speed */
#define ACCELERATION  16          /* lower = smoother but slower accel */

#define RAMP_STEPS    (MAX_DELAY - MIN_DELAY) / ACCELERATION

Servo servo1;
volatile uint8_t stepPhase = 0;
volatile int8_t direction = FORWARD;
volatile uint16_t stepCounter = 0;
volatile int16_t servo_pos = 0;
String takeStr;

int pos_sweep;
void servo_sweep(){
  for (pos_sweep = 0; pos_sweep <= 180; pos_sweep += 1) { // goes from 0 degrees to 180 degrees
    // in steps of 1 degree
    servo1.write(pos_sweep);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
    Serial.println(pos_sweep);
  }
  for (pos_sweep = 180; pos_sweep >= 0; pos_sweep -= 1) { // goes from 180 degrees to 0 degrees
    servo1.write(pos_sweep);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
    Serial.println(pos_sweep);
  }
}

void takeSteps(uint16_t howManySteps, uint8_t __delay) {
  servo_pos = servo_pos + (howManySteps * direction);
  servo1.write(servo_pos);  
  delay(__delay);
  takeStr = String(servo_pos)+","+String(__delay)+";";
  Serial.println(takeStr);

}

void trapezoidMove(int16_t howManySteps) {
  uint8_t __delay = MAX_DELAY;
  uint16_t stepsTaken = 0;
  Serial.println("trapezoidMove "+String(howManySteps));
                               /* set direction, make howManySteps > 0 */
  if (howManySteps > 0) {
    direction = FORWARD;
  }
  else {
    direction = BACKWARD;
    howManySteps = -howManySteps;
  }

  if (howManySteps > (RAMP_STEPS * 2)) {
                             /* Have enough steps for a full trapezoid */
                                                         /* Accelerate */
    while (stepsTaken < RAMP_STEPS) {
      takeSteps(1, __delay);
      __delay -= ACCELERATION;
      stepsTaken++;
    }
                                                             /* Cruise */
    __delay = MIN_DELAY;
    takeSteps((howManySteps - 2 * RAMP_STEPS), __delay);
    stepsTaken += (howManySteps - 2 * RAMP_STEPS);
                                                         /* Decelerate */
    while (stepsTaken < howManySteps) {
      takeSteps(1, __delay);
      __delay += ACCELERATION;
      stepsTaken++;
    }
  }
  else {
                                               /* Partial ramp up/down */
    while (stepsTaken <= howManySteps / 2) {
      takeSteps(1, __delay);
      __delay -= ACCELERATION;
      stepsTaken++;
    }
    __delay += ACCELERATION;
    while (stepsTaken < howManySteps) {
      takeSteps(1, __delay);
      __delay += ACCELERATION;
      stepsTaken++;
    }
  }
}

void trapezoidSequence(){
    /* Smooth movements, trapezoidal acceleration */
    trapezoidMove(TURN);                         /* one full turn */
    trapezoidMove(-TURN / 2);                             /* half turn */
    trapezoidMove(TURN / 4);                           /* quarter turn */
    trapezoidMove(-TURN / 8);                                /* eighth */
    _delay_ms(1000);
    trapezoidMove(-TURN / 4);                         /* the other way */
    trapezoidMove(TURN / 8);
    trapezoidMove(TURN / 2);                /* half turn back to start */
    _delay_ms(1000);
}

void setup() {
  servo1.attach(9);
  Serial.begin(9600);
}

void loop() {
  trapezoidSequence();
}