Programs/code

#include "TimerOne.h" //package to implement internal timer interrupt

// Initialize pins

int encoder_input_A_pin = 3;       // Digital input pin

int encoder_input_B_pin = 2;       // Digital input pin

// Variables for motor and encoder operation

int direction_pin = 7;

int pwm_pin = 6;

int motor_direction = 0;

int pwm_val = 0;

// Variables for detecting encoder pulse and counting

long int count = 0;

int input_encoder_A_prev = 0;  

int input_encoder_A_now = 0;

// Variables for timing

unsigned long int prev_time = 0;

unsigned long int time_diff = 0;

unsigned long int curr_time = 0;

float encoder_pulse_frequency = 0;

int time_duration=0; // (milisecond) time for sampling delay

float n_output = 400; //counts per rev of output

unsigned long int desired_rpm = 1000;

//velocity tracker variable

float delta_t = 0.01; //sec

float prev_rev = 0;

float curr_rev = 0;

float prev_rps = 0;

float curr_rps = 0;

float curr_rpm =0;

float tau = 0.001; //sec

//averageing velocity to display variables

unsigned int sample_size = 30;

float rpm_history[30];

int rpm_index = 0;

float avg_rpm = 0;

float rpm_history_sum = 0;

void setup() {

  // put your setup code here, to run once:

  Serial.begin(115200);

  pinMode(encoder_input_A_pin, INPUT);      // sets the digital pin as input

  pinMode(encoder_input_B_pin, INPUT);      // sets the digital pin as input

  pinMode(direction_pin, OUTPUT);       // Set the direction pin output

  pinMode(pwm_pin, OUTPUT);       // Set the pwm pin output

  attachInterrupt(digitalPinToInterrupt(encoder_input_A_pin),encoderA,CHANGE); //hardware interrupt for encoder sensor

  attachInterrupt(digitalPinToInterrupt(encoder_input_B_pin),encoderB,CHANGE); //hardware interrupt for encoder sensor

  //Internal Timer Interrupt

  Timer1.initialize(delta_t * 1000000); //input in microsec

  Timer1.attachInterrupt(get_rpm);

  digitalWrite(direction_pin, motor_direction);      // Set the motor in motion

}

void loop() {

  // put your main code here, to run repeatedly:

  if (Serial.available() > 0) {

    // read the incoming byte:

    desired_rpm = Serial.parseInt(); // Serial.parseInt is blocking, but we are not really dealing with time sensitive information

    while(Serial.available() > 0) { // flush the serial buffer after reading the integer

    Serial.read();

    }

  }

  while(desired_rpm >= curr_rpm){//bang-bang control

    if(pwm_val != 255){

      pwm_val++;

    }

    else{

      Serial.print("Target RPM:");

      Serial.print(desired_rpm);

      Serial.println("======================================");

      Serial.print("Current PWM: ");

      Serial.print(pwm_val);

      Serial.print("---------");

      Serial.print("Current RPM: ");

      Serial.print(curr_rpm);

      Serial.print("--------- Average Past 30 RPM Values:");

      average_rpm_for_display(); //call function to average velocity for display only

      delay(100);

      break;

    }

    analogWrite(pwm_pin, pwm_val);

    Serial.print("Target RPM:");

    Serial.print(desired_rpm);

    Serial.println("======================================");

    Serial.print("Current PWM: ");

    Serial.print(pwm_val);

    Serial.print("---------");

    Serial.print("Current RPM: ");

    Serial.print(curr_rpm);

    Serial.print("--------- Average Past 30 RPM Values:");

    average_rpm_for_display(); //call function to average velocity for display only

    delay(100);

  }

  while(desired_rpm < curr_rpm){//bang-bang control

    if(pwm_val != 0){

      pwm_val--;

    }

    analogWrite(pwm_pin, pwm_val);

    Serial.print("Target RPM:");

    Serial.print(desired_rpm);

    Serial.println("======================================");

    Serial.print("Current PWM: ");

    Serial.print(pwm_val);

    Serial.print("---------");

    Serial.print("Current RPM: ");

    Serial.print(curr_rpm);

    Serial.print("--------- Average Past 30 RPM Values:");

    average_rpm_for_display(); //call function to average velocity for display only

    delay(100);

  }

}

float average_rpm_for_display(){ //function to average velocity for display only. this function does not affect velocity used for controlling speed

  if(rpm_index<=sample_size-1){

    rpm_history[rpm_index] = curr_rpm;

  }

  else{

    rpm_index = 0;

    rpm_history[rpm_index] = curr_rpm;

  }

    rpm_history_sum = 0;

  for(int i = 0; i<=sample_size-1; i++){

    rpm_history_sum = rpm_history_sum + rpm_history[i];

  }

  avg_rpm = rpm_history_sum/sample_size;

  Serial.println(avg_rpm);

  rpm_index++;

}

void get_rpm(){ //function used to get instataneous rpm evertime an internal timer interrupt gets triggered

  prev_rev = curr_rev;

  curr_rev = count/n_output;

  prev_rps = curr_rps;

  curr_rps = (curr_rev - prev_rev + tau*prev_rps) / (tau + delta_t); //rps

  curr_rpm = curr_rps * 60;

  //Serial.println(curr_rpm);

}

void encoderA() { //encoder logic function

  // look for a low-to-high on channel A

  if (digitalRead(encoder_input_A_pin) == HIGH) {

    // check channel B to see which way encoder is turning

    if (digitalRead(encoder_input_B_pin) == LOW) {

      count = count + 1;         // CW

    }

    else {

      count = count - 1;         // CCW

    }

  }

  else   // must be a high-to-low on channel A

  {

    // check channel B to see which way encoder is turning

    if (digitalRead(encoder_input_B_pin) == HIGH) {

      count = count + 1;          // CW

    }

    else {

      count = count - 1;          // CCW

    }

  }

}

void encoderB() { //encoder logic function

  // look for a low-to-high on channel B

  if (digitalRead(encoder_input_B_pin) == HIGH) {

    // check channel A to see which way encoder is turning

    if (digitalRead(encoder_input_A_pin) == HIGH) {

      count = count + 1;         // CW

    }

    else {

      count = count - 1;         // CCW

    }

  }

  // Look for a high-to-low on channel B

  else {

    // check channel B to see which way encoder is turning

    if (digitalRead(encoder_input_A_pin) == LOW) {

      count = count + 1;          // CW

    }

    else {

      count = count - 1;          // CCW

    }

  }

}

%% Clear command window and workspace variables

clc;                                           

clear;

%% Parameter section (Modify to see the effect in the system's dynamics)

%Pendulum Parameters

l = .06;                 % Pendulum Length m

g = 9.8;                % Gravity m^2/s

m = 1*.005;                  % Mass %kg

gamma = 0.1;            %Friction coefficient

omega0 = sqrt(g/l);     %Natural frequency

%Vibration Parameters

a = 1*0.02;               %Amplitude (m)

omega = 6e1;            %Frequency

(a*omega^2)/10

fprintf("\n\n==== You want a/l<<1." + ...

           " Current value: a/l = %f ====\n\n", a/l);

fprintf("==== You want omega0/omega <<1." + ...

               " Current value: omega0/omega = %f ====\n\n", ...

                                                           omega0/omega);

%% Simulation

t0 = 0;                 %Simulation Initial Time

tf = 4;               %Simulation Final Time

tspan = [t0 tf];

x0 = [0.95*pi; 0];        %Initial Condition for simulation

                          %NOTE: To see convergence to theta=pi

                          %       pick x0(1) close to pi and small

                          %       initial velocity x0(2)

%Instantiate vector field

F = @(t, x) FKapitza(t, x, m, l, g, gamma, a, omega);

[timeDomain, xSol] = ode15s(F, tspan, x0);

%% Plot Evolution of Simulation

figure(1)

% Plot the evolution of variable x1 = theta

pos = plot(timeDomain, xSol(:, 1), LineWidth=3, ...

                   DisplayName='$\theta(t)$')

yline(pi, LineStyle="--", LineWidth=3, Color="black", ...

                   DisplayName='$\theta=\pi$')

%title(['Amplitude: ',num2str(a),' meters  Frequency: ',num2str(omega),' rad/s  Mass: ',num2str(m),' kg  Length: ',num2str(l),' meters'])

xlabel("Time [s]", "Interpreter","latex", ...

           fontsize=20)

ylabel("Angular Position (Rad)", "Interpreter","latex", ...

           fontsize=20)

%grid("on")

hl = legend('show', Location='southwest', fontsize=20);

set(hl, 'Interpreter','latex')

figure(2)

plot(timeDomain, xSol(:, 2), LineWidth=3, ...

                   DisplayName='$\theta(t)$')

xlabel("Time [s]", "Interpreter","latex", ...

           fontsize=20)

ylabel("Angular Velocity(Rad/s)", "Interpreter","latex", ...

           fontsize=20)

title('Angular Velocity')

figure(3)

asol= diff(a*xSol(:,2))/diff(timeDomain);

plot(timeDomain(1:end-1),asol)

xlabel("Time [s]", "Interpreter","latex", ...

           fontsize=20)

ylabel("Accel(m^2/s)", "Interpreter","latex", ...

           fontsize=20)

%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%

% Vector Field Declaration:

%

%   Implements the vector field of the

%   equation of motion of Kapitza's pendulum

%

%            Do Not Modify

%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function dotx = FKapitza(t, x, m, l, g, gamma, a, omega)

   %State selection

   x1 = x(1); %Angular Position: theta

   x2 = x(2); %Angular velocity: dottheta

   %Calculate Oscillations

   dotmu = a*omega*cos(omega*t);

   ddotmu = -a*omega^2*sin(omega*t);

   %Dot x

   dotx1 = x2;

   dotx2 = -(g/l)*sin(x1)...

               -(1/l)*sin(x1)*ddotmu...

                   - gamma/(l*m)*(l*x2 + sin(x1)*dotmu);

    %Return value for the function

   dotx = [dotx1; dotx2];

end

#include <Wire.h>

#include <Servo.h>

// initialize target rpm

int target_rpm = 900; // declaring target_rpm as integer and initializing

// controller to achieve target rpm

double error; // declaring error as double

double gain; // declaring gain as double  

// declaring ESC

Servo ESC;

double motor_input = 0; //declaring motor_input as integer and initializing to 0

// declaring myservo

Servo myservo;

int pos = 6/1.5; //declaring pos as integer and initializing to 0

// reading rpm

const int escRpmPin = 8;  // the RPM feedback signal is connected to digital pin 8

const int numPoles = 14;  // the number of magnetic poles in the motor

unsigned long pwmDuration = 0;  // the duration of the PWM signal in microseconds

unsigned long rpm = 0;  // the calculated RPM value

// declaring counter

int i = 0; // counter for reading rpm

void setup() {

Serial.begin(9600); // defining baud rate

// ESC set up

ESC.attach(9,1000,2000); // attach ESC to pin 9, 1000 min pulse width & 2000 max pulse width

ESC.write(motor_input);  // setting ESC to initial value

// myservo set up

myservo.attach(10); // attach myservo to pin 11

myservo.write(pos); // setting servo to initial value

// rpm set up

pinMode(escRpmPin, INPUT); // configure the RPM feedback pin as input

delay(1000);

}

void rpmdelay(int delay_time)

{

 for (i=0; i<=delay_time; i+=1000) // reading rpm at one second interval

 {

pwmDuration = pulseIn(escRpmPin, HIGH);  // read the duration of the PWM signal

rpm = 60000000 / (pwmDuration * numPoles);  // calculate the RPM value

Serial.print("RPM: ");

Serial.println(rpm);  // print the RPM value to the serial monitor

if (delay_time <1000)

{

  delay(delay_time);

}

  else

{

  delay(1000);// wait for a short time before taking the next measurement

}

}

}

void rpmcontrol(int target_rpm, int rpm)

{

  error = (target_rpm - rpm);

  while (abs(error) > 10)

  {

    gain = 0.01;

    motor_input = motor_input + error*gain;

    ESC.write(int(motor_input));

    pwmDuration = pulseIn(escRpmPin, HIGH);  // read the duration of the PWM signal

    rpm = 60000000 / (pwmDuration * numPoles);  // calculate the RPM value

    error = (target_rpm - rpm);

    Serial.print("target_rpm: ");

    Serial.println(target_rpm);

    Serial.print("error: ");

    Serial.println(error);

    Serial.print("motor_input: ");

    Serial.println(motor_input);

    Serial.print("rpm: ");

    Serial.println(rpm);

  }

  delay(1000);

}

void loop()

{

// ramping up motor to desired RPM

for (motor_input = 0; motor_input <= 35; motor_input += 1)

{

  delay(1);

  ESC.write(motor_input);

  Serial.println(motor_input);

}

rpmdelay(1000); // read rpm during delay

rpmcontrol(target_rpm, rpm); // adjusting motor_input to match target_input

rpmdelay(1000);

// set servo angles

myservo.write(60/1.5);  // 45 deg

rpmdelay(3000);

myservo.write(105/1.5); // 90 deg

rpmdelay(3000);

myservo.write(140/1.5); // 135 deg

rpmdelay(3000);

myservo.write(201/1.5); // 180 deg

rpmdelay(10000); // motor still running

for (motor_input = 35; motor_input > 0; motor_input -= 1)

{

ESC.write(int(motor_input));

Serial.println(motor_input);

delay(100);

}

ESC.write(0);

rpmdelay(1000);

myservo.write(5/1.5);

rpmdelay(5000);

}