Introduction: 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 ????
{
//
}
}
}