Robotic Hand for the Rubber Hand Illusion (RHI)
Intro: Robotic Hand for the Rubber Hand Illusion (RHI)
Here is a first scetch on an ongoing project. Here is already the arduino code and a few pictures, complete instructions will follow.
STEP 1: Arduino Code
//begin the timer
//TCNT2 = 99; // reset timer ct to 99 out of 255
//TIFR2 = 0x00; // timer2 int flag reg: clear timer overflow flag
#include <Servo.h>
int ledPin=13;
boolean changeData; //disable timer2 interrupt if false
Servo servo0; // create servo object to control a servo 0 is for thumb
Servo servo1; // 1 is for the index
Servo servo2; // 2 is for the middle finger
Servo servo3; // 3 is for the ring
Servo servo4; // 4 is for the little
int potpin_servo[5]; // analog entry pin connected to the flex sensor
int tmp; // working variable
int time; //wanted delay in ms
int in[5]; // memorize the position in vector positions where we write the measured value for each servo
int out[5]; // memorize the position in vector positions where we read the measured value for each servo
int high[7]; // memorize for each finger the highest value of resistance (when hand is closed)
int low[7]; // memorize for each finger the lowest value of resistance (when hand is open)
int positions[5][100]; // delay max = 1000ms and interrupt occur every 10ms => 100 positions max
int f; // frequency of wanted movement in Hz
float count;
int posf[5];// working variable for imposed movement
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); // attaches the servo on pin 3 to the servo object
servo2.attach(5);
servo3.attach(6);
servo4.attach(9);
servo0.attach(10);
lowerAngle=70;
upperAngle=100;
time=0; //initial delay
f=100; // frequency in cHz (1Hz = 100)
count=(10000/f); // for 1 Hz, count=100 (100* 10 ms = 1 sec => temps d'un mouvement !)
P=99;
//main tendue:
high[0]=586;
high[1]=576;
high[2]=564;
high[3]=562;
high[4]=528;
//main fermée
low[0]= 694;
low[1]= 764;
low[2]= 736;
low[3]= 760;
low[4]= 738;
for (int i=0; i<5; i++)
{
in[i]=0; // intitialization of vector containing position of end and beginning
out[i]=0;
potpin_servo[i]=i; //initial program : normal mode
posf[i]=count;
}
ServoInit(); //Set an initial position for servo
interrupts();
Timer2init();
changeData=true; // no start untill receive the signal from matlab
} ;
void ServoInit()
{
servo0.write(upperAngle);
servo1.write(upperAngle);
servo2.write(upperAngle);
servo3.write(upperAngle);
servo4.write(upperAngle);
}
void Timer2init()
{
// Setup Timer2 overflow to fire every 8ms (125Hz)
// period [sec] = (1 / f_clock [sec]) * prescale * (255-count)
// (1/16000000) * 1024 * (255-99) = .00998 sec = 10ms
TCCR2B = 0x00; // Disable Timer2 while we set it up
TCNT2 = P; // Reset Timer Count (255-99) = execute ev 125-th T/C clock
TIFR2 = 0x00; // Timer2 INT Flag Reg: Clear Timer Overflow Flag
TIMSK2 = 0x01; // Timer2 INT Reg: Timer2 Overflow Interrupt Enable
TCCR2A = 0x00; // Timer2 Control Reg A: Wave Gen Mode normal
TCCR2B = 0x07; // Timer2 Control Reg B: Timer Prescaler set to 1024
};
ISR(TIMER2_OVF_vect)
{
SREG=SREG | B10000000; // set bit 7 to 1 (global interrupt disabled)
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)) // If we wait 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 of postion of writing and reading in vector (in time is smaller after changing data for example)
{
for (int i=0; i<5; i++)
{
in[i]=0;
out[i]=0;
}
}
else // if the user is changing variables, stop the interrupt (it will be started at the end of the aquisition of data from user)
{
TCNT2 = P; // reset timer ct to 99 out of 255
TIFR2 = 0x00; // timer2 int flag reg: clear timer overflow flag
}
};
void write_position_on_servo(int number_of_servo, Servo servo)
{
tmp=positions[number_of_servo][out[number_of_servo]]; //read first value in vector containing data from flex sensor
servo.write(tmp);
out[number_of_servo]=out[number_of_servo]+1; //increment number that memorize where we must read
if (out[number_of_servo]>=100) //if end of array reached, go to the beginning
{
out[number_of_servo]=0;
}
};
void write_position_from_finger(int number_of_servo, int potpin_finger)
{
if(changeData)
{tmp=upperAngle;}
else
{
if(potpin_finger==9) // If 9, servo is disabled
{
tmp=upperAngle;
}
if(potpin_finger==8) //If 8, move alone mode
{
if(posf[number_of_servo]>((count*5)/6))// this number depends on the speed of the servo (is the number of 10ms that the servo spends "up" before it goes down again)
{tmp=upperAngle;}
else if (posf[number_of_servo]>((count*2)/3))
{tmp=upperAngle-6;}
else if (posf[number_of_servo]>(count/2))
{tmp=upperAngle-12;}
else if (posf[number_of_servo]>(count/3))
{tmp=upperAngle-17;}
else if (posf[number_of_servo]>(count/6))
{tmp=upperAngle-12;}
else
{tmp=upperAngle-6;}
posf[number_of_servo]=posf[number_of_servo]-1;
if(posf[number_of_servo]==0)
{posf[number_of_servo]=count;}
}
if(potpin_finger==0 || potpin_finger==1 || potpin_finger==2 || potpin_finger==3 || potpin_finger==4)
{
tmp=analogRead(potpin_finger); // read the value of flex
tmp>>1; // Bit shift to neglect last value (error of measure)
//Serial.println(tmp);
// if(potpin_finger==0 || potpin_finger==2 || potpin_finger==6 )
//{tmp= map(tmp, low[potpin_finger], high[potpin_finger], upperAngle, lowerAngle);
//} // invert movement of servo
// else
//{tmp= map(tmp, low[potpin_finger], high[potpin_finger], lowerAngle, upperAngle);}
tmp= map(tmp, low[potpin_finger], high[potpin_finger], lowerAngle, upperAngle);
}
}
if (tmp<lowerAngle) // protection of the servo if analog input signal defective
{tmp=lowerAngle;}
if(tmp>upperAngle)
{tmp=upperAngle;}
positions[number_of_servo][in[number_of_servo]]=tmp;
in[number_of_servo]=in[number_of_servo]+1; //increment number that memorize where we must stock data
if (in[number_of_servo]>=100) //if end of array reached, go to the beginning
{in[number_of_servo]=0;}
}
void loop()
{
if(Serial.available()>0) // if there is data to read => MATLAB envoie qqchose
{
a=Serial.read();
if(a==1) // START
{
changeData=false;
} // pour relancer
if(a==2) // STOP
{ changeData=true;
} //va bloquer les actions en ne relancant pas le timer
if(a==3) // CHANGE DATA
{
changeData=true;
while(Serial.available()<7) // attends de recevoir les 7 valeurs
{}
delay(2000);
time=Serial.read();
time=time*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);
for(int i=0; i<5; i++)
{posf[i]=count;}
}
if(a==4)// INIT ????
{
//
}
}
}
//TCNT2 = 99; // reset timer ct to 99 out of 255
//TIFR2 = 0x00; // timer2 int flag reg: clear timer overflow flag
#include <Servo.h>
int ledPin=13;
boolean changeData; //disable timer2 interrupt if false
Servo servo0; // create servo object to control a servo 0 is for thumb
Servo servo1; // 1 is for the index
Servo servo2; // 2 is for the middle finger
Servo servo3; // 3 is for the ring
Servo servo4; // 4 is for the little
int potpin_servo[5]; // analog entry pin connected to the flex sensor
int tmp; // working variable
int time; //wanted delay in ms
int in[5]; // memorize the position in vector positions where we write the measured value for each servo
int out[5]; // memorize the position in vector positions where we read the measured value for each servo
int high[7]; // memorize for each finger the highest value of resistance (when hand is closed)
int low[7]; // memorize for each finger the lowest value of resistance (when hand is open)
int positions[5][100]; // delay max = 1000ms and interrupt occur every 10ms => 100 positions max
int f; // frequency of wanted movement in Hz
float count;
int posf[5];// working variable for imposed movement
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); // attaches the servo on pin 3 to the servo object
servo2.attach(5);
servo3.attach(6);
servo4.attach(9);
servo0.attach(10);
lowerAngle=70;
upperAngle=100;
time=0; //initial delay
f=100; // frequency in cHz (1Hz = 100)
count=(10000/f); // for 1 Hz, count=100 (100* 10 ms = 1 sec => temps d'un mouvement !)
P=99;
//main tendue:
high[0]=586;
high[1]=576;
high[2]=564;
high[3]=562;
high[4]=528;
//main fermée
low[0]= 694;
low[1]= 764;
low[2]= 736;
low[3]= 760;
low[4]= 738;
for (int i=0; i<5; i++)
{
in[i]=0; // intitialization of vector containing position of end and beginning
out[i]=0;
potpin_servo[i]=i; //initial program : normal mode
posf[i]=count;
}
ServoInit(); //Set an initial position for servo
interrupts();
Timer2init();
changeData=true; // no start untill receive the signal from matlab
} ;
void ServoInit()
{
servo0.write(upperAngle);
servo1.write(upperAngle);
servo2.write(upperAngle);
servo3.write(upperAngle);
servo4.write(upperAngle);
}
void Timer2init()
{
// Setup Timer2 overflow to fire every 8ms (125Hz)
// period [sec] = (1 / f_clock [sec]) * prescale * (255-count)
// (1/16000000) * 1024 * (255-99) = .00998 sec = 10ms
TCCR2B = 0x00; // Disable Timer2 while we set it up
TCNT2 = P; // Reset Timer Count (255-99) = execute ev 125-th T/C clock
TIFR2 = 0x00; // Timer2 INT Flag Reg: Clear Timer Overflow Flag
TIMSK2 = 0x01; // Timer2 INT Reg: Timer2 Overflow Interrupt Enable
TCCR2A = 0x00; // Timer2 Control Reg A: Wave Gen Mode normal
TCCR2B = 0x07; // Timer2 Control Reg B: Timer Prescaler set to 1024
};
ISR(TIMER2_OVF_vect)
{
SREG=SREG | B10000000; // set bit 7 to 1 (global interrupt disabled)
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)) // If we wait 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 of postion of writing and reading in vector (in time is smaller after changing data for example)
{
for (int i=0; i<5; i++)
{
in[i]=0;
out[i]=0;
}
}
else // if the user is changing variables, stop the interrupt (it will be started at the end of the aquisition of data from user)
{
TCNT2 = P; // reset timer ct to 99 out of 255
TIFR2 = 0x00; // timer2 int flag reg: clear timer overflow flag
}
};
void write_position_on_servo(int number_of_servo, Servo servo)
{
tmp=positions[number_of_servo][out[number_of_servo]]; //read first value in vector containing data from flex sensor
servo.write(tmp);
out[number_of_servo]=out[number_of_servo]+1; //increment number that memorize where we must read
if (out[number_of_servo]>=100) //if end of array reached, go to the beginning
{
out[number_of_servo]=0;
}
};
void write_position_from_finger(int number_of_servo, int potpin_finger)
{
if(changeData)
{tmp=upperAngle;}
else
{
if(potpin_finger==9) // If 9, servo is disabled
{
tmp=upperAngle;
}
if(potpin_finger==8) //If 8, move alone mode
{
if(posf[number_of_servo]>((count*5)/6))// this number depends on the speed of the servo (is the number of 10ms that the servo spends "up" before it goes down again)
{tmp=upperAngle;}
else if (posf[number_of_servo]>((count*2)/3))
{tmp=upperAngle-6;}
else if (posf[number_of_servo]>(count/2))
{tmp=upperAngle-12;}
else if (posf[number_of_servo]>(count/3))
{tmp=upperAngle-17;}
else if (posf[number_of_servo]>(count/6))
{tmp=upperAngle-12;}
else
{tmp=upperAngle-6;}
posf[number_of_servo]=posf[number_of_servo]-1;
if(posf[number_of_servo]==0)
{posf[number_of_servo]=count;}
}
if(potpin_finger==0 || potpin_finger==1 || potpin_finger==2 || potpin_finger==3 || potpin_finger==4)
{
tmp=analogRead(potpin_finger); // read the value of flex
tmp>>1; // Bit shift to neglect last value (error of measure)
//Serial.println(tmp);
// if(potpin_finger==0 || potpin_finger==2 || potpin_finger==6 )
//{tmp= map(tmp, low[potpin_finger], high[potpin_finger], upperAngle, lowerAngle);
//} // invert movement of servo
// else
//{tmp= map(tmp, low[potpin_finger], high[potpin_finger], lowerAngle, upperAngle);}
tmp= map(tmp, low[potpin_finger], high[potpin_finger], lowerAngle, upperAngle);
}
}
if (tmp<lowerAngle) // protection of the servo if analog input signal defective
{tmp=lowerAngle;}
if(tmp>upperAngle)
{tmp=upperAngle;}
positions[number_of_servo][in[number_of_servo]]=tmp;
in[number_of_servo]=in[number_of_servo]+1; //increment number that memorize where we must stock data
if (in[number_of_servo]>=100) //if end of array reached, go to the beginning
{in[number_of_servo]=0;}
}
void loop()
{
if(Serial.available()>0) // if there is data to read => MATLAB envoie qqchose
{
a=Serial.read();
if(a==1) // START
{
changeData=false;
} // pour relancer
if(a==2) // STOP
{ changeData=true;
} //va bloquer les actions en ne relancant pas le timer
if(a==3) // CHANGE DATA
{
changeData=true;
while(Serial.available()<7) // attends de recevoir les 7 valeurs
{}
delay(2000);
time=Serial.read();
time=time*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);
for(int i=0; i<5; i++)
{posf[i]=count;}
}
if(a==4)// INIT ????
{
//
}
}
}
Comments
StarkillerC 9 years ago
Hi, i would like to know if you can send me the 3D files of the hand. We are students in twelve grades and we have to make a professional project. We have choice to make a hand, and for study the cinematic of the hand, we would like to have yours, because we have to do it very quickly and your hand seems to be perfect for us. Please can you contact me by mail ? Thanks.
(PS : Sorry for our english, we are french.)