1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109
|
/**
* servoSD20.cpp
*/
#include <servoSD20.h>
#define SD20_I2C_ADRESS 0xC2
#define MOVEMENT_DELAY 60
/**
* Constructor
*/
servoSD20::servoSD20(const uint8_t id, const uint8_t leftPosition, const uint8_t rightPosition)
: m_id(id), m_leftPosition(leftPosition), m_rightPosition(rightPosition)
{
Wire.begin();
switchRight();
};
/**
* Switch to right position
*/
void servoSD20::switchRight()
{
if(!isRight())
{
gotoPosition(m_rightPosition);
servoSD20::ms_positionsRegister |= _BV(m_id);
servoSD20::ms_moved = true;
}
};
/**
* Switch to left position
*/
void servoSD20::switchLeft()
{
if(isRight())
{
gotoPosition(m_leftPosition);
servoSD20::ms_positionsRegister &= ~_BV(m_id);
servoSD20::ms_moved = true;
}
};
/**
* Switch positions : Left to right or right to left
*/
void servoSD20::switchPosition()
{
if(isRight())
{
switchLeft();
} else {
switchRight();
}
};
/**
* Init Wire/SD20 & static members
*/
void servoSD20::begin()
{
Wire.begin();
servoSD20::ms_positionsRegister = 0;
servoSD20::ms_moved = false;
};
/**
* Go to the specified position
*/
void servoSD20::gotoPosition(const uint8_t position)
{
Wire.beginTransmission(SD20_I2C_ADRESS);
Wire.write(m_id);
Wire.write(position);
Wire.endTransmission();
delayMicroseconds(MOVEMENT_DELAY);
};
/**
* Finding position
*/
void servoSD20::findPosition(uint8_t fromPosition, uint8_t toPosition, const uint8_t increment, const uint8_t waitingTime)
{
uint8_t ptr;
if(fromPosition > toPosition)
{
ptr = fromPosition;
fromPosition = toPosition;
toPosition = ptr;
}
Serial.println("Finding position : Start");
for(uint8_t ptr = fromPosition; ptr <= toPosition; ptr += increment)
{
Serial.print(" Pos:");
Serial.println(ptr);
gotoPosition(ptr);
delay(waitingTime);
}
Serial.println("Finding position : Stop");
}; |
Partager