MHVLib  20111011
An efficiency oriented runtime library for AVR microcontrollers
A:/eclipse/mhvlib/MHV_ServoControl.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Make, Hack, Void Inc
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *  * Redistributions of source code must retain the above copyright
00008  *    notice, this list of conditions and the following disclaimer.
00009  *  * Redistributions in binary form must reproduce the above copyright
00010  *    notice, this list of conditions and the following disclaimer in the
00011  *    documentation and/or other materials provided with the distribution.
00012  *  * Neither the name of the Make, Hack, Void nor the
00013  *    names of its contributors may be used to endorse or promote products
00014  *    derived from this software without specific prior written permission.
00015  *
00016  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019  * DISCLAIMED. IN NO EVENT SHALL MAKE, HACK, VOID BE LIABLE FOR ANY
00020  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026  */
00027 
00028 
00029 /*
00030  *      Fixme:  need to measure how many clocks it takes to get to the point in the handler to toggle the pin,
00031  *      then subtract that from the trigger time
00032  *      Fixme: We lose accuracy if multiple servos are positioned on the same clock
00033  *              one possible solution is to cluster the servos into groups such that there are no closely positioned
00034  *                      servos in any group, then stagger the triggering of the groups
00035  *
00036  */
00037 
00038 #include <MHV_ServoControl.h>
00039 
00040 #ifdef MHV_TIMER16_1
00041 
00042 #define SERVO_ORDER(mhvServoIndex) \
00043         (_controlBlocks[mhvServoIndex].servoOrder)
00044 
00049 void MHV_ServoControl::refreshServos(void *data) {
00050         if (255 == _nextServoIndex) {
00051                 // Start of the servo pulse
00052                 for (uint8_t i = 0; i < _count; i++) {
00053                         if (_controlBlocks[i].port) {
00054                                 mhv_pinOn(0, _controlBlocks[SERVO_ORDER(i)].port,
00055                                                 0, _controlBlocks[SERVO_ORDER(i)].pin, -1);
00056                         }
00057                 }
00058                 _nextServoIndex = 0;
00059                 // Set up the timer to end the first servo pulse
00060                 // Note that the timer is reset to 0 when the interrupt is triggered, so the current value is how long to reach this point
00061                 _timer->setPeriods(MHV_TIMER_PRESCALER_5_1,
00062                                 _controlBlocks[SERVO_ORDER(_nextServoIndex)].position - _timer->current(), 0, 0);
00063                 _timer->enable();
00064                 return;
00065         }
00066 
00067         uint16_t startPosition = _controlBlocks[SERVO_ORDER(_nextServoIndex)].position;
00068 
00069         for (;;) {
00070                 mhv_pinOff(0, _controlBlocks[SERVO_ORDER(_nextServoIndex)].port,
00071                                 0, _controlBlocks[SERVO_ORDER(_nextServoIndex)].pin, -1);
00072                 _nextServoIndex++;
00073                 if (_nextServoIndex >= _count || 255 == SERVO_ORDER(_nextServoIndex)) {
00074                         _nextServoIndex = 255;
00075                         // sleep for 20ms to the next servo pass
00076                         _timer->setPeriods(20000UL, 0UL, 0UL); // sleep for 20ms
00077                         _timer->enable();
00078                         return;
00079                 }
00080 
00081                 // Set up the next servo if it is sufficiently far away from the current one
00082                 if ((uint32_t)_controlBlocks[SERVO_ORDER(_nextServoIndex)].position >
00083                                 (uint32_t)_controlBlocks[SERVO_ORDER(_nextServoIndex-1)].position + _timer->current()) {
00084 
00085                         _timer->setPeriods(MHV_TIMER_PRESCALER_5_1,
00086                                         _controlBlocks[SERVO_ORDER(_nextServoIndex)].position - startPosition - _timer->current(), 0, 0);
00087                         _timer->enable();
00088                         return;
00089                 }
00090         }
00091 }
00092 
00099 MHV_ServoControl::MHV_ServoControl(MHV_Timer16 *timer, MHV_SERVOCONTROLBLOCK *controlBlocks, uint8_t count) {
00100         _timer = timer;
00101         _controlBlocks = controlBlocks;
00102         _count = count;
00103         _nextServoIndex = 255;
00104 
00105         uint8_t i;
00106         for (i = 0; i < count; i++) {
00107                 controlBlocks[i].clockMaxOffset = 0;
00108                 controlBlocks[i].clockMinOffset = 0;
00109                 controlBlocks[i].pin = 0;
00110                 controlBlocks[i].port = 0;
00111                 controlBlocks[i].position = 0;
00112                 controlBlocks[i].servoOrder = 255;
00113         }
00114 
00115         _timer->setMode(MHV_TIMER_ONE_SHOT);
00116 }
00117 
00123 void MHV_ServoControl::addServo(uint8_t servo, volatile uint8_t *pinDir, volatile uint8_t *pinOut, volatile uint8_t *pinIn, uint8_t pinBit, int8_t pinChangeInterrupt) {
00124         _controlBlocks[servo].pin = pinBit;
00125         _controlBlocks[servo].port = pinOut;
00126         _controlBlocks[servo].position = (MHV_SERVO_MIN + MHV_SERVO_MAX) / 2;
00127 
00128         mhv_setOutput(pinDir, pinOut, pinIn, pinBit, -1);
00129 
00130         if (_timer->enabled()) {
00131                 sortServos();
00132         }
00133 }
00134 
00140 void MHV_ServoControl::tweakServo(uint8_t servo, int8_t minOffset, int8_t maxOffset) {
00141         _controlBlocks[servo].clockMinOffset = minOffset * F_CPU / (2 * 1000000); // timer ticks per us
00142         _controlBlocks[servo].clockMaxOffset = maxOffset * F_CPU / (2 * 1000000);
00143 }
00144 
00150 void MHV_ServoControl::positionServo(uint8_t servo, uint16_t newPosition) {
00151         _controlBlocks[servo].position = ((uint32_t)newPosition) * (MHV_SERVO_MAX + _controlBlocks[servo].clockMaxOffset -
00152                         (MHV_SERVO_MIN + _controlBlocks[servo].clockMinOffset) ) / 65535 + MHV_SERVO_MIN + _controlBlocks[servo].clockMinOffset;
00153 
00154         if (_timer->enabled()) {
00155                 sortServos();
00156         }
00157 }
00158 
00163 bool MHV_ServoControl::canPosition() {
00164         return 255 == _nextServoIndex;
00165 }
00166 
00172 void MHV_ServoControl::positionServoBusyWait(uint8_t servo, uint16_t newPosition) {
00173         while (255 != _nextServoIndex) {}
00174         positionServo(servo, newPosition);
00175 }
00176 
00180 void MHV_ServoControl::enable() {
00181         uint8_t i;
00182         for (i = 0; i < _count; ++i) {
00183                 if (_controlBlocks[i].port) {
00184                         _controlBlocks[i].servoOrder = i;
00185                 } else {
00186                         _controlBlocks[i].servoOrder = 255;
00187                 }
00188         }
00189 
00190         _nextServoIndex = 0;
00191         sortServos();
00192         refreshServos(0);
00193 }
00194 
00198 void MHV_ServoControl::disable() {
00199         _timer->disable();
00200 }
00201 
00205 void MHV_ServoControl::sortServos() {
00206         uint8_t i;
00207         uint16_t curPosition;
00208 
00209         // We'll use a bubble sort here since the general case is that the list is sorted, with only 1 servo out of position
00210         for (i = 0; i < _count - 1; ++i) {
00211                 if ((curPosition = _controlBlocks[_controlBlocks[i].servoOrder].position) > _controlBlocks[_controlBlocks[i+1].servoOrder].position) {
00212                         _controlBlocks[_controlBlocks[i].servoOrder].position = _controlBlocks[_controlBlocks[i+1].servoOrder].position;
00213                         _controlBlocks[_controlBlocks[i+1].servoOrder].position = curPosition;
00214                         if (i > 1) {
00215                                 i -= 2;
00216                         } else {
00217                                 // We want i to wrap around to 0 on the next iteration
00218                                 i = -1;
00219                         }
00220                 }
00221         }
00222 }
00223 
00224 
00225 #endif // MHV_TIMER16_1