Arduino Code Wave Power Experiment

#include "Servo.h"
#include <math.h>

#define SQUARE 0
#define SINE 1
#define TRIANGLE 2

// Usage Guide
//
// Enter commands through serial. A command is formed by the identifier, followed by arguments, and finally a period.
//
// List of commands
//
// w [Min angle] [Max angle] [Wave type] [Period, multiples of 20ms]
// Rotates the servo at a set interval
// Example: "w 70 100 0 15."
// Rotates servo between 70 and 100 degrees, using a square wave, and period is 15*20 = 300ms
//
// rc [period]
// rv [period]
// collects current or voltage data, respectively
// Example: "rc 25."
// Collects current data with intervals of 25*20 = 500ms
//

// Pin definitions. You can redefine
#define SERVO_PIN 5
#define READ_PIN 1
#define MUX_PIN 6

#define DATA_POINTS 100

Servo servo;

void setup() {
  Serial.begin(9600);
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(MUX_PIN, OUTPUT);
  digitalWrite(MUX_PIN, false);
  servo.attach(SERVO_PIN);
}

int period = 1000;
int data[DATA_POINTS] = {0};
float low = 45.0;
float high = 135.0;
int mode = SQUARE;
int tick = 0;
bool led_st = false;
String input_mode;
String input_rp;
String input_h;
String input_l;
String input_t;
String input_p;
int index = -1;
int rp = 10;
void loop() {
  if (Serial.available() > 0) {
    input_mode = Serial.readStringUntil(' ');
    if (input_mode == "w") {
      input_h = Serial.readStringUntil(' ');
      input_l = Serial.readStringUntil(' ');
      input_t = Serial.readStringUntil(' ');
      input_p = Serial.readStringUntil('.');
      high = atof(input_h.c_str());
      low = atof(input_l.c_str());
      mode = atoi(input_t.c_str());
      period = atoi(input_p.c_str());
    } else if (input_mode == "rv") {
      input_rp = Serial.readStringUntil('.');
      rp = atoi(input_rp.c_str());
      index = 0;
    } else if (input_mode == "rc") {
      input_rp = Serial.readStringUntil('.');
      rp = atoi(input_rp.c_str());
      index = 0;
      digitalWrite(MUX_PIN, true);
    }
  }
  digitalWrite(LED_BUILTIN, led_st);
  if (tick%period == 0 || tick % period == period/2)
    led_st = !led_st;
  if (tick%rp == 0) {
    if (index >= 0) {
      data[index] = analogRead(READ_PIN);
      index++;
      if (index == DATA_POINTS) {
        index = -1;
        digitalWrite(MUX_PIN, false);
        Serial.write("START: ");
        for (int i = 0; i < DATA_POINTS; i++) {
          Serial.print(data[i]);
          Serial.write(", ");
        }
        Serial.write("END");
      }
    }
  }
  float angle = 90.0;
  switch(mode) {
    case SQUARE:
      angle = ((tick%period < period/2)? low : high);
      break;
    case SINE:
      angle = (high-low)*(sin(2.0 * PI * (float)tick / (float)period) + 1.0) / 2.0 + low;
      break;
    case TRIANGLE:
      angle = (high-low)*(2.0*fabs((float)(tick%period) - period/2.0)) / period + low;
      break;
  }
  servo.write(angle);
  tick++;
  delay(20);
}