mathematics to servo

Projects Programming
1

Guys, I am trying to write trigonometry function to control servo using PS2 Controller. The servo will move to angles calculated by the formula. I managed to write the formula, but the servo doesnt react well (slow response, even to the point of the servo trembling), while the servo works fine if I control it manually using serial monitor. I suspect I must change the float to int or maybe adding float to the formula. Any help will be appreciated.

#include <Servo.h>
Servo pan;
Servo servol1;
Servo servol2;

int xe = 10;
int ye = 10;
int ze = 0;
const int l1 = 10;
const int l2 = 10;
float a;
float b;
float thetaA;
float thetaB;
float thetaC;
float theta1;
float theta2;
float theta3;
const float pi = M_PI;

void setup(){
pan.attach(2);
servol1.attach(3);
servol2.attach(4);
Serial.begin(57600);
}

void loop(){
calculate_theta();
move_arm();
}

void calculate_theta(){
a = sqrt(sq(xe) + sq(ze));
theta1 = (acos(ze/a)) * (180/pi);

b = sqrt(sq(a) + sq(ye));
thetaA = (acos(a/b)) * (180/pi);
thetaB = (acos((sq(l1) + sq(b) - sq(l2))/(2l1b))) * (180/pi);
theta2 = thetaA + thetaB;

thetaC = (acos((sq(l1) + sq(l2) - sq(b))/(2l1l2))) * (180/pi);
theta3 = 180 - thetaC;
}

void move_arm(){
pan.write(theta1);
servol1.write(theta2);
servol2.write(theta3);
}

2

Why do you initialize the Serial port and not use it?

You may want to use Serial.writeln to Monitor the values you are writing to the servos. You might see where the jitters are coming from.

The servo library defines the angle Parameter as int. Since you are using float, it will be converted to int and you may be encountering rounding Problems.

3

You are sending the move_arm signal to the servo hundreds of thousands of times a second - no wonder it gets confused!

How about sending the "move" signal no more than once every half second? See if that sorts it out:

uint32_t last_update_ms = 0;

void loop(){
  if(millis() - last_update_ms > 500) {
    calculate_theta();
    move_arm();
    last_update_ms = millis();
  }
}
4

Here's how it works. The input will be xe,ye and ze, while the output is theta1, theta2 and theta3. Basically, I change the value of xe, ye and ze using PS2 controller to increase or decrease the value by 1. The arduino will do the math given the formula to calculate theta1, theta2 and theta3 and the servo will move. I have tried serial print to check the value of theta1 before actually use the value to move servo and nothing's wrong. So, do I have to convert every float to int instead of using float? I afraid that it will affect on how theta1 will be rounded to move servo.

5

Auuwwww, poor Arduino. That is A LOT of float and trigonometry... The Arduino is, saying it mildly, bad at it...

But luckily most of the time you can simplify it a lot and use int math :wink:

6

Hi,
Welcome to the forum.

Please read the first post in any forum entitled how to use this forum.
http://forum.arduino.cc/index.php/topic,148850.0.html then look down to item #7 about how to post your code.
It will be formatted in a scrolling window that makes it easier to read.

If you need finer servo control try servoWriteMicroseconds.

Tom.... :slight_smile:

7

PaulMurrayCbr:
You are sending the move_arm signal to the servo hundreds of thousands of times a second - no wonder it gets confused!

How about sending the "move" signal no more than once every half second? See if that sorts it out:

uint32_t last_update_ms = 0;

void loop(){
  if(millis() - last_update_ms > 500) {
    calculate_theta();
    move_arm();
    last_update_ms = millis();
  }
}

Thanks for the help, but still doesnt work. Is there any other way? I have tried using microseconds, but its not helping. The servo still shakes (maybe thats what you mean by confused).

8

but still doesnt work.

It does something. That means that the Arduino IS working. That what it does is not what you expect simply means that your expectations are wrong.

You need to tell us what the code actually does, what you expect it to do, and post ALL of the code.

9

Hi,
180 in your calculations should be 180.0

Tom... :slight_smile: