yikestone/ros_arduino_bridge
1
0
Fork 0
mirror of https://github.com/YikeStone/ros_arduino_bridge.git synced 2025-08-05 20:14:07 +05:30
Code Issues Projects Releases Wiki Activity

Added check for duplicate servo pins in servo array

Browse Source
This commit is contained in:
Patrick Goebel 2015-12-15 06:45:31 -08:00
parent 932282526d
commit 4fd35aa2b5
2 changed files with 16 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino
View File

@ -194,8 +194,10 @@ int runCommand() {
#elif defined(USE_SERVOS2) #elif defined(USE_SERVOS2)
case CONFIG_SERVO: case CONFIG_SERVO:
myServos[arg1].initServo(arg1, arg2); myServos[arg1].initServo(arg1, arg2);
myServoPins[nServos] = arg1; if (!haveServo(arg1)) {
nServos++; myServoPins[nServos] = arg1;
nServos++;
}
Serial.println("OK"); Serial.println("OK");
break; break;
case SERVO_WRITE: case SERVO_WRITE:

14
ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos2.ino
View File

@ -18,7 +18,7 @@
// Constructor // Constructor
SweepServo2::SweepServo2() SweepServo2::SweepServo2()
{ {
this->currentPositionDegrees = 0; this->currentPositionDegrees = 90;
this->targetPositionDegrees = 90; this->targetPositionDegrees = 90;
this->lastSweepCommand = 0; this->lastSweepCommand = 0;
} }
@ -29,7 +29,7 @@ void SweepServo2::initServo(
int stepDelayMs) int stepDelayMs)
{ {
this->stepDelayMs = stepDelayMs; this->stepDelayMs = stepDelayMs;
this->currentPositionDegrees = 0; this->currentPositionDegrees = 90;
this->targetPositionDegrees = 90; this->targetPositionDegrees = 90;
this->lastSweepCommand = millis(); this->lastSweepCommand = millis();
this->servo.attach(servoPin); this->servo.attach(servoPin);
@ -74,11 +74,21 @@ void SweepServo2::setTargetPosition(int position)
this->targetPositionDegrees = position; this->targetPositionDegrees = position;
} }
// Get the current servo position
int SweepServo2::getCurrentPosition() int SweepServo2::getCurrentPosition()
{ {
return this->currentPositionDegrees; return this->currentPositionDegrees;
} }
// Check whether we have already configured this servo
bool haveServo(int pin) {
int i;
for (i = 0; i < nServos; i++) {
if (myServoPins[i] == pin) return true;
}
return false;
}
// Accessor for servo object // Accessor for servo object
Servo SweepServo2::getServo() Servo SweepServo2::getServo()
{ {