Matlab / Arduino Code

<< Results, Conclusion, and Further Work

MATLAB Code

function [ ] = retractor( )

RAD = pi/180;
DEG = 1/RAD;

a = 1.19;
b = 2.2;
c = a;
d = b;

finger = 4.4;

phi1_open = -29.17;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Angles for opening and closing

phi1 = phi1_open:(phi1_open+100);

phi1a = phi1 + 90;
phi2 = zeros(size(phi1));
phi3 = zeros(size(phi1));
phi4 = 90 * ones(size(phi1));

for i = 1:length(phi1)
    [phi2(i), phi3(i)] = four_bar2(a,b,c,d,RAD*phi1a(i),'o');
end

phi2 = DEG*(phi2) - 90;
phi3 = DEG*(phi3) - 90;

fingerX1 = finger.*cos((phi1+90)*RAD);
fingerY1 = finger.*sin((phi1+90)*RAD);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Angles for retracting and extending

theta11 = -90:phi1_open;
theta12 = phi1_open:90;
theta2 = zeros(1,(length(theta11)+length(theta12)));
theta3 = zeros(1,(length(theta11)+length(theta12)));
theta4 = phi1_open * ones(size(theta2));

theta11a = theta11 - phi1_open;
theta12a = theta12 - phi1_open;

for i = 1:length(theta11a)
    [theta2(i), theta3(i)] = four_bar2(d,a,b,c,RAD*theta11a(i),'c');
end
for i = 1:length(theta12a)
    [theta2(length(theta11a)+i), theta3(length(theta11a)+i)] =...
        four_bar2(d,a,b,c,RAD*theta12a(i),'o');
end

theta1 = [theta11,theta12];
theta2 = DEG*theta2 + phi1_open;
theta3 = DEG*theta3 + phi1_open;

fingerX2 = d.*cos((theta1)*RAD) + finger.*cos((theta2+90)*RAD);
fingerY2 = -d + d.*sin((theta1)*RAD) + finger.*sin((theta2+90)*RAD);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

fingerX = [fingerX2, fingerX1];
fingerY = [fingerY2, fingerY1];

figure
plot(fingerX,fingerY)
xlabel('Finger Tip X (in from wrist)')
ylabel('Finger Tip Y (in from wrist)')
grid on

figure
pause(5)
%Closing
for i = 1:length(phi1)
    vecplot(a,b,c,d,phi1(i),phi2(i),phi3(i)+180,phi4(i))
end
%Opening
for i = 1:length(phi1)
    vecplot(a,b,c,d,phi1(length(phi1)-i+1),...
        phi2(length(phi1)-i+1),phi3(length(phi1)-i+1 )+180,phi4(i))
end

%Retracting
for i = 1:length(theta1)
    vecplot(a,b,c,d,theta2(length(theta1)-i+1),...
        theta3(length(theta1)-i+1)+180,theta4(length(theta1)-i+1)+180,theta1(length(theta1)-i+1))
end
%Extending
for i = 1:length(theta1)
    vecplot(a,b,c,d,theta2(i),theta3(i)+180,theta4(i)+180,theta1(i))
end

end

Arduino Code (C++):

#include "DualMC33926MotorShield.h"
DualMC33926MotorShield md;

int i=0;
int switchPin = 13; //pin for the button
int ledPin = 2;
int reading = LOW;
int lastButtonState = LOW;
int buttonState;
int count;
int everything = 0;
unsigned long lastDebounceTime = 0;
unsigned long debounceDelay = 200; //grace period for button press to avoid false positives

void stopIfFault()
{
if (md.getFault())
{
Serial.println("fault");
while(1);
}
}

void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
Serial.println("Dual MC33926 Motor Shield");
md.init();
md.setM1Speed(0);
pinMode(switchPin,INPUT);
}

void loop() {
// put your main code here, to run repeatedly:
reading = digitalRead(switchPin);
if ((millis() - lastDebounceTime) >= debounceDelay && reading == LOW) {
count++;
digitalWrite(ledPin, HIGH);
everything = 1;
lastDebounceTime = millis();
}
else
{everything = 0;}

if (everything == 1)
{
for (i = 0; i<9000; i++)
{
md.setM1Speed(-50);
delay(1);
stopIfFault();
}

delay(5);
md.setM1Speed(0);

reading = digitalRead(switchPin);
while (reading == HIGH)
{
reading = digitalRead(switchPin);
}
if ((millis() - lastDebounceTime) >= debounceDelay && reading == LOW) {
count++;
digitalWrite(ledPin, HIGH);
everything = 1;
lastDebounceTime = millis();
}
else
{everything = 0;}

if (everything == 1)
{
for (i = 0; i<8500; i++)
{
md.setM1Speed(50);
delay(1);
stopIfFault();
}
}
}
everything =0;
md.setM1Speed(0);
}

Matlab / Arduino Code

<< Results, Conclusion, and Further Work

MATLAB Code

Arduino Code (C++):

>> Citations