<< 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 buttonint 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);
}