Merge pull request #18 from theroboticsheep/indigo-devel

Indigo devel
This commit is contained in:
pirobot 2015-04-22 06:34:41 -07:00
commit 9002150231
3 changed files with 140 additions and 22 deletions

View File

@ -45,8 +45,8 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#define USE_BASE // Enable the base controller code
//#undef USE_BASE // Disable the base controller code
//#define USE_BASE // Enable the base controller code
#undef USE_BASE // Disable the base controller code
/* Define the motor controller and encoder library you are using */
#ifdef USE_BASE
@ -63,8 +63,8 @@
//#define ARDUINO_ENC_COUNTER
#endif
//#define USE_SERVOS // Enable use of PWM servos as defined in servos.h
#undef USE_SERVOS // Disable use of PWM servos
#define USE_SERVOS // Enable use of PWM servos as defined in servos.h
//#undef USE_SERVOS // Disable use of PWM servos
/* Serial port baud rate */
#define BAUDRATE 57600
@ -184,11 +184,11 @@ int runCommand() {
break;
#ifdef USE_SERVOS
case SERVO_WRITE:
servos[arg1].write(arg2);
servos[arg1].setTargetPosition(arg2);
Serial.println("OK");
break;
case SERVO_READ:
Serial.println(servos[arg1].read());
Serial.println(servos[arg1].getServo().read());
break;
#endif
@ -265,12 +265,15 @@ void setup() {
#endif
/* Attach servos if used */
#ifdef USE_SERVOS
#ifdef USE_SERVOS
int i;
for (i = 0; i < N_SERVOS; i++) {
servos[i].attach(servoPins[i]);
servos[i].initServo(
servoPins[i],
stepDelay[i],
servoInitPosition[i]);
}
#endif
#endif
}
/* Enter the main loop. Read and parse input from the serial port
@ -330,12 +333,14 @@ void loop() {
setMotorSpeeds(0, 0);
moving = 0;
}
#endif
// Sweep servos
#ifdef USE_SERVOS
int i;
for (i = 0; i < N_SERVOS; i++) {
servos[i].doSweep();
}
#endif
}

View File

@ -1,9 +1,44 @@
/* Define the attachment of any servos here.
The example shows two servos attached on pins 3 and 5.
*/
#ifndef SERVOS_H
#define SERVOS_H
#define N_SERVOS 2
Servo servos [N_SERVOS];
byte servoPins [N_SERVOS] = {3, 5};
// This delay in milliseconds determines the pause
// 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

View File

@ -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