mirror of
https://github.com/YikeStone/ros_arduino_bridge.git
synced 2025-08-03 19:24:09 +05:30
commit
9002150231
@ -45,8 +45,8 @@
|
|||||||
* POSSIBILITY OF SUCH DAMAGE.
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
*********************************************************************/
|
*********************************************************************/
|
||||||
|
|
||||||
#define USE_BASE // Enable the base controller code
|
//#define USE_BASE // Enable the base controller code
|
||||||
//#undef USE_BASE // Disable the base controller code
|
#undef USE_BASE // Disable the base controller code
|
||||||
|
|
||||||
/* Define the motor controller and encoder library you are using */
|
/* Define the motor controller and encoder library you are using */
|
||||||
#ifdef USE_BASE
|
#ifdef USE_BASE
|
||||||
@ -63,8 +63,8 @@
|
|||||||
//#define ARDUINO_ENC_COUNTER
|
//#define ARDUINO_ENC_COUNTER
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
//#define USE_SERVOS // Enable use of PWM servos as defined in servos.h
|
#define USE_SERVOS // Enable use of PWM servos as defined in servos.h
|
||||||
#undef USE_SERVOS // Disable use of PWM servos
|
//#undef USE_SERVOS // Disable use of PWM servos
|
||||||
|
|
||||||
/* Serial port baud rate */
|
/* Serial port baud rate */
|
||||||
#define BAUDRATE 57600
|
#define BAUDRATE 57600
|
||||||
@ -184,11 +184,11 @@ int runCommand() {
|
|||||||
break;
|
break;
|
||||||
#ifdef USE_SERVOS
|
#ifdef USE_SERVOS
|
||||||
case SERVO_WRITE:
|
case SERVO_WRITE:
|
||||||
servos[arg1].write(arg2);
|
servos[arg1].setTargetPosition(arg2);
|
||||||
Serial.println("OK");
|
Serial.println("OK");
|
||||||
break;
|
break;
|
||||||
case SERVO_READ:
|
case SERVO_READ:
|
||||||
Serial.println(servos[arg1].read());
|
Serial.println(servos[arg1].getServo().read());
|
||||||
break;
|
break;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
@ -265,12 +265,15 @@ void setup() {
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
/* Attach servos if used */
|
/* Attach servos if used */
|
||||||
#ifdef USE_SERVOS
|
#ifdef USE_SERVOS
|
||||||
int i;
|
int i;
|
||||||
for (i = 0; i < N_SERVOS; i++) {
|
for (i = 0; i < N_SERVOS; i++) {
|
||||||
servos[i].attach(servoPins[i]);
|
servos[i].initServo(
|
||||||
}
|
servoPins[i],
|
||||||
#endif
|
stepDelay[i],
|
||||||
|
servoInitPosition[i]);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Enter the main loop. Read and parse input from the serial port
|
/* Enter the main loop. Read and parse input from the serial port
|
||||||
@ -330,12 +333,14 @@ void loop() {
|
|||||||
setMotorSpeeds(0, 0);
|
setMotorSpeeds(0, 0);
|
||||||
moving = 0;
|
moving = 0;
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Sweep servos
|
||||||
|
#ifdef USE_SERVOS
|
||||||
|
int i;
|
||||||
|
for (i = 0; i < N_SERVOS; i++) {
|
||||||
|
servos[i].doSweep();
|
||||||
|
}
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
@ -1,9 +1,44 @@
|
|||||||
/* Define the attachment of any servos here.
|
#ifndef SERVOS_H
|
||||||
The example shows two servos attached on pins 3 and 5.
|
#define SERVOS_H
|
||||||
*/
|
|
||||||
|
|
||||||
#define N_SERVOS 2
|
#define N_SERVOS 2
|
||||||
|
|
||||||
Servo servos [N_SERVOS];
|
// This delay in milliseconds determines the pause
|
||||||
byte servoPins [N_SERVOS] = {3, 5};
|
// between each one degree step the servo travels. Increasing
|
||||||
|
// this number will make the servo sweep more slowly.
|
||||||
|
// Decreasing this number will make the servo sweep more quickly.
|
||||||
|
// Zero is the default number and will make the servos spin at
|
||||||
|
// full speed. 150 ms makes them spin very slowly.
|
||||||
|
int stepDelay [N_SERVOS] = { 0, 0 }; // ms
|
||||||
|
|
||||||
|
// Pins
|
||||||
|
byte servoPins [N_SERVOS] = { 3, 4 };
|
||||||
|
|
||||||
|
// Initial Position
|
||||||
|
byte servoInitPosition [N_SERVOS] = { 90, 90 }; // [0, 180] degrees
|
||||||
|
|
||||||
|
|
||||||
|
class SweepServo
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
SweepServo();
|
||||||
|
void initServo(
|
||||||
|
int servoPin,
|
||||||
|
int stepDelayMs,
|
||||||
|
int initPosition);
|
||||||
|
void doSweep();
|
||||||
|
void setTargetPosition(int position);
|
||||||
|
Servo getServo();
|
||||||
|
|
||||||
|
private:
|
||||||
|
Servo servo;
|
||||||
|
int stepDelayMs;
|
||||||
|
int currentPositionDegrees;
|
||||||
|
int targetPositionDegrees;
|
||||||
|
long lastSweepCommand;
|
||||||
|
};
|
||||||
|
|
||||||
|
SweepServo servos [N_SERVOS];
|
||||||
|
|
||||||
|
#endif
|
||||||
|
@ -0,0 +1,78 @@
|
|||||||
|
/***************************************************************
|
||||||
|
Servo Sweep - by Nathaniel Gallinger
|
||||||
|
|
||||||
|
Sweep servos one degree step at a time with a user defined
|
||||||
|
delay in between steps. Supports changing direction
|
||||||
|
mid-sweep. Important for applications such as robotic arms
|
||||||
|
where the stock servo speed is too fast for the strength
|
||||||
|
of your system.
|
||||||
|
|
||||||
|
*************************************************************/
|
||||||
|
|
||||||
|
#ifdef USE_SERVOS
|
||||||
|
|
||||||
|
|
||||||
|
// Constructor
|
||||||
|
SweepServo::SweepServo()
|
||||||
|
{
|
||||||
|
this->currentPositionDegrees = 0;
|
||||||
|
this->targetPositionDegrees = 0;
|
||||||
|
this->lastSweepCommand = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Init
|
||||||
|
void SweepServo::initServo(
|
||||||
|
int servoPin,
|
||||||
|
int stepDelayMs,
|
||||||
|
int initPosition)
|
||||||
|
{
|
||||||
|
this->servo.attach(servoPin);
|
||||||
|
this->stepDelayMs = stepDelayMs;
|
||||||
|
this->currentPositionDegrees = initPosition;
|
||||||
|
this->targetPositionDegrees = initPosition;
|
||||||
|
this->lastSweepCommand = millis();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Perform Sweep
|
||||||
|
void SweepServo::doSweep()
|
||||||
|
{
|
||||||
|
|
||||||
|
// Get ellapsed time
|
||||||
|
int delta = millis() - this->lastSweepCommand;
|
||||||
|
|
||||||
|
// Check if time for a step
|
||||||
|
if (delta > this->stepDelayMs) {
|
||||||
|
// Check step direction
|
||||||
|
if (this->targetPositionDegrees > this->currentPositionDegrees) {
|
||||||
|
this->currentPositionDegrees++;
|
||||||
|
this->servo.write(this->currentPositionDegrees);
|
||||||
|
}
|
||||||
|
else if (this->targetPositionDegrees < this->currentPositionDegrees) {
|
||||||
|
this->currentPositionDegrees--;
|
||||||
|
this->servo.write(this->currentPositionDegrees);
|
||||||
|
}
|
||||||
|
// if target == current position, do nothing
|
||||||
|
|
||||||
|
// reset timer
|
||||||
|
this->lastSweepCommand = millis();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Set a new target position
|
||||||
|
void SweepServo::setTargetPosition(int position)
|
||||||
|
{
|
||||||
|
this->targetPositionDegrees = position;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Accessor for servo object
|
||||||
|
Servo SweepServo::getServo()
|
||||||
|
{
|
||||||
|
return this->servo;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
#endif
|
Loading…
x
Reference in New Issue
Block a user