SHARE
TWEET

arduino DS3115 and RDS3115 servo control

a guest Jun 17th, 2016 653 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. // Sketch for rapidly moving two servers to a point from -90 to 90 degrees on one motor while
  2. // at the same time driving a second servo from -45 to 90 degrees
  3. // Using a serial interface to allow me to type in test coordinates to direct the servos.
  4. //
  5. //
  6.  
  7. //#define DEBUG
  8.  
  9. #include <Servo.h>
  10.  
  11. Servo lowerservo;  // create servo object to control a servo  DS3115 MG (0-180 degrees) Digital Servo
  12. Servo upperservo;  // create servo object to control a servo RDS3115 MG (0-135 degrees) Robot Digital Servo
  13.  
  14.  
  15. void setup() {
  16.   Serial.begin(9600);
  17.   lowerservo.attach( 9,544,2400);  // attaches lower servo on pin  9 on servo object  DS3115 MG
  18.   upperservo.attach(10,544,2400);  // attaches upper servo on pin 10 on servo object RDS3115 MG
  19. //  544uS =   0 degrees
  20. // 1500uS =  90 degrees
  21. // 2400uS = 180 degrees
  22.   servos_init();
  23. }
  24.  
  25.  
  26. void loop() {
  27.  while (Serial.available() == 0);
  28.  int lower = Serial.parseInt(); //read int from serial
  29.  lower=constrain(lower, -90, 90); // limit the input value from -90 degrees to +90 degrees for the lower servo
  30.  lower=map(lower,-90, 90, 0, 180); // here I am re-mapping -90<->+90 to 0<->180
  31.  
  32.  while (Serial.available() == 0);
  33.  int upper = Serial.parseInt(); //read int from serial
  34.  upper=constrain(upper, -45, 90); // limit the input value from -45 degrees to +90 degrees for the upper servo
  35.  upper=map(upper,-45,90,135,0); // here I am re-mapping  -45<->90 to 135<->0
  36.  servo_line(lower,upper);
  37.  //delay(1000);
  38.  //servo_line(random(0, 180),random(0, 135)); // point in random directions.
  39. }
  40.  
  41.  
  42. int servos_init() // wiggle the servo about to make sure that its internal tracking latches.
  43. {                 // Only needed at power on when the internal state is unknown to the servo from powerdown.
  44.   servo_line( 90, 135); //   0,-45
  45.   servo_line( 90,   0); //   0, 90
  46.   servo_line(  0,   0); // -90, 90
  47.   servo_line(180,   0); //  90, 90
  48.   servo_line( 90, 135); //   0,-45
  49.   servo_line( 90,  90); //   0,  0
  50. }
  51.  
  52.  
  53. void servo_line(int new_lower_angle, int new_upper_angle)
  54. {
  55.  int new_lower_pulsewidth = map(new_lower_angle, 0, 180, 544,  2400); // convert angles to uS duration for PWM(high)
  56.  int new_upper_pulsewidth = map(new_upper_angle, 0, 180, 544,  2400); // convert angles to uS duration for PWM(high)
  57. #ifdef DEBUG
  58. // print out the angles in degrees
  59.  Serial.print(new_lower_angle); Serial.print(" "); Serial.println(new_upper_angle);
  60. // print out the Microseconds that the PWM is high the rest of the 20 miliseconds it is low
  61.  Serial.print(lowerservo.readMicroseconds()); Serial.print(" ");  
  62.  Serial.print(upperservo.readMicroseconds()); Serial.print(" ");
  63.  Serial.print(new_lower_pulsewidth); Serial.print(" ");  
  64.  Serial.println(new_upper_pulsewidth);
  65. #endif  
  66.  line(lowerservo.readMicroseconds(), upperservo.readMicroseconds(), new_lower_pulsewidth, new_upper_pulsewidth);
  67. }
  68.  
  69.  
  70. // line drawing code from http://rosettacode.org/wiki/Bitmap/Bresenham's_line_algorithm#C
  71.  
  72. void line(int x0, int y0, int x1, int y1) // current x_pwm, current y_pwm, destination x_pwm, destination y_pwm
  73. {
  74.   int dx = abs(x1-x0), sx = x0<x1 ? 1 : -1;
  75.   int dy = abs(y1-y0), sy = y0<y1 ? 1 : -1;
  76.   int err = (dx>dy ? dx : -dy)/2, e2;
  77.  
  78.   for(;;){
  79. //    setPixel(x0,y0);
  80.     lowerservo.writeMicroseconds(x0);
  81.    delay(1);           //delay for settling and to not move both servos at once (less power required)
  82.     upperservo.writeMicroseconds(y0);
  83.     delay(1);           //delay for settling and to not move both servos at once (less power required)
  84.  
  85.     if (x0==x1 && y0==y1) break;
  86.     e2 = err;
  87.     if (e2 >-dx) { err -= dy; x0 += sx; }
  88.     if (e2 < dy) { err += dx; y0 += sy; }
  89.   }
  90.  
  91. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top