Summary

When you plug this little robot in, all he wants to do is unplug himself and go back to sleep. Unfortunately for him, the cable is so far away and he only has a stubby little servo arm. So, he has to strategically boost a nearby pendulum (like pushing a swing) until it knocks over the release lever which unplugs his power cable.

Dynamics

To achieve this dynamical boosting, the robot must keep track of where the pendulum is in order to push it at just the right time. Unfortunately, this little guy doesn’t have any sensors; so, he must rely on an in-depth knowledge of dynamics to estimate where the pendulum is at any given point in time based on where, when, and for how long he pushed it based on the following principles:
Pratfall Dynamics

Outcome

Unfortunately for the robot, his pushes were just not strong enough to overcome the frictional losses in the system; so, the pendulum couldn’t maintain any momentum after he let go; so, he needed a bit of a helping hand to get unplugged.

Video

Code

#include <Servo.h>
#define P_SERV 11 // MOSI on ICSP header
Servo S1;

#define sgnf(x) ( (x) ? (x) / abs(x) : 0) // Basic sign function

// Constants (SI units):
#define MU_A  18e-6f    // [Pa s] Dynamic Viscosity of Air at 20C, 1atm
#define grav  9.0867f   // [m s-2] Standard Gravitational Acceleration

#define deg2rad M_PI / 180.0f
#define rad2deg 180.0f / M_PI

// Mechanical Properties (SI units):
#define M     0.04275f  // [kg] Mass of Pendulum Arm
#define L     0.12460f  // [m] Total Length of Pendulum Arm
#define LCM   0.06387f  // [m] Distance to Center of Mass of Pendulum Arm from Center of Rotation

#define TSM   0.1765f   // [N-m] Standard Torque Output of Servo Motor at 5V.
#define LH    0.02f     // [m] Distance to Point where Servo Horn Contacts the Pendulum from the Servo's Center of Rotation

// Settings:
#define TH_PUSH   (1.3f * deg2rad)  // [rad] Magnitude of Angle of Pendulum at Which Servo should begin a Push.
#define TH_INIT   (8.97f * deg2rad) // [rad] Angle at which Pendulum Starts.

#define THS_INIT  80      // [deg] Angle at which Servo Starts.
#define THS_REST  90      // [deg] Angular Distance of Resting Spot of Servo Horn Relative from 90 degrees (vertical) - just outside of the swing of the pendulum arm.

#define KD        24.0f   // Tunable Linear Drag Coefficient
#define KF        0.0f    // Tunable Constant Acceleration Losses due to Frictional Moments in Pendulum Bearing

// Pre-Computed Properties:
float inv_I = 1.0f / M / LCM / LCM;               // Inverse Moment of Inertia of Pendulum Arm
float alpha_app = TSM * L * inv_I / LH;           // Angular Acceleration Experience By Level
float k_drag = KD * MU_A * L*L*L * inv_I / 6.0f;  // Contant Parameters in Drag Calculation

float k_acc = grav / LCM; // Contant Parameters in Standard Acceleration

// State Variables:
int pushing = 0; // Whether the Servo is Currently Pushing the Pendulum
unsigned long last_model_update = 0; // Millis Time of Previous Model Update
float alpha = 0; // [rad s-2] Current Estimated Angular Acceleration of Pendulum
float omega = 0; // [rad s-1] Current Estimated Angular Velocity of Pendulum
float theta = 0; // [rad] Current Estimated Angle of the Pendulum

/****
 * Perform an update on the model of the pendulum factoring in whether or not the servo is currently pushing the pendulum (and in what direction).
 * pushing = 1 -> pushing pendulum CCW
 * pushing = -1 -> pushing pendulum CW
 * pushing = 0 -> not pushing pendulum
 *
 * Returns: Current Estimated Position of Pendulum Arm
**/
void updateModel(){
  // Basic Mid-Point Algorithm
  unsigned long t = millis();
  float dt = (t - last_model_update) / 1000.0f;
  last_model_update = t; // Set immediately

  omega += alpha * dt / 2;
  theta += omega * dt / 2;

  alpha = - k_acc * sinf(theta);
          - k_drag * omega
          - KF * sgnf(omega)
          + pushing ? pushing * alpha_app : 0;

  omega += alpha * dt / 2;
  theta += omega * dt / 2;
}

void setup(){
  Serial.begin(115200);
  S1.attach(P_SERV);

  // Get to Staring Position and Sit:
  S1.write(90 - THS_INIT);
  delay(1000);

  // Release Pendulum and Set Initial Conditions for Model
  S1.write(90 - THS_REST);
  theta = TH_INIT;
  omega = 0.0f; // Starts from Rest
  last_model_update = millis();
} // #setup

void loop(){
  static unsigned long last_model_printout = 0; // Time of Previous Serial Model Update
  // Check Model to Determine if Push is Needed (/before/ updating model):
  if(abs(theta) < TH_PUSH){ pushing = -sgnf(theta); // Push by running servo to other resting position as fast as possible S1.write(90 + sgnf(theta)*THS_REST); } if(pushing && abs(theta) > TH_INIT){
    pushing = 0;
  }

  updateModel();

  if(millis() - last_model_printout > 50){ // Plot points infrequently so as to not continuously disturb model.
    Serial.print(10*pushing);Serial.print(","); Serial.println(theta * rad2deg);
    last_model_printout = millis();
  }
  delay(2);
} // #loop

Design Files

CAD