|
| 1 | +/****************************************************************************** |
| 2 | + * The MIT License |
| 3 | + * |
| 4 | + * Copyright (c) 2010, LeafLabs, LLC. |
| 5 | + * |
| 6 | + * Permission is hereby granted, free of charge, to any person |
| 7 | + * obtaining a copy of this software and associated documentation |
| 8 | + * files (the "Software"), to deal in the Software without |
| 9 | + * restriction, including without limitation the rights to use, copy, |
| 10 | + * modify, merge, publish, distribute, sublicense, and/or sell copies |
| 11 | + * of the Software, and to permit persons to whom the Software is |
| 12 | + * furnished to do so, subject to the following conditions: |
| 13 | + * |
| 14 | + * The above copyright notice and this permission notice shall be |
| 15 | + * included in all copies or substantial portions of the Software. |
| 16 | + * |
| 17 | + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, |
| 18 | + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF |
| 19 | + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND |
| 20 | + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS |
| 21 | + * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN |
| 22 | + * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN |
| 23 | + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE |
| 24 | + * SOFTWARE. |
| 25 | + *****************************************************************************/ |
| 26 | + |
| 27 | + |
| 28 | +#include "CubeCellServoTimers.h" |
| 29 | +#include "Arduino.h" |
| 30 | +#include "math.h" |
| 31 | + |
| 32 | + |
| 33 | +#define CYCLES_PER_MICROSECOND 10000 |
| 34 | +#define MAX_OVERFLOW ((1 << 16) - 1) |
| 35 | +#define CYC_MSEC (1000 * CYCLES_PER_MICROSECOND) |
| 36 | +#define TAU_MSEC 22.1 |
| 37 | +#define TAU_USEC (TAU_MSEC * 1000) |
| 38 | +#define TAU_CYC (TAU_MSEC * CYC_MSEC) |
| 39 | +#define SERVO_PRESCALER (TAU_CYC / MAX_OVERFLOW + 1) |
| 40 | +#define SERVO_OVERFLOW ((uint16_t)round((double)TAU_CYC / SERVO_PRESCALER)) |
| 41 | + |
| 42 | +// Unit conversions |
| 43 | +#define US_TO_COMPARE(us) ((uint16_t)map((us), 0, TAU_USEC, 0, SERVO_OVERFLOW)) |
| 44 | +#define COMPARE_TO_US(c) ((uint32_t)map((c), 0, SERVO_OVERFLOW, 0, TAU_USEC)) |
| 45 | +#define ANGLE_TO_US(a) ((uint16_t)(map((a), this->minAngle, this->maxAngle, \ |
| 46 | + this->minPW, this->maxPW))) |
| 47 | +#define US_TO_ANGLE(us) ((int16_t)(map((us), this->minPW, this->maxPW, \ |
| 48 | + this->minAngle, this->maxAngle))) |
| 49 | + |
| 50 | +Servo::Servo() |
| 51 | +{ |
| 52 | + this->resetFields(); |
| 53 | +} |
| 54 | + |
| 55 | +bool Servo::attach(uint8_t pin, uint16_t minPW, uint16_t maxPW, int16_t minAngle, int16_t maxAngle) |
| 56 | +{ |
| 57 | + // put your setup code here, to run once: |
| 58 | + |
| 59 | + /*PWM CLK frequency default is 12M; |
| 60 | + *PWM CLK frequency value: |
| 61 | + *PWM_CLK_FREQ_48M |
| 62 | + *PWM_CLK_FREQ_24M |
| 63 | + *PWM_CLK_FREQ_16M |
| 64 | + *PWM_CLK_FREQ_12M |
| 65 | + *PWM_CLK_FREQ_8M |
| 66 | + *PWM_CLK_FREQ_6M |
| 67 | + *PWM_CLK_FREQ_4M |
| 68 | + *PWM_CLK_FREQ_3M |
| 69 | + *PWM_CLK_FREQ_2M |
| 70 | + *PWM_CLK_FREQ_1M |
| 71 | + */ |
| 72 | + setPWM_Frequency(PWM_CLK_FREQ_3M); |
| 73 | + |
| 74 | + //pwm period can be 0xFF~0xFFFF, default is 0xFFFF |
| 75 | + setPWM_ComparePeriod(0xFFFF); |
| 76 | + |
| 77 | + // pinMode(PWM1,OUTPUT); |
| 78 | + |
| 79 | + pinMode(pin, OUTPUT); |
| 80 | + |
| 81 | + if (this->attached()) { |
| 82 | + this->detach(); |
| 83 | + } |
| 84 | + |
| 85 | + this->pin = pin; |
| 86 | + this->minPW = (minPW); |
| 87 | + this->maxPW = (maxPW ); |
| 88 | + this->minAngle = minAngle; |
| 89 | + this->maxAngle = maxAngle; |
| 90 | + |
| 91 | + return true; |
| 92 | +} |
| 93 | + |
| 94 | +bool Servo::detach() { |
| 95 | + if (!this->attached()) { |
| 96 | + return false; |
| 97 | + } |
| 98 | + |
| 99 | + |
| 100 | + this->resetFields(); |
| 101 | + |
| 102 | + return true; |
| 103 | +} |
| 104 | + |
| 105 | +void Servo::write(int degrees) { |
| 106 | + degrees = constrain(degrees, this->minAngle, this->maxAngle); |
| 107 | + this->writeMicroseconds(ANGLE_TO_US(degrees)); |
| 108 | +} |
| 109 | + |
| 110 | +int Servo::read() const { |
| 111 | + int a = US_TO_ANGLE(this->readMicroseconds()); |
| 112 | + // map() round-trips in a weird way we mostly correct for here; |
| 113 | + // the round-trip is still sometimes off-by-one for write(1) and |
| 114 | + // write(179). |
| 115 | + return a == this->minAngle || a == this->maxAngle ? a : a + 1; |
| 116 | +} |
| 117 | + |
| 118 | +void Servo::writeMicroseconds(uint16_t pulseWidth) { |
| 119 | + if (!this->attached()) { |
| 120 | + // ASSERT(0); |
| 121 | + return; |
| 122 | + } |
| 123 | + pulseWidth = constrain(pulseWidth, this->minPW, this->maxPW); |
| 124 | + analogWrite(this->pin, US_TO_COMPARE(pulseWidth)); |
| 125 | +} |
| 126 | + |
| 127 | +uint16_t Servo::readMicroseconds() const { |
| 128 | + if (!this->attached()) { |
| 129 | + // ASSERT(0); |
| 130 | + return 0; |
| 131 | + } |
| 132 | + uint16_t compare =0; |
| 133 | + if(this->pin ==GPIO3) |
| 134 | + { |
| 135 | + compare =PWM2_ReadCompare(); |
| 136 | + } |
| 137 | + else if(this->pin ==GPIO2) |
| 138 | + { |
| 139 | + compare =PWM1_ReadCompare(); |
| 140 | + } |
| 141 | + else |
| 142 | + { |
| 143 | + return 0xFFFF; //error |
| 144 | + } |
| 145 | + |
| 146 | + return COMPARE_TO_US(compare); |
| 147 | +} |
| 148 | + |
| 149 | +void Servo::resetFields(void) { |
| 150 | + this->pin = NOT_ATTACHED; |
| 151 | + this->minAngle = MIN_ANGLE; |
| 152 | + this->maxAngle = MAX_ANGLE; |
| 153 | + this->minPW = MIN_PULSE_WIDTH; |
| 154 | + this->maxPW = MAX_PULSE_WIDTH; |
| 155 | +} |
| 156 | + |
| 157 | + |
0 commit comments