JimboZA:
It's simply 4 knob programs in one and is simple in comparison to inverse kinematics in 3D and real time.
Frankly speaking, it is truely simple :%, but at least it worked. Considering on the scale of the robotic arm, this kind of code would satisfy when the arm is small(You won't feel the difference between using function "map" and using function "PID").
However the problem about response time did exist. So I think if want to make the arm larger like human arm, this problem can't be ignored. In that case negtive feedback and PID algorithm may be indispensable. And the driving unite would probably be switched into DC gear motors. So I wrote down this code below, I call it single-neuroned self-adapting NN PID algorithm(Whatever it is...never mind, It is the simplest NN algorithm). Actually it is only test version and anyone can adapt it to his own device if needed. By the way, the code is not perfect yet...and I will be pretty appreciate if you can find mistakes. XD
Here is the code(for only one motor),
const int analogInPin1 = A0;
const int analogInPin2 = A1;
const int analogOutPin = 9;
int y_in = 0;
int r_in = 0;
int u_k = 0;
int u_k_old = 0;
int e_k;
int e_k_old = 0;
int e_k_old2 = 0;
double xite_i = 0.35;
double xite_p = 0.40;
double xite_d = 0.40;
double wk1 = 0.1;
double wk2 = 0.1;
double wk3 = 0.1;
double wk1_old = 0;
double wk2_old = 0;
double wk3_old = 0;
double wk11;
double wk22;
double wk33;
double w;
double k = 0.12;
double x1;
double x2;
double x3;
void setup() {
Serial.begin(9600);
}
void loop() {
y_in = analogRead(analogInPin2);
r_in = analogRead(analogInPin1);
e_k = y_in - r_in;
x1 = e_k;
x2 = e_k - e_k_old;
x3 = e_k - 2*e_k_old + e_k_old2;
w = abs(wk1)+abs(wk2)+abs(wk3);
wk11 = wk1/w;
wk22 = wk2/w;
wk33 = wk3/w;
u_k = u_k_old + k*(wk11*x1 + wk22*x2 + wk33*x3);
wk1_old = wk1;
wk2_old = wk2;
wk3_old = wk3;
wk1 = wk1_old + xite_i*e_k*u_k*(2*e_k - e_k_old);
wk2 = wk2_old + xite_p*e_k*u_k*(2*e_k - e_k_old);
wk3 = wk3_old + xite_d*e_k*u_k*(2*e_k - e_k_old);
analogWrite(analogOutPin, u_k);
Serial.print("r_in = " );
Serial.print(r_in);
Serial.print("\t y_in = ");
Serial.println(y_in);
Serial.print("\t u_k = ");
Serial.println(u_k);
e_k_old2 = e_k_old;
e_k_old = e_k;
u_k_old = u_k;
delay(2);
}
A0 is attached to controlling pot;
A1 is attached to feed-back pot which shares the same shaft with the arthrosis of the arm.