MHVLib
20111011
An efficiency oriented runtime library for AVR microcontrollers
|
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