Team 26 - Appendix
- Jared Rosenbaum (Unlicensed)
- 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;
}