PDA

View Full Version : Beta IK code - Bash plz?



Noodle
07-01-2009, 08:59 PM
Here's the code I was using to test the number of calls per 1000ms. Results were around 340Hz. I'm pretty sure I'm doing it right..

//Shoulder -> forearm -> hand
int a = 58;//mm length from servo 2-3
int c = 110;//mm length from servo 3- End Effector
int i=0;
unsigned long time;

void echo(int echo){
Serial.println(echo);
}
void moveArm(int legNum, int x, int y,int z){
//Where should the legs be
double g;
double f;
double b;
int d;
int e;
double truex;
int h;
truex = sqrt(sq(x)+sq(z));
b = sqrt(sq(truex)+sq(y));
f = atan2(y, x) * 180 / PI;
g = acos((sq(a)-sq(c)+sq(b)) / (2 * a * b)) * 180 / PI;
d = g + f; //femur
e = 180-(acos( ( sq(a)+sq(c)-sq(b) )/ (2 * a * c)) * 180 / PI); //tibia
h = atan2(z,y) * 180 / PI; // coxa
//Serial.println(h); <- for debugging, these are the angles
//Serial.println(d);
//Serial.println(e);
writeServo(0,180-h);
writeServo(1,180-d+5);
writeServo(2,180-e);
}
void writeServo(int pin, int angle){
Serial.print("#");
Serial.print(pin);
Serial.print("P");
int pulse = angleToPulse(angle);
Serial.print(pulse);
}
int angleToPulse(int angle){
int pulseAtPos = 2400; //pulse at positive 90*
int pulseAtNeg = 600;
int returnVar = (angle * 10) + pulseAtNeg;
return returnVar;
}

void setup(){
Serial.begin(115200);
int timeToFinish = millis()+1000;
while (millis() < timeToFinish){
moveArm(0, 50, 50,50);
Serial.println(i);
i++;
}

}
void loop(){
Serial.println(i);
}
Edit: This was from a chat in IRC, people wanted to see my code.
Double Edit: And here's what it's implemented on.