int A,B,C ;
int fsrPin = 0; // the FSR and 10K pulldown are connected to a0
int fsrReading; // the analog reading from the FSR resistor divider
int fsrVoltage; // the analog reading converted to voltage
unsigned long fsrResistance; // The voltage converted to resistance, can be very big so make "long"
unsigned long fsrConductance;
long fsrForce; // Finally, the resistance converted to force
int count = 0; //count of position
double Force_corr, Pos_corr;
int Status_out, enable, Regler_Start, Init_Start, corr_Start, Auto_Status;
int Enable; //emergency stop
double SettingValue, PosGain;
double x, Sampletime,v, w;
double SatForce;
int V1;
double Freq;
double clock;
int state = HIGH;
void setup()
{
Serial.begin(9600);
Status_out = enable = Regler_Start = Init_Start = corr_Start = Auto_Status = 0 ;
Enable = 1;
attachInterrupt(4, emergency, RISING); //interupt 4 , digital pin 19
state = HIGH;
}
void loop()
{
// analogRead(1);
digitalWrite (10, state);
Position_Count();
Force();
Flow_Control(2);
Correction(fsrForce, count, corr_Start);
if (Regler_Start == 0)
{
int f = 0-10*25;
Pos_SetValue(353.81,3.8095,22.8571);
Force_SetValue(f);
Potential();
Virtual_System(0.099048,0.2,0,-0.04,150);
}
else
{
}
state =!state;
}
void Position_Count()
{
int p=0;
int a1 = 0;
int b1 = 0;
A = digitalRead(22);
B = digitalRead(24);
if(A==1 && a1!=A)
{
if(B==1)
{
p=p-1;
a1=1;
b1=1;
}
count=p;
}
if(A==1 && a1!=A)
{
if(B==0)
{
p=p+1;
a1=1;
b1=0;
}
count=p;
}
if(A==0 && a1!=A)
{
if(B==1)
{
p=p+1;
a1=0;
b1=1;
}
count=p;
}
if(A==0 && a1!=A)
{
if(B==0)
{
p=p-1;
a1=0;
b1=0;
}
count=p;
}
if(B==1 && b1!=B)
{
if(A==1)
{
p=p+1;
b1=1;
a1=1;
}
count=p;
}
if(B==1 && b1!=B)
{
if(A==0)
{
p=p-1;
b1=1;
a1=0;
}
count=p;
}
if(B==0 && b1!=B)
{
if(A==1)
{
p=p-1;
b1=0;
a1=1;
}
count=p;
}
if(B==0 && b1!=B)
{
if(A==0)
{
p=p+1;
b1=0;
a1=0;
}
count=p;
}
}
void Force()
{
fsrReading = analogRead(fsrPin);
// analog voltage reading ranges from about 0 to 1023 which maps to 0V to 5V (= 5000mV)
// fsrVoltage = map(fsrReading, 0, 1023, 0, 5000);
fsrVoltage = fsrReading*5;
// The voltage = Vcc * R / (R + FSR) where R = 10K and Vcc = 5V
// so FSR = ((Vcc - V) * R) / V yay math!
fsrResistance = 5000 - fsrVoltage; // fsrVoltage is in millivolts so 5V = 5000mV
fsrResistance *= 10000; // 10K resistor
fsrResistance /= fsrVoltage;
fsrConductance = 1000000; // we measure in micromhos so
fsrConductance /= fsrResistance;
// Use the two FSR guide graphs to approximate the force
if (fsrConductance <= 100) //range 0-2N
{
fsrForce = fsrConductance /20;
// Serial.print("Force in Newtons: ");
// Serial.println(fsrForce);
}
else
{
fsrForce = fsrConductance /50; //range 80N-100N
// fsrForce /= 50;
}
fsrForce = fsrForce*3;
}
void Force2()
{
fsrReading = analogRead(1);
// analog voltage reading ranges from about 0 to 1023 which maps to 0V to 5V (= 5000mV)
// fsrVoltage = map(fsrReading, 0, 1023, 0, 5000);
fsrVoltage = fsrReading*5;
// The voltage = Vcc * R / (R + FSR) where R = 10K and Vcc = 5V
// so FSR = ((Vcc - V) * R) / V yay math!
fsrResistance = 5000 - fsrVoltage; // fsrVoltage is in millivolts so 5V = 5000mV
fsrResistance *= 10000; // 10K resistor
fsrResistance /= fsrVoltage;
fsrConductance = 1000000; // we measure in micromhos so
fsrConductance /= fsrResistance;
// Use the two FSR guide graphs to approximate the force
if (fsrConductance <= 100) //range 0-2N
{
fsrForce = fsrConductance /20;
// Serial.print("Force in Newtons: ");
// Serial.println(fsrForce);
}
else
{
fsrForce = fsrConductance /50; //range 80N-100N
// fsrForce /= 50;
}
fsrForce = fsrForce*3;
}
void Correction(double Force, double Position, int Reset)
{
int mtime = 0;
int force = 0;
int pos = 0;
int mreset = 0;
if(Reset==1 && mreset==0)
{
if(mtime==0)
{
mtime = clock;
}
if(mtime >= 0)
{
if(clock>(mtime + 5))
{
force = Force;
pos = Position;
mreset = 1;
}
}
}
if(Reset==0)
{
mreset=0;
mtime=0;
}
Force_corr = force;
Pos_corr = pos;
}
void Flow_Control(int Status)
{
// int Status_out, enable, Regler_Start, Init_Start, corr_Start, Auto_Status;
// Status_out = enable = Regler_Start = Init_Start = corr_Start = Auto_Status = 0;
Status_out=Status; //Status = input, Status_out = output
if(Enable==1) //Enable = input, enable = output
{
if(Status==0)
{
enable=0;
}
if(Status==1)
{
enable=4.5;
Init_Start = 1;
}
else
{
Init_Start = 0;
}
if(Status==2)
{
enable=0;
corr_Start=1;
}
else
{
corr_Start=0;
}
if(Status==3)
{
enable=4.5;
Regler_Start=1;
}
else
{
Regler_Start=0;
}
}
if(Enable==0)
{
enable==0;
Regler_Start=0;
corr_Start=0;
Auto_Status=0;
}
}
void emergency()
{
Enable = 0;
}
// Pos_SetValue(353.81, 3.8095, 22.8571 )
void Pos_SetValue(double P_Person, double I_Gain, double D_Gain)
{
double istPosition ;
double SetValue;
double PosDiff;
int Position;
Position = count - Pos_corr;
istPosition = Position *0.007;
PosDiff = x - Position;
double intPosDiff;
//intPosDiff ---> integration of PosDiff
intPosDiff += PosDiff*Sampletime;
SetValue = v*w*D_Gain + PosDiff*P_Person +I_Gain*intPosDiff;
// SettingValue ---> go through saturation function
if(SetValue >= 10)
{
SettingValue = 10;
}
else if(SetValue <= -10)
{
SettingValue = -10;
}
else
{
SettingValue = SetValue; // Output Setting Value to control motor
}
PosGain = SetValue - SettingValue;
}
///////// Force_SetValue( -10*25 / -5*15, )
void Force_SetValue(int Ks)
{
int Force;
double SetForce;
Force = fsrForce - Force_corr;
SetForce = x*Ks -23*Force;
// SatForce --> SetForce go through saturation
if(SetForce >= 50)
{
SatForce = 50;
}
else if(SetForce <= -50)
{
SatForce = -50;
}
else
{
SatForce = SetForce; // Output Setting Value to control motor
}
}
/////////////////// Virtual_System(0.099048, 0.2, ??, -0.04, 150)
void Virtual_System(double maxampl, double SollAmpl, double Kp, double Damp, int Mass )
{
double Amplitude, P_ampl;
double Sum;
double dampingValue, Damping;
double Vset;
double acceleration, acceleration2;
Amplitude = sqrt(v*v + x*x);
// C code for dampingValue, if...else
if(Amplitude >= maxampl && Amplitude <= maxampl+0.05)
{
dampingValue = 0.1*Vset;
}
else if(Amplitude > maxampl+0.05)
{
dampingValue = 0.3*Vset;
}
else if(Amplitude > maxampl+0.1)
{
dampingValue = 10*Vset;
}
else
{
dampingValue = 0;
}
P_ampl = Amplitude - SollAmpl;
Damping = P_ampl*Kp;
Sum = Vset*Damping;
acceleration = 0 - dampingValue - Sum - Vset*Damp - x*w - SatForce/Mass;
double V_Conversion;
if(v >= 0)
{
V_Conversion = 1 / ((1+V1)*(1+V1)) ;
}
else
{
V_Conversion = 1 / ((1-V1)*(1-V1)) ;
}
acceleration2 = acceleration*V_Conversion;
// v = integrate of acceleration2
v = v + acceleration2;
Potential();
w = Freq * 2*PI/60 ;
Vset = v*w - PosGain;
//x = integrate of Vset
x = x + Vset*Sampletime ;
}
void Potential()
{
int PotentioPin = 2;
int PotentioReading;
int Potentiometer;
PotentioReading = analogRead(PotentioPin);
// Potentiometer reading ranges from about 0 to 1023 which maps to 0V to 5V (= 5000mV)
// Potentiometer = map(PotentioReading, 0, 934, -2500, 2500);
Potentiometer = PotentioReading * 5;
double U;
U = Potentiometer;
if (PotentioReading != 0 )
{
Freq = floor((8+3*U)*4+0.5)/4;
}
else
{
Freq = 1;
}
}