Robotic hand för gummi Hand Illusion (RHI) (1 / 2 steg)

Steg 1: Arduino kod

#include < Servo.h >
int ledPin = 13.
booleska changeData; Inaktivera timer2 avbrott om falska

Servo servo0; skapa servo objekt till kontroll en servo 0 är för tumme
Servo servo1; 1 är för indexet
Servo servo2; 2 är för långfingret
Servo servo3; 3 är för ring
Servo servo4; 4 är för det lilla

int potpin_servo [5]. analoga posten pin ansluten till flex sensorn
int tmp; arbetande variabel
int tid; önskad fördröjning i ms
int i [5]. memorera placeringen i vektor positioner där vi skriver det uppmätta värdet för varje servo
int ut [5]. memorera placeringen i vektor positioner där vi läser det uppmätta värdet för varje servo
int hög [7]. memorera för varje finger det högsta värdet av motstånd (när sidan är stängd)
int låg [7]. memorera för varje finger det lägsta värdet för motstånd (när hand är öppet)
int positioner [5] [100]. fördröja max = 1000ms och avbrott inträffar varje 10ms = > 100 positioner max
int f; frekvensen av önskad rörelse i Hz
float count;
int posf [5] ;// arbetar variabel för påtvingade förflyttning
int P; prescaller timer

int lowerAngle; valeur max et min des servo: 30 et 160°
int upperAngle;

int a;

void setup()
{
pinMode(ledPin,OUTPUT);
Serial.BEGIN(9600);
servo1.attach(3); tillmäter objektet servo servo på stift 3
servo2.attach(5);
servo3.attach(6);
servo4.attach(9);
servo0.attach(10);
lowerAngle = 70.
upperAngle = 100;
tid = 0; inledande förseningen
f = 100; frekvens i cHz (1 Hz = 100)
count=(10000/f); för 1 Hz, räkning = 100 (100 * 10 ms = 1 sek = > temps d'un mouvement!)
P = 99.
viktigaste tendue:
hög [0] = 586;
hög [1] = 576;
hög [2] = 564;
hög [3] = 562;
hög [4] = 528;

viktigaste fermée
låg [0] = 694;
låg [1] = 764;
låg [2] = 736;
låg [3] = 760;
låg [4] = 738;

för (int jag = 0; jag < 5; i ++)
{
i [i] = 0; intitialization av vektor som innehåller position i slutet och början
ute [i] = 0;
potpin_servo [i] = i. inledande program: normalläge
posf [i] = count;
}

ServoInit(); Ange en inledande position för servo
interrupts();
Timer2init();
changeData = sant; ingen start tills ta emot signalen från matlab

} ;

void ServoInit()
{
servo0.write(upperAngle);
servo1.write(upperAngle);
servo2.write(upperAngle);
servo3.write(upperAngle);
servo4.write(upperAngle);

}

void Timer2init()
{
Setup Timer2 overflow eld varje 8ms (125Hz)
tid [s] = (1 / f_clock [s]) * prescale * (255-count)
(1/16000000) * 1024 * (255-99) =.00998 SEK = 10ms

TCCR2B = 0X00; Inaktivera Timer2 medan vi upp det
TCNT2 = P; Nollställa timern Count (255-99) = kör ev 125-th T/C klockan
TIFR2 = 0X00; Timer2 INT flagga Reg: Tydlig Timer Overflow flagga
TIMSK2 = 0X01; Timer2 INT Reg: Timer2 Overflow Interrupt Enable
TCCR2A = 0X00; Timer2 kontroll Reg A: Wave Gen läge normal
TCCR2B = 0X07; Timer2 kontroll Reg B: Timer Prescaler inställd på 1024
};

ISR(TIMER2_OVF_vect)
{

SREG = SREG | B10000000; ställa in stycke 7 till 1 (global avbrott inaktiverad)

write_position_from_finger(0,potpin_servo[0]);
write_position_from_finger(1,potpin_servo[1]);
write_position_from_finger(2,potpin_servo[2]);
write_position_from_finger(3,potpin_servo[3]);
write_position_from_finger(4,potpin_servo[4]);

IF((in[0]-Out[0]) > =(time/10) || (out[0]-in[0]) > =(99-(time/10)-1)) / / om vi vänta enougth
{

write_position_on_servo (0, servo0);
write_position_on_servo (1, servo1);
write_position_on_servo (2, servo2).
write_position_on_servo (3, servo3).
write_position_on_servo (4, servo4).
}

IF(changeData==true) / / changeData = true, reinitialisation av position för skriva och läsa i vektor (i tid är mindre efter byte data till exempel)
{

för (int jag = 0; jag < 5; i ++)
{
i [i] = 0;
ute [i] = 0;
}
}
annat / / om användaren ändrar variabler, stoppa avbrottet (det kommer att startas i slutet av förvärvet av data från användare)
{

TCNT2 = P; nollställa timern ct till 99 av 255
TIFR2 = 0X00; timer2 int flagga reg: rensa timer overflow flagga
}
};

void write_position_on_servo (int number_of_servo, Servo servo)
{
tmp = positioner [number_of_servo] [out [number_of_servo]]; Läs första värdet i vektorn som innehåller data från flex sensor
servo.write(tmp);
ut [number_of_servo] = ut [number_of_servo] + 1; steg nummer som memorera där vi måste läsa
om (ut [number_of_servo] > = 100) //if slutet av array, gå till början
{
ut [number_of_servo] = 0;
}
};

void write_position_from_finger (int number_of_servo, int potpin_finger)
{

IF(changeData)
{tmp = upperAngle;}
annat
{

IF(potpin_finger==9) / / om 9, servo är inaktiverat
{
tmp = upperAngle;
}
IF(potpin_finger==8) //If 8, flytta ensam läge
{
om (posf [number_of_servo] > ((count*5)/6)) / / detta nummer beror på hastigheten på servo (är antalet 10ms som servo spenderar "upp" innan det går ner igen)
{tmp = upperAngle;}
annars om (posf [number_of_servo] > ((count*2)/3))
{tmp = upperAngle-6;}
annars om (posf [number_of_servo] >(count/2))
{tmp = upperAngle-12.}
annars om (posf [number_of_servo] >(count/3))
{tmp = upperAngle-17;}
annars om (posf [number_of_servo] >(count/6))
{tmp = upperAngle-12.}
annat
{tmp = upperAngle-6;}

posf [number_of_servo] = posf [number_of_servo] -1;
IF(posf[number_of_servo]==0)
{posf [number_of_servo] = count;}
}

om (potpin_finger == 0 || potpin_finger == 1 || potpin_finger == 2 || potpin_finger == 3 || potpin_finger == 4).
{
tmp=analogRead(potpin_finger); läsa värdet på flex
tmp >> 1; Bitars SKIFT att försumma sista värdet (fel åtgärd)
Serial.println(tmp);
om (potpin_finger == 0 || potpin_finger == 2 || potpin_finger == 6)
{tmp = karta (tmp låg [potpin_finger], hög [potpin_finger], upperAngle, lowerAngle);

} / / Invertera förflyttning av servo
annat
{tmp = karta (tmp låg [potpin_finger], hög [potpin_finger], lowerAngle, upperAngle);}
tmp = karta (tmp låg [potpin_finger], hög [potpin_finger], lowerAngle, upperAngle);

}
}

om (tmp < lowerAngle) / / skydd av servo om analog input signal defekt
{tmp = lowerAngle;}
IF(tmp>upperAngle)
{tmp = upperAngle;}

positioner [number_of_servo] [i [number_of_servo]] = tmp;

i [number_of_servo] = i [number_of_servo] + 1; steg nummer som memorera där måste vi lagerför data
om (i [number_of_servo] > = 100) //if slutet av array, gå till början
{i [number_of_servo] = 0;}

}

void loop()
{

IF(Serial.available() > 0) / / om det finns data att läsa = > MATLAB envoie qqchose
{
a=serial.Read();

IF(a==1) / / START
{

changeData = false;
} / / pour relancer

IF(a==2) / / stopp
{changeData = sant;
} //va bloquer les åtgärder sv ne relancant pas le timer

IF(a==3) / / ändra uppgifter
{
changeData = sant;

While(Serial.available() < 7) / / deltar de recevoir les 7 valeurs
{}
Delay(2000);

Time=Serial.Read();
tid = tid * 10;
f=Serial.Read();
potpin_servo[0]=Serial.Read();
potpin_servo[1]=Serial.Read();
potpin_servo[2]=Serial.Read();
potpin_servo[3]=Serial.Read();
potpin_servo[4]=Serial.Read();
count=(5000/f);
för (int jag = 0; jag < 5; i ++)
{posf [i] = count;}

}

IF(a==4) / / INIT???
{
//
}

}

}