Results 1 to 1 of 1

Thread: Beta IK code - Bash plz?

  1. #1
    Join Date
    Apr 2009
    Location
    Brisbane, Australia
    Posts
    48
    Rep Power
    36

    Beta IK code - Bash plz?

    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..
    Code:
    //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.
    Last edited by Noodle; 07-01-2009 at 09:30 PM.

Thread Information

Users Browsing this Thread

There are currently 1 users browsing this thread. (0 members and 1 guests)

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts
  •