First off, I'm very sorry. This is quite a bit of code, and I'm a beginner, so the mistakes are probably quite obvious.
Background information: I have 3 motors with encoders attached around a bike wheel. Strings from these motors are attached to fingers (only one finger now). By knowing the length of these strings (from these encoders), the co-ordinates of the fingers can be calculated. By varying the torque of these motors, a force vector can be created it the motor's direction, perhaps creating the feeling of something pushing against your finger. this is known as haptics
If anyone can advise me on fixing any of the many errors this code creates I would be so grateful! To go through the formulas and things I've made for this code would take ages, but it's fairly obvious what I'm trying to do line by lane (I hope).
void setup ()
{
pinMode(0,OUTPUT);//PWMs (3 motors)
pinMode(1,OUTPUT);
pinMode(2,OUTPUT);
pinMode(22,INPUT); //encoders (2 signals for each motor)
pinMode(23,INPUT);
pinMode(24,INPUT);
pinMode(25,INPUT);
pinMode(26,INPUT);
pinMode(27,INPUT);
/*
PinMode(42,OUTPUT); //LEDs (not necessary)
PinMode(43,OUTPUT);
PinMode(44,OUTPUT);
*/
}
float length[9]={
0.40,0.40,0.40,0.40,0.40,0.40,0.40,0.40,0.40 }; //String length from each motor (used to be 9), giving them starting values
boolean init[6] = {
digitalRead(22),digitalRead(23), digitalRead(24),digitalRead(25), digitalRead(26), digitalRead(27) };// encoder values shall be kept here
float R= 0.34 ;
float Pi=3.14159;
float sr3=sqrt(3);
float g=2Rsr3;
boolean iniA;
boolean iniB;
boolean A;
boolean B; //These shall be used when working out the motor turning direction
void pos1(){
float Lab=sq(length[0])–sq(length[1]);
float Lac=sq(length[0])–sq(length[2]); //Why are these generating errors??
float Xa = (Lac)/g;
float Ya = (2Lab – RLac)/(6R);
float Za = sqrt(length[1]length[1]-LacXa+ YaYa+Ya2R-R*R);
FunctionBlocks1 ();
}
int FunctionDir ()//Encoder direction
{
int C;
if (iniA==1)
{
if(iniB==1)
{
if(B==1)
{
C=-1;
}
else
{
C=1 ; }
}
else
{
if(A==1)
{
C=-1;
}
else
{
C=1 ;
}
}
}
else if (iniB==1)
{
if (A==1)
{
C==1;
}
else
{
C==-1 ;
}
}
else if (A==1)
{
C=-1 ;
}
else
{
C=1 ;
}
return (C);
}
void FunctionEnc (int m,int e) {
//digitalwrite(m+42,HIGH);
A=digitalRead(e);
B=digitalRead(e+1);
int iniA =init[m2];
int iniB =init[m2+1];
if(iniA!=A|iniB!=B)
{
length[m]=+FunctionDir();
pos1();
}
init[m2]=A;
init[m2+1]=B;
}
void FunctionBlocks1// creates virtual blocks for the user to "feel"
{
if (Za>0.3)
{
if(Ya>0.07)
{
Fz = 1+(Za-0.3)*100;
}
else{
if(x>0)
{
Fz = (Za-0.3)*20;
}
else{
Fz = (Za-0.3)*100;
}
}
FunctionForce(1,Fz)
}
void FunctionForce(t,f){//calculated torque pwm values for each motor
float Fa=length[0]f(( Ya+sr3Xa-R)/(RZa))/3;
float Fb=length[1]f(( 2Ya+R)/(RZa))/3;
float Fc=length [2]f(( Ya-sr3Xa-R)/(RZa))/3;
Ta=Fa0.07;
Tb=Fb0.07;
Tc=Fc0.07;
float defaul = 0.01;
if (defaul<Ta<0.1)
{
Wa=255Ta/0.1 };
else if (Ta<defaul)
{
Wa=255defaul/0.1 };
else if(Ta>0.1)
{
Wa=255 };
analogWrite(0,Wa) ;
if (defaul<Tb<0.1)
{
Wb=255Tb/0.1 };
else if (Tb<defaul)
{
Wb=255defaul/0.1 };
else if(Tb>0.1)
{
Wb=255 };
analogWrite(1,Wb);
if (defaul<Tc<0.1)
{
Wa=255Tc/0.1 };
else if (Tc<defaul)
{
Wc=255defaul/0.1 };
else if(Tc>0.1)
{
Wc=255 };
analogWrite(2,Wc);
}
void loop ()
{
FunctionEnc(0,22);
//digitalwrite(42,LOW)
FunctionEnc(1,24);
//digitalwrite(43,LOW)
FunctionEnc(2,26);
//digitalwrite(44,LOW)
}