Team 26 - Appendix

Owned by Jared Rosenbaum (Unlicensed)
Last updated: Apr 27, 2023 by Steven Swanbeck

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;

}



Confluence Documentation | Web Privacy Policy | Web Accessibility