/********************************************************************** Copyright (C) 2013 SARVESH KULKARNI Associate Professor, ECE Department Villanova University, PA, USA. LEGAL NOTICE: This library is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . ------------------------------------------------------------------------- Library of routines for controlling analog and digital servomotors using Adafruit's PCA9685 16-channel PWM breakout board. You may use *any* PCA9685 based as long as the correct I2C device address is used. Version 0.3, Released August 2013. You will need the C++ library for PCA9685; get the following file pca9685.o OR Compile the source files pca9685_pwm.h & pca9685_pwm.cc to pca9685.o -------------------------------------------------------------------------- Class for controlling Servomotors Typical Wire Connections on Servo motors: (please cross-check!) (+V) : Red (GND) : Black or Brown (PWM) : Orange or White *********************************************************************/ #include "pca9685_pwm.h" #include "servo.h" // Class Constructor. No i/p validation is performed since the expected // defaults cannot be reasonably guessed. // BE VERY CAREFUL in setting these parameters for your class objects. // Incorrect parameters WILL DAMAGE YOUR SERVO!!! You have been warned! // range: degrees; pycle, minpulse, maxpulse: microseconds Servo::Servo(float range, float pcycle, float minpulse, float maxpulse, PWM_Board& pwm, unsigned int ch_num, float freq) { s_range = range; s_pcycle = pcycle; s_minpulse = minpulse; s_maxpulse = maxpulse; pwm_channel = ch_num; // Safe default: off if continuous rotation servo, centered position otherwise // if the shaft on the continuous rotation servo is rotating, then you need // to adjust the trim screw that sets the center point center(pwm); // should I check to see if it was actually centered? maybe not! if (s_range == 360.0) // continuous rotation servo s_angle = 360.0; // position cannot be fixed else // non-continuous rotation servo; limited angular range s_angle = 0; // centered position } // Returns current position of servo shaft // NOTE: This is really a hobbyist-level determination. If you need accurate positioning // info, you *must* use an encoder. float Servo::getPosition(void) { if (s_range < 360) return s_angle; // angle of shaft (0 deg = center) else return 360.0; // position unknown } // Sets the servo motor shaft to the specified angle (in degrees) // e.g. for a std range-limited servo, -45 or 45 means 45 deg on either side of the center. // For a continuous rotation servo, you may use any angle from -180 to 180. The polarity // indicates the rotational direction, and the magnitude is translated into angular speed. // A magnitude of zero implies that the cont. rotation servo is halted (not rotating). // // SAFETY NOTES: Permanent damage to a servo may result under various conditions as follows .. // 1. angle = -s_range/2 implies that the servo shaft is to be moved to the left extreme // position (corresponding to its min pulse width). // angle = +s_range/2 implies that the servo shaft is to be moved to the right extreme // position (corresponding to the max pulse width). // angle = 0 implies that the servo shaft is to be centered. // 2. If the pulse frequency is set too high (usually >> 50 Hz), the control electronics in // the servo motor may exhibit strange behavior with the possibility of permanent damage. // 3. If you attempt to set the angle of the servo shaft beyond its normal range of movement // (for example, by setting the pulse width to a value smaller than the specified minimum, // or by setting the pulse width to a value greater than the specified maximum), then you // are, in effect, attempting to drive the servo motor beyond its internal physical stops. // This will cause the motor to stall and draw a very large current ultimately resulting // in the physical destruction of your servo, and possibly a fire!!! This WARNING is not // to be taken lightly. int Servo::setPosition(float angle, PWM_Board& pwm) { int pulsefreq; float pulse_range, midpoint, pulse_width; // Center point of servo shaft is 0 deg in our convention. However, computations assume // that 0 deg is the left extreme for the servo shaft i.e. for a servo with 140 deg // range, the center point is computed as 70 degrees, and the left and right // extremes are 0 deg and 140 deg resp. So, we must translate our conventions to // the std conventions used in computation. midpoint = s_range/2; // center point acc. to std convention angle = angle + midpoint; // convert our angle to std convention // limit servo shaft's anticlockwise, or clockwise excursions if (angle < 0) angle = 0; if (angle > s_range) angle = s_range; pulsefreq = pwm.getFreq(); // a negative return value indicates error in // reading the PCA9685's prescale register if ((pulsefreq < 0)||(pulsefreq > MAXFREQ)) return FREQ_ILLEGAL; pulse_range = s_maxpulse - s_minpulse; // in microseconds pulse_width = (s_minpulse + (pulse_range * angle/s_range))/1000; // in milliseconds if (pwm.setPulseWidth(pulse_width, pwm_channel) < 0) return POSITIONING_ERR; else { s_angle = angle - midpoint; // record new angle in class data member return 0; } } // Centers the servo motor shaft // 0 deg is the center position in our convention int Servo::center(PWM_Board& pwm) { return setPosition(0, pwm); } // Turns off the specific channel that this servo is being driven from // In other words, turn off all pulses on this channel int Servo::sleep(PWM_Board& pwm) { if (pwm.fullOff(pwm_channel) < 0) return SLEEP_ERR; else return 0; }