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

}

>> Citations