ServoBehaviours.h

#ifndef SERVO_BEHAVIOUR_h
#define SERVO_BEHAVIOUR_h
#include "Arduino.h"
#include "Behaviours.h"

#include <Servo.h>

class ServoGoto : public Behaviour {
  Servo _servo;

public:
  ServoGoto(Servo servo, String name = "goto") :  Behaviour(name), _servo(servo){ }
  //ServoMove(Servo servo, String name) : Behaviour(name), _servo(servo) {}

  char* args() {return "<int angle>"; };
  String start(String args) {
    Serial.println("Goto: '"+args+"'");
    int val = args.toInt();
    _servo.write(val);
    return "";
  }

};

class ServoWiggle : public Behaviour {
  Servo _servo;
  int _start_time = 0;
  int _wiggle_time = 300;
  int _num_wiggles = 5;
  int _wiggles = 0;
  int _wiggle_angle = 0;
  //Calculate wiggle time by multiplying the angle by this...
  int _wiggle_factor = 5;

public:
  ServoWiggle(Servo servo, String name, int slowness=3) : Behaviour(name), _servo(servo),_wiggle_factor(slowness) {}
  char* args() {return "<int wiggle_angle>"; };
  String start(String args) {
    _wiggle_angle = args.toInt();
    _wiggles = 0;
    _running = true;
    _wiggle_time = _wiggle_factor * _wiggle_angle;
    return "Wiggling " + String(_num_wiggles) + " times";
  }

  void update() {
    int time_since = millis() - _start_time;
    if( time_since > _wiggle_time ) {
      _wiggles++;
      _start_time = millis();
      int angle = ( _wiggles % 2 ) ? (90+_wiggle_angle) : (90-_wiggle_angle);
      if( _wiggles > _num_wiggles ) {
        angle = 90;
        _running = false;
      }
      Serial.println("Wiggling to: " + String(angle));
      _servo.write(angle);
    }
  }

};

#endif