Team 26 - Appendix

Bill of Materials:

MATLAB code:

Main code:

clc; clear; format compact; clf; close all;

set(0,'defaultTextInterpreter','latex');
set(0, 'defaultAxesTickLabelInterpreter','latex');
set(0, 'defaultLegendInterpreter','latex');
set(0,'defaultAxesFontSize',18);
set(0, 'DefaultLineLineWidth', 2);
set(groot, 'defaultFigureUnits', 'pixels', 'defaultFigurePosition', [440   278   560   420]);

d = 46.47;
d_dot = 10;
[ta, tb, wa, wb] = mechanism1(d, d_dot);
tb = ta + 75 * pi/180;
wb = wa;
[a, ta, a_dot, wa] = mechanism2(tb, wb);


% Calculating input slider position and velocity
d = linspace(30.25, 111, 100);
pitch = 2; % mm/rev
w_motor = 200 * 2*pi / 60; % rad/s (200 RPM)
d_dot = w_motor * pitch / (2 * pi); % mm/s

tb_bank = zeros(1, length(d));
wb_bank = zeros(1, length(d));
ta_bank = zeros(1, length(d));
wa_bank = zeros(1, length(d));
for i = 1:length(d)
[ta, tb, wa, wb] = mechanism1(d(i), d_dot);
tb = ta + 75 * pi/180;
wb = wa;
[a, ta, a_dot, wa] = mechanism2(tb, wb);

    tb_bank(i) = tb;
wb_bank(i) = wb;
ta_bank(i) = ta;
wa_bank(i) = wa;
end

figure
hold on
box on
plot(d - 30, ta_bank, '-r')
plot(d - 30, tb_bank, '-b')
xlabel('Input Slider Actuation, $x$ [mm]')
ylabel('$\theta$ [rad]')
legend('Distal Finger Angle, $\theta_{a}$', 'Proximal Finger Angle, $\theta_{b}$')
legend('location', 'best')
title('Finger Angular Positions vs. Slider Input')
xlim([0, 80])

figure
hold on
box on
plot(d - 30, wa_bank, '-r')
plot(d - 30, wb_bank, '-b')
xlabel('Input Slider Actuation, $x$ [mm]')
ylabel('$\dot{\theta}$ [rad/s]')
legend('Distal Finger Velocity, $\dot{\theta_{a}}$', 'Proximal Finger Velocity, $\dot{\theta_{b}}$')
legend('location', 'best')
title('Finger Angular Velocities vs. Slider Input')
xlim([0, 80])

Function for mechanism #1:

function [ta, tb, wa, wb] = mechanism1(d, d_dot, is_symbolic)
if nargin < 3
    is_symbolic = false;
end

if is_symbolic == true
    syms a b c d ta tb tc td d_dot wa wb;

    % Position
    eqn1 = a*cos(ta) + c + b*cos(tb) == 0;
    eqn2 = a*sin(ta) - d + b*sin(tb) == 0;

    eqns = [eqn1, eqn2];
    vars = [ta, tb];

    [solv, solu] = solve(eqns, vars);

    ta = simplify(solv(2));
    tb = simplify(solu(2));

    % Velocity
    eqn1 = -a*wa*sin(ta) - b*wb*sin(tb) == 0;
    eqn2 = a*wa*cos(ta) - d_dot + b*wb*cos(tb) == 0;

    eqns = [eqn1, eqn2];
    vars = [wa, wb];

    [solv, solu] = solve(eqns, vars);

    wa = simplify(solv);
    wb = simplify(solu);

else
    syms ta tb wa wb;

    % Position
    a = 54.45;
    b = 80;
    c = 1.72;

    eqn1 = a*cos(ta) + c + b*cos(tb) == 0;
    eqn2 = a*sin(ta) - d + b*sin(tb) == 0;

    eqns = [eqn1, eqn2];
    vars = [ta, tb];

    [solv, solu] = solve(eqns, vars);

    ta_set = eval(solv);
    tb_set = eval(solu);

    ta = ta_set(2);
    tb = tb_set(2);

    % Velocity
    eqn1 = -a*wa*sin(ta) - b*wb*sin(tb) == 0;
    eqn2 = a*wa*cos(ta) - d_dot + b*wb*cos(tb) == 0;

        eqns = [eqn1, eqn2];
        vars = [wa, wb];

    [solv, solu] = solve(eqns, vars);

        wa = eval(solv);
        wb = eval(solu);
end

end

Function for mechanism #2:

function [a, ta, a_dot, wa] = mechanism2(tb, wb, is_symbolic)
if nargin < 3
    is_symbolic = false;
end

if is_symbolic == true
    syms a b c d ta tb tc td a_dot wa wb wc wd;

    % Position
    eqn1 = a*cos(ta) - b*cos(tb) + c*cos(tc) + d*sin(ta) == 0;
    eqn2 = a*sin(ta) - b*sin(tb) + c*sin(tc) - d*cos(ta) == 0;

    eqns = [eqn1, eqn2];
    vars = [a, ta];

    [solv, solu] = solve(eqns, vars);

    a = simplify(solv(1));
    b = simplify(solu(1));

    % Velocity
    eqn1 = a_dot * cos(ta) - a * wa * sin(ta) + b * wb * sin(tb) + d * wa * cos(ta);
    eqn2 = a_dot * sin(ta) + a * wa * cos(ta) - b * wb * cos(tb) + d * wa * sin(ta);

    eqns = [eqn1, eqn2];
    vars = [a_dot, wa];

    [solv, solu] = solve(eqns, vars);

    a_dot = simplify(solv);
    wa = simplify(solu);

else
    syms a ta a_dot wa;

    % Position
    b = 120;
    c = 90;
    d = 26.42;
    tc = 3*pi/4;

    eqn1 = a*cos(ta) - b*cos(tb) + c*cos(tc) + d*sin(ta) == 0;
    eqn2 = a*sin(ta) - b*sin(tb) + c*sin(tc) - d*cos(ta) == 0;

    eqns = [eqn1, eqn2];
    vars = [a, ta];

    [solv, solu] = solve(eqns, vars);

    a_set = eval(solv);
    ta_set = eval(solu);

    a = a_set(1);
    ta = ta_set(1);

    % Velocity
    eqn1 = a_dot * cos(ta) - a * wa * sin(ta) + b * wb * sin(tb) + d * wa * cos(ta);
    eqn2 = a_dot * sin(ta) + a * wa * cos(ta) - b * wb * cos(tb) + d * wa * sin(ta);

        eqns = [eqn1, eqn2];
        vars = [a_dot, wa];

    [solv, solu] = solve(eqns, vars);

        a_dot = eval(solv);
        wa = eval(solu);
end

end

Arduino code:

#include <Arduino.h>

#include <Stepper.h>

// Define FSR pin:

#define fsrpin1 A0

#define fsrpin2 A1

#define potpin1 A2

#define potpin2 A3

const int dirPin = 2;

const int stepPin = 3;

int potMax {660};

int potMin {0};

int LEDState=0;

int LEDPinBlue=7;

int LEDPinRed=8;

int buttonPin=12;

int buttonNew;

int buttonOld=1;

int dt=20;

// Define number of steps per revolution:

const int stepsPerRevolution = 200;

// Initialize the stepper library on pins 8 through 11:

Stepper myStepper = Stepper(stepsPerRevolution, 3, 4, 5, 6);

//-------------------------------------------------------------

// Settings

int target_force {12}; // N

float conversion_factor {0.02}; // N/bit(/8)

int prop_gain {1};

float tolerance_window {0.3};

float pot_tol {0.8};

int min_step_size {5};

int max_step_size {200};

int speedMax {250};

bool activeFeedback {0};

signed long currentRotation {0};

bool dir {};

//-------------------------------------------------------------

// Function prototypes

bool setControlMode ();

int adjustSpeed ();

float readForce ();

int calculateStep (float current_force);

int potentiometerControl ();

int adjustDirection ();

void stepMotor (int steps);

//-------------------------------------------------------------

void setup () {

  pinMode(LEDPinBlue, OUTPUT);

  pinMode(LEDPinRed, OUTPUT);

  pinMode(buttonPin, INPUT);

  pinMode(stepPin, OUTPUT);

    pinMode(dirPin, OUTPUT);

  Serial.begin(9600);

}

void loop () {

  // - Check button to determine whether should be using remote control or autonomous

  bool activeFeedback {setControlMode()};

  // * If closed-loop control should be active

  if (activeFeedback == true) {

    // - read in force

    float current_force {readForce()};

    // - use it to determine update in motor step

    int step_size {calculateStep(current_force)};

    // - execute update

    stepMotor(step_size);

  // * If remote control should be active

  } else {

    // - determine direction from user control

    signed int direction {adjustDirection()};

    // - execute update

    stepMotor(direction * 10);

  }

  delay(10);

}

//-------------------------------------------------------------

void stepMotor(int steps)

{

  Serial.println(steps);

  if (steps > 0) {

    digitalWrite(dirPin, HIGH);

  } else if (steps < 0) {

    digitalWrite(dirPin, LOW);

  }

  if (steps != 0) {

    for(int x = 0; x < stepsPerRevolution; x++)

    {

      digitalWrite(stepPin, LOW);

      delayMicroseconds(2000);

      digitalWrite(stepPin, HIGH);

      delayMicroseconds(2000);

    }

  }

}

int adjustSpeed () {

  int potVal = analogRead(potpin2);

  int speed = (map(potVal, potMin, potMax, 1, speedMax));

  return speed;

}

int adjustDirection () {

  int potVal = analogRead(potpin2);

  int potMed {((potMin + potMax) / 2)};

  Serial.println();

  if (potVal > potMed && potVal > (1 + pot_tol) * potMed) {

    return 1;

  } else if ((potVal < potMed && potVal < (1 - pot_tol) * potMed)) {

    return -1;

  } else {

    return 0;

  }

}

bool setControlMode ()

{

  buttonNew=digitalRead(buttonPin);

  if(buttonOld==0 && buttonNew==1){

    if (LEDState==0){

      digitalWrite(LEDPinBlue,HIGH);

      digitalWrite(LEDPinRed,LOW);

      LEDState=1;

    }

    else{

      digitalWrite(LEDPinBlue, LOW);

      digitalWrite(LEDPinRed,HIGH);

      LEDState=0;

    }

  }

  buttonOld=buttonNew;

  delay(dt);

  return LEDState;

}

float readForce ()

{

  int fsrreading1 = analogRead(fsrpin1);

  int fsrreading2 = analogRead(fsrpin2);

  float current_force = conversion_factor * ((fsrreading1 + fsrreading2) / 2);

  Serial.print("Reading = ");

  Serial.println(current_force);

  return current_force;

}

int calculateStep (float current_force)

{

  int step_size {};

  if (((current_force < (1 - tolerance_window) * target_force) && (current_force < target_force)) || ((current_force > (1 + tolerance_window) * target_force) && (current_force > target_force))) {

    // step_size = prop_gain * (target_force - current_force);

    step_size = prop_gain * ((target_force / conversion_factor) - (current_force / conversion_factor - 0)) * (max_step_size - min_step_size) / (1023 - 0);

  }

  return step_size;

}