mirror of
https://github.com/YikeStone/ros_arduino_bridge.git
synced 2025-08-03 19:24:09 +05:30
Added support for RoboGaia 3-axis encoder shield
This commit is contained in:
Patrick Goebel
parent
b8b51d4714
commit
5933239b70
@ -22,7 +22,7 @@ Overview
|
||||
--------
|
||||
This branch (indigo-devel) is intended for ROS Indigo and above, and uses the Catkin buildsystem. It may also be compatible with ROS Hydro.
|
||||
|
||||
This ROS stack includes an Arduino library (called ROSArduinoBridge) and a collection of ROS packages for controlling an Arduino-based robot using standard ROS messages and services. The stack does **not** depend on ROS Serial.
|
||||
This ROS metapackage includes an Arduino library (called ROSArduinoBridge) and a collection of ROS packages for controlling an Arduino-based robot using standard ROS messages and services. The stack does **not** depend on ROS Serial.
|
||||
|
||||
Features of the stack include:
|
||||
|
||||
@ -43,7 +43,9 @@ the PC. The base controller requires the use of a motor controller and encoders
|
||||
* Pololu VNH5019 dual motor controller shield (http://www.pololu.com/catalog/product/2502) or Pololu MC33926 dual motor shield (http://www.pololu.com/catalog/product/2503).
|
||||
|
||||
* Robogaia Mega Encoder shield
|
||||
(http://www.robogaia.com/two-axis-encoder-counter-mega-shield-version-2.html) or on-board wheel encoder counters.
|
||||
(http://www.robogaia.com/two-axis-encoder-counter-mega-shield-version-2.html)
|
||||
|
||||
* Instead of the Encoder shield, wheel encoders can be [connected directly](#using-the-on-board-wheel-encoder-counters-arduino-uno-only) if using an Arduino Uno
|
||||
|
||||
**NOTE:** The Robogaia Mega Encoder shield can only be used with an Arduino Mega. The on-board wheel encoder counters are currently only supported by Arduino Uno.
|
||||
|
||||
@ -91,7 +93,7 @@ http://www.robogaia.com/uploads/6/8/0/9/6809982/__megaencodercounter-1.3.tar.gz
|
||||
These libraries should be installed in your standard Arduino
|
||||
sketchbook/libraries directory.
|
||||
|
||||
Finally, it is assumed you are using version 1.0 or greater of the
|
||||
Finally, it is assumed you are using version 1.6.6 or greater of the
|
||||
Arduino IDE.
|
||||
|
||||
Preparing your Serial Port under Linux
|
||||
|
||||
@ -1,341 +0,0 @@
|
||||
/*********************************************************************
|
||||
* ROSArduinoBridge
|
||||
|
||||
A set of simple serial commands to control a differential drive
|
||||
robot and receive back sensor and odometry data. Default
|
||||
configuration assumes use of an Arduino Mega + Pololu motor
|
||||
controller shield + Robogaia Mega Encoder shield. Edit the
|
||||
readEncoder() and setMotorSpeed() wrapper functions if using
|
||||
different motor controller or encoder method.
|
||||
|
||||
Created for the Pi Robot Project: http://www.pirobot.org
|
||||
|
||||
Inspired and modeled after the ArbotiX driver by Michael Ferguson
|
||||
|
||||
Software License Agreement (BSD License)
|
||||
|
||||
Copyright (c) 2012, Patrick Goebel.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions
|
||||
are met:
|
||||
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above
|
||||
copyright notice, this list of conditions and the following
|
||||
disclaimer in the documentation and/or other materials provided
|
||||
with the distribution.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
#define USE_BASE // Enable the base controller code
|
||||
//#undef USE_BASE // Disable the base controller code
|
||||
|
||||
//#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
|
||||
|
||||
/* Maximum PWM signal */
|
||||
#define MAX_PWM 255
|
||||
|
||||
#if defined(ARDUINO) && ARDUINO >= 100
|
||||
#include "Arduino.h"
|
||||
#else
|
||||
#include "WProgram.h"
|
||||
#endif
|
||||
|
||||
/* Include definition of serial commands */
|
||||
#include "commands.h"
|
||||
|
||||
/* Sensor functions */
|
||||
#include "sensors.h"
|
||||
|
||||
/* Include servo support if required */
|
||||
#ifdef USE_SERVOS
|
||||
#include <Servo.h>
|
||||
#include "servos.h"
|
||||
#endif
|
||||
|
||||
#ifdef USE_BASE
|
||||
/* The Pololu motor driver shield */
|
||||
#include "DualVNH5019MotorShield.h"
|
||||
|
||||
/* The Robogaia Mega Encoder shield */
|
||||
#include "MegaEncoderCounter.h"
|
||||
|
||||
/* Create the motor driver object */
|
||||
DualVNH5019MotorShield drive;
|
||||
|
||||
/* Create the encoder shield object */
|
||||
MegaEncoderCounter encoders(4); // Initializes the Mega Encoder Counter in the 4X Count mode
|
||||
|
||||
/* PID parameters and functions */
|
||||
#include "diff_controller.h"
|
||||
|
||||
/* Run the PID loop at 30 times per second */
|
||||
#define PID_RATE 30 // Hz
|
||||
|
||||
/* Convert the rate into an interval */
|
||||
const int PID_INTERVAL = 1000 / PID_RATE;
|
||||
|
||||
/* Track the next time we make a PID calculation */
|
||||
unsigned long nextPID = PID_INTERVAL;
|
||||
|
||||
/* Stop the robot if it hasn't received a movement command
|
||||
in this number of milliseconds */
|
||||
#define AUTO_STOP_INTERVAL 2000
|
||||
long lastMotorCommand = AUTO_STOP_INTERVAL;
|
||||
|
||||
/* Wrap the encoder reading function */
|
||||
long readEncoder(int i) {
|
||||
if (i == LEFT) return encoders.YAxisGetCount();
|
||||
else return encoders.XAxisGetCount();
|
||||
}
|
||||
|
||||
/* Wrap the encoder reset function */
|
||||
void resetEncoder(int i) {
|
||||
if (i == LEFT) return encoders.YAxisReset();
|
||||
else return encoders.XAxisReset();
|
||||
}
|
||||
|
||||
/* Wrap the encoder reset function */
|
||||
void resetEncoders() {
|
||||
resetEncoder(LEFT);
|
||||
resetEncoder(RIGHT);
|
||||
}
|
||||
|
||||
/* Wrap the motor driver initialization */
|
||||
void initMotorController() {
|
||||
drive.init();
|
||||
}
|
||||
|
||||
/* Wrap the drive motor set speed function */
|
||||
void setMotorSpeed(int i, int spd) {
|
||||
if (i == LEFT) drive.setM1Speed(spd);
|
||||
else drive.setM2Speed(spd);
|
||||
}
|
||||
|
||||
// A convenience function for setting both motor speeds
|
||||
void setMotorSpeeds(int leftSpeed, int rightSpeed) {
|
||||
setMotorSpeed(LEFT, leftSpeed);
|
||||
setMotorSpeed(RIGHT, rightSpeed);
|
||||
}
|
||||
#endif
|
||||
|
||||
/* Variable initialization */
|
||||
|
||||
// A pair of varibles to help parse serial commands (thanks Fergs)
|
||||
int arg = 0;
|
||||
int index = 0;
|
||||
|
||||
// Variable to hold an input character
|
||||
char chr;
|
||||
|
||||
// Variable to hold the current single-character command
|
||||
char cmd;
|
||||
|
||||
// Character arrays to hold the first and second arguments
|
||||
char argv1[16];
|
||||
char argv2[16];
|
||||
|
||||
// The arguments converted to integers
|
||||
long arg1;
|
||||
long arg2;
|
||||
|
||||
/* Clear the current command parameters */
|
||||
void resetCommand() {
|
||||
cmd = NULL;
|
||||
memset(argv1, 0, sizeof(argv1));
|
||||
memset(argv2, 0, sizeof(argv2));
|
||||
arg1 = 0;
|
||||
arg2 = 0;
|
||||
arg = 0;
|
||||
index = 0;
|
||||
}
|
||||
|
||||
/* Run a command. Commands are defined in commands.h */
|
||||
int runCommand() {
|
||||
int i = 0;
|
||||
char *p = argv1;
|
||||
char *str;
|
||||
int pid_args[4];
|
||||
arg1 = atoi(argv1);
|
||||
arg2 = atoi(argv2);
|
||||
|
||||
switch(cmd) {
|
||||
case GET_BAUDRATE:
|
||||
Serial.println(BAUDRATE);
|
||||
break;
|
||||
case ANALOG_READ:
|
||||
Serial.println(analogRead(arg1));
|
||||
break;
|
||||
case DIGITAL_READ:
|
||||
Serial.println(digitalRead(arg1));
|
||||
break;
|
||||
case ANALOG_WRITE:
|
||||
analogWrite(arg1, arg2);
|
||||
Serial.println("OK");
|
||||
break;
|
||||
case DIGITAL_WRITE:
|
||||
if (arg2 == 0) digitalWrite(arg1, LOW);
|
||||
else if (arg2 == 1) digitalWrite(arg1, HIGH);
|
||||
Serial.println("OK");
|
||||
break;
|
||||
case PIN_MODE:
|
||||
if (arg2 == 0) pinMode(arg1, INPUT);
|
||||
else if (arg2 == 1) pinMode(arg1, OUTPUT);
|
||||
Serial.println("OK");
|
||||
break;
|
||||
case PING:
|
||||
Serial.println(Ping(arg1));
|
||||
break;
|
||||
#ifdef USE_SERVOS
|
||||
case SERVO_WRITE:
|
||||
servos[arg1].write(arg2);
|
||||
Serial.println("OK");
|
||||
break;
|
||||
case SERVO_READ:
|
||||
Serial.println(servos[arg1].read());
|
||||
break;
|
||||
#endif
|
||||
|
||||
#ifdef USE_BASE
|
||||
case READ_ENCODERS:
|
||||
Serial.print(readEncoder(LEFT));
|
||||
Serial.print(" ");
|
||||
Serial.println(readEncoder(RIGHT));
|
||||
break;
|
||||
case RESET_ENCODERS:
|
||||
resetEncoders();
|
||||
Serial.println("OK");
|
||||
break;
|
||||
case MOTOR_SPEEDS:
|
||||
/* Reset the auto stop timer */
|
||||
lastMotorCommand = millis();
|
||||
if (arg1 == 0 && arg2 == 0) {
|
||||
setMotorSpeeds(0, 0);
|
||||
moving = 0;
|
||||
}
|
||||
else moving = 1;
|
||||
leftPID.TargetTicksPerFrame = arg1;
|
||||
rightPID.TargetTicksPerFrame = arg2;
|
||||
Serial.println("OK");
|
||||
break;
|
||||
case UPDATE_PID:
|
||||
while ((str = strtok_r(p, ":", &p)) != '\0') {
|
||||
pid_args[i] = atoi(str);
|
||||
i++;
|
||||
}
|
||||
Kp = pid_args[0];
|
||||
Kd = pid_args[1];
|
||||
Ki = pid_args[2];
|
||||
Ko = pid_args[3];
|
||||
Serial.println("OK");
|
||||
break;
|
||||
#endif
|
||||
default:
|
||||
Serial.println("Invalid Command");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* Setup function--runs once at startup. */
|
||||
void setup() {
|
||||
Serial.begin(BAUDRATE);
|
||||
|
||||
// Initialize the motor controller if used */
|
||||
#ifdef USE_BASE
|
||||
initMotorController();
|
||||
#endif
|
||||
|
||||
/* Attach servos if used */
|
||||
#ifdef USE_SERVOS
|
||||
int i;
|
||||
for (i = 0; i < N_SERVOS; i++) {
|
||||
servos[i].attach(servoPins[i]);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
/* Enter the main loop. Read and parse input from the serial port
|
||||
and run any valid commands. Run a PID calculation at the target
|
||||
interval and check for auto-stop conditions.
|
||||
*/
|
||||
void loop() {
|
||||
while (Serial.available() > 0) {
|
||||
|
||||
// Read the next character
|
||||
chr = Serial.read();
|
||||
|
||||
// Terminate a command with a CR
|
||||
if (chr == 13) {
|
||||
if (arg == 1) argv1[index] = NULL;
|
||||
else if (arg == 2) argv2[index] = NULL;
|
||||
runCommand();
|
||||
resetCommand();
|
||||
}
|
||||
// Use spaces to delimit parts of the command
|
||||
else if (chr == ' ') {
|
||||
// Step through the arguments
|
||||
if (arg == 0) arg = 1;
|
||||
else if (arg == 1) {
|
||||
argv1[index] = NULL;
|
||||
arg = 2;
|
||||
index = 0;
|
||||
}
|
||||
continue;
|
||||
}
|
||||
else {
|
||||
if (arg == 0) {
|
||||
// The first arg is the single-letter command
|
||||
cmd = chr;
|
||||
}
|
||||
else if (arg == 1) {
|
||||
// Subsequent arguments can be more than one character
|
||||
argv1[index] = chr;
|
||||
index++;
|
||||
}
|
||||
else if (arg == 2) {
|
||||
argv2[index] = chr;
|
||||
index++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// If we are using base control, run a PID calculation at the appropriate intervals
|
||||
#ifdef USE_BASE
|
||||
if (millis() > nextPID) {
|
||||
updatePID();
|
||||
nextPID += PID_INTERVAL;
|
||||
}
|
||||
|
||||
// Check to see if we have exceeded the auto-stop interval
|
||||
if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {;
|
||||
setMotorSpeeds(0, 0);
|
||||
moving = 0;
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -1,22 +0,0 @@
|
||||
/* Define single-letter commands that will be sent by the PC over the
|
||||
serial link.
|
||||
*/
|
||||
|
||||
#define ANALOG_READ 'a'
|
||||
#define GET_BAUDRATE 'b'
|
||||
#define PIN_MODE 'c'
|
||||
#define DIGITAL_READ 'd'
|
||||
#define READ_ENCODERS 'e'
|
||||
#define MOTOR_SPEEDS 'm'
|
||||
#define PING 'p'
|
||||
#define RESET_ENCODERS 'r'
|
||||
#define SERVO_WRITE 's'
|
||||
#define SERVO_READ 't'
|
||||
#define UPDATE_PID 'u'
|
||||
#define DIGITAL_WRITE 'w'
|
||||
#define ANALOG_WRITE 'x'
|
||||
#define LEFT 0
|
||||
#define RIGHT 1
|
||||
|
||||
|
||||
|
||||
@ -1,71 +0,0 @@
|
||||
/* Functions and type-defs for PID control.
|
||||
|
||||
Taken mostly from Mike Ferguson's ArbotiX code which lives at:
|
||||
|
||||
http://vanadium-ros-pkg.googlecode.com/svn/trunk/arbotix/
|
||||
*/
|
||||
|
||||
/* PID setpoint info For a Motor */
|
||||
typedef struct {
|
||||
double TargetTicksPerFrame; // target speed in ticks per frame
|
||||
long Encoder; // encoder count
|
||||
long PrevEnc; // last encoder count
|
||||
int PrevErr; // last error
|
||||
int Ierror; // integrated error
|
||||
int output; // last motor setting
|
||||
}
|
||||
SetPointInfo;
|
||||
|
||||
SetPointInfo leftPID, rightPID;
|
||||
|
||||
/* PID Parameters */
|
||||
int Kp = 20;
|
||||
int Kd = 12;
|
||||
int Ki = 0;
|
||||
int Ko = 50;
|
||||
|
||||
unsigned char moving = 0; // is the base in motion?
|
||||
|
||||
/* PID routine to compute the next motor commands */
|
||||
void doPID(SetPointInfo * p) {
|
||||
long Perror;
|
||||
long output;
|
||||
|
||||
Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc);
|
||||
|
||||
// Derivative error is the delta Perror
|
||||
output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
|
||||
p->PrevErr = Perror;
|
||||
p->PrevEnc = p->Encoder;
|
||||
|
||||
output += p->output;
|
||||
// Accumulate Integral error *or* Limit output.
|
||||
// Stop accumulating when output saturates
|
||||
if (output >= MAX_PWM)
|
||||
output = MAX_PWM;
|
||||
else if (output <= -MAX_PWM)
|
||||
output = -MAX_PWM;
|
||||
else
|
||||
p->Ierror += Perror;
|
||||
|
||||
p->output = output;
|
||||
}
|
||||
|
||||
/* Read the encoder values and call the PID routine */
|
||||
void updatePID() {
|
||||
/* Read the encoders */
|
||||
leftPID.Encoder = readEncoder(0);
|
||||
rightPID.Encoder = readEncoder(1);
|
||||
|
||||
/* If we're not moving there is nothing more to do */
|
||||
if (!moving)
|
||||
return;
|
||||
|
||||
/* Compute PID update for each motor */
|
||||
doPID(&rightPID);
|
||||
doPID(&leftPID);
|
||||
|
||||
/* Set the motor speeds accordingly */
|
||||
setMotorSpeeds(leftPID.output, rightPID.output);
|
||||
}
|
||||
|
||||
@ -1,34 +0,0 @@
|
||||
/* Functions for various sensor types */
|
||||
|
||||
float microsecondsToCm(long microseconds)
|
||||
{
|
||||
// The speed of sound is 340 m/s or 29 microseconds per cm.
|
||||
// The ping travels out and back, so to find the distance of the
|
||||
// object we take half of the distance travelled.
|
||||
return microseconds / 29 / 2;
|
||||
}
|
||||
|
||||
long Ping(int pin) {
|
||||
long duration, range;
|
||||
|
||||
// The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
|
||||
// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
|
||||
pinMode(pin, OUTPUT);
|
||||
digitalWrite(pin, LOW);
|
||||
delayMicroseconds(2);
|
||||
digitalWrite(pin, HIGH);
|
||||
delayMicroseconds(5);
|
||||
digitalWrite(pin, LOW);
|
||||
|
||||
// The same pin is used to read the signal from the PING))): a HIGH
|
||||
// pulse whose duration is the time (in microseconds) from the sending
|
||||
// of the ping to the reception of its echo off of an object.
|
||||
pinMode(pin, INPUT);
|
||||
duration = pulseIn(pin, HIGH);
|
||||
|
||||
// convert the time into meters
|
||||
range = microsecondsToCm(duration);
|
||||
|
||||
return(range);
|
||||
}
|
||||
|
||||
@ -1,16 +0,0 @@
|
||||
/* Define the attachment of any servos here.
|
||||
The example shows two servos attached on pins 3 and 5.
|
||||
*/
|
||||
|
||||
#define N_SERVOS 2
|
||||
|
||||
Servo servos [N_SERVOS];
|
||||
byte servoPins [N_SERVOS] = {3, 5};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -50,13 +50,22 @@
|
||||
/* Define the motor controller and encoder library you are using */
|
||||
#ifdef USE_BASE
|
||||
/* The Pololu VNH5019 dual motor driver shield */
|
||||
#define POLOLU_VNH5019
|
||||
//#define POLOLU_VNH5019
|
||||
|
||||
/* The Pololu MC33926 dual motor driver shield */
|
||||
//#define POLOLU_MC33926
|
||||
|
||||
/* The Ardunino Motor Shield R3 */
|
||||
//#define ARDUINO_MOTOR_SHIELD_R3
|
||||
|
||||
/* For testing only */
|
||||
//#define NO_MOTOR_CONTROLLER
|
||||
|
||||
/* The RoboGaia encoder shield */
|
||||
#define ROBOGAIA
|
||||
//#define ROBOGAIA
|
||||
|
||||
/* The RoboGaia 3-axis encoder shield */
|
||||
//#define ROBOGAIA_3_AXIS
|
||||
|
||||
/* Encoders directly attached to Arduino board */
|
||||
//#define ARDUINO_ENC_COUNTER
|
||||
@ -94,6 +103,11 @@
|
||||
/* Motor driver function definitions */
|
||||
#include "motor_driver.h"
|
||||
|
||||
#ifdef ROBOGAIA_3_AXIS
|
||||
// The sensor communicates using SPI, so include the library:
|
||||
#include <SPI.h>
|
||||
#endif
|
||||
|
||||
/* Encoder driver function definitions */
|
||||
#include "encoder_driver.h"
|
||||
|
||||
@ -193,15 +207,18 @@ int runCommand() {
|
||||
break;
|
||||
#elif defined(USE_SERVOS2)
|
||||
case CONFIG_SERVO:
|
||||
myServos[arg1].initServo(arg1, arg2);
|
||||
if (!haveServo(arg1)) {
|
||||
myServos[arg1].initServo(arg1, arg2);
|
||||
myServoPins[nServos] = arg1;
|
||||
myServos[arg1].enable();
|
||||
nServos++;
|
||||
}
|
||||
Serial.println("OK");
|
||||
break;
|
||||
case SERVO_WRITE:
|
||||
myServos[arg1].setTargetPosition(arg2);
|
||||
if (myServos[arg1].isEnabled()) {
|
||||
myServos[arg1].setTargetPosition(arg2);
|
||||
}
|
||||
Serial.println("OK");
|
||||
break;
|
||||
case SERVO_READ:
|
||||
@ -213,10 +230,19 @@ int runCommand() {
|
||||
break;
|
||||
case DETACH_SERVO:
|
||||
myServos[arg1].getServo().detach();
|
||||
myServos[arg1].disable();
|
||||
Serial.println("OK");
|
||||
break;
|
||||
case ATTACH_SERVO:
|
||||
myServos[arg1].getServo().attach(arg1);
|
||||
if (!haveServo(arg1)) {
|
||||
myServos[arg1].initServo(arg1, 0);
|
||||
myServoPins[nServos] = arg1;
|
||||
nServos++;
|
||||
}
|
||||
else {
|
||||
myServos[arg1].getServo().attach(arg1);
|
||||
}
|
||||
myServos[arg1].enable();
|
||||
Serial.println("OK");
|
||||
break;
|
||||
|
||||
@ -270,27 +296,8 @@ void setup() {
|
||||
|
||||
// Initialize the motor controller if used */
|
||||
#ifdef USE_BASE
|
||||
#ifdef ARDUINO_ENC_COUNTER
|
||||
//set as inputs
|
||||
DDRD &= ~(1<<LEFT_ENC_PIN_A);
|
||||
DDRD &= ~(1<<LEFT_ENC_PIN_B);
|
||||
DDRC &= ~(1<<RIGHT_ENC_PIN_A);
|
||||
DDRC &= ~(1<<RIGHT_ENC_PIN_B);
|
||||
|
||||
// Enable pull up resistors
|
||||
PORTD |= (1<<LEFT_ENC_PIN_A);
|
||||
PORTD |= (1<<LEFT_ENC_PIN_B);
|
||||
PORTC |= (1<<RIGHT_ENC_PIN_A);
|
||||
PORTC |= (1<<RIGHT_ENC_PIN_B);
|
||||
|
||||
// Tell pin change mask to listen to left encoder pins
|
||||
PCMSK2 |= (1 << LEFT_ENC_PIN_A)|(1 << LEFT_ENC_PIN_B);
|
||||
// Tell pin change mask to listen to right encoder pins
|
||||
PCMSK1 |= (1 << RIGHT_ENC_PIN_A)|(1 << RIGHT_ENC_PIN_B);
|
||||
|
||||
// Enable PCINT1 and PCINT2 interrupt in the general interrupt mask
|
||||
PCICR |= (1 << PCIE1) | (1 << PCIE2);
|
||||
#endif
|
||||
/* Initialize the encoder interface */
|
||||
initEncoders();
|
||||
initMotorController();
|
||||
resetPID();
|
||||
#endif
|
||||
|
||||
@ -11,10 +11,21 @@
|
||||
|
||||
//below can be changed, but should be PORTC pins
|
||||
#define RIGHT_ENC_PIN_A PC4 //pin A4
|
||||
#define RIGHT_ENC_PIN_B PC5 //pin A5
|
||||
#define RIGHT_ENC_PIN_B PC5 //pin A5
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef ROBOGAIA_3_AXIS
|
||||
#define chipSelectPin1 10
|
||||
#define chipSelectPin2 9
|
||||
#define chipSelectPin3 8
|
||||
|
||||
#define CNTR B00100000
|
||||
#define CLR B00000000
|
||||
#endif
|
||||
|
||||
void initEncoders();
|
||||
long readEncoder(int i);
|
||||
void resetEncoder(int i);
|
||||
void resetEncoders();
|
||||
|
||||
|
||||
|
||||
@ -22,11 +22,149 @@
|
||||
else return encoders.XAxisGetCount();
|
||||
}
|
||||
|
||||
/* Wrap the encoder initializeing to be run in the main setup() function */
|
||||
void initEncoders() {
|
||||
/* Nothing to do here */
|
||||
}
|
||||
|
||||
/* Wrap the encoder reset function */
|
||||
void resetEncoder(int i) {
|
||||
if (i == LEFT) return encoders.YAxisReset();
|
||||
else return encoders.XAxisReset();
|
||||
}
|
||||
#elif defined(ROBOGAIA_3_AXIS)
|
||||
/* Robogaia 3-axis encoder shield */
|
||||
|
||||
//*****************************************************
|
||||
void initEncoders()
|
||||
//*****************************************************
|
||||
{
|
||||
pinMode(chipSelectPin1, OUTPUT);
|
||||
pinMode(chipSelectPin2, OUTPUT);
|
||||
pinMode(chipSelectPin3, OUTPUT);
|
||||
|
||||
digitalWrite(chipSelectPin1, HIGH);
|
||||
digitalWrite(chipSelectPin2, HIGH);
|
||||
digitalWrite(chipSelectPin3, HIGH);
|
||||
|
||||
LS7366_Init();
|
||||
|
||||
delay(100);
|
||||
}
|
||||
|
||||
long readEncoder(int encoder)
|
||||
//*****************************************************
|
||||
{
|
||||
unsigned int count1Value, count2Value, count3Value, count4Value;
|
||||
long result;
|
||||
|
||||
/* The 3-axis encoder uses a 1-based index */
|
||||
if (encoder == 2) encoder = 3;
|
||||
if (encoder == 1) encoder = 2;
|
||||
if (encoder == 0) encoder = 1;
|
||||
|
||||
selectEncoder(encoder);
|
||||
|
||||
SPI.transfer(0x60); // Request count
|
||||
count1Value = SPI.transfer(0x00); // Read highest order byte
|
||||
count2Value = SPI.transfer(0x00);
|
||||
count3Value = SPI.transfer(0x00);
|
||||
count4Value = SPI.transfer(0x00); // Read lowest order byte
|
||||
|
||||
deselectEncoder(encoder);
|
||||
|
||||
result= ((long)count1Value<<24) + ((long)count2Value<<16) + ((long)count3Value<<8) + (long)count4Value;
|
||||
|
||||
return result;
|
||||
}//end func
|
||||
|
||||
//*************************************************
|
||||
void selectEncoder(int encoder)
|
||||
//*************************************************
|
||||
{
|
||||
switch(encoder)
|
||||
{
|
||||
case 1:
|
||||
digitalWrite(chipSelectPin1,LOW);
|
||||
break;
|
||||
case 2:
|
||||
digitalWrite(chipSelectPin2,LOW);
|
||||
break;
|
||||
case 3:
|
||||
digitalWrite(chipSelectPin3,LOW);
|
||||
break;
|
||||
}//end switch
|
||||
|
||||
}//end func
|
||||
|
||||
//*************************************************
|
||||
void deselectEncoder(int encoder)
|
||||
//*************************************************
|
||||
{
|
||||
switch(encoder)
|
||||
{
|
||||
case 1:
|
||||
digitalWrite(chipSelectPin1,HIGH);
|
||||
break;
|
||||
case 2:
|
||||
digitalWrite(chipSelectPin2,HIGH);
|
||||
break;
|
||||
case 3:
|
||||
digitalWrite(chipSelectPin3,HIGH);
|
||||
break;
|
||||
}//end switch
|
||||
|
||||
}//end func
|
||||
|
||||
//*************************************************
|
||||
void resetEncoder(int encoder)
|
||||
//*************************************************
|
||||
{
|
||||
/* The 3-axis encoder uses a 1-based index */
|
||||
if (encoder == 2) encoder = 3;
|
||||
if (encoder == 1) encoder = 2;
|
||||
if (encoder == 0) encoder = 1;
|
||||
|
||||
selectEncoder(encoder);
|
||||
SPI.transfer(CLR | CNTR);
|
||||
deselectEncoder(encoder);
|
||||
}
|
||||
|
||||
/* Wrap the encoder reset function */
|
||||
void resetEncoders() {
|
||||
resetEncoder(LEFT);
|
||||
resetEncoder(RIGHT);
|
||||
}
|
||||
|
||||
// LS7366 Initialization and configuration
|
||||
//*************************************************
|
||||
void LS7366_Init(void)
|
||||
//*************************************************
|
||||
{
|
||||
// SPI initialization
|
||||
SPI.begin();
|
||||
SPI.setClockDivider(SPI_CLOCK_DIV16); // SPI at 1Mhz (on 16Mhz clock)
|
||||
delay(10);
|
||||
|
||||
digitalWrite(chipSelectPin1,LOW);
|
||||
SPI.transfer(0x88);
|
||||
SPI.transfer(0x03);
|
||||
digitalWrite(chipSelectPin1,HIGH);
|
||||
|
||||
|
||||
digitalWrite(chipSelectPin2,LOW);
|
||||
SPI.transfer(0x88);
|
||||
SPI.transfer(0x03);
|
||||
digitalWrite(chipSelectPin2,HIGH);
|
||||
|
||||
|
||||
digitalWrite(chipSelectPin3,LOW);
|
||||
SPI.transfer(0x88);
|
||||
SPI.transfer(0x03);
|
||||
digitalWrite(chipSelectPin3,HIGH);
|
||||
|
||||
}//end func
|
||||
|
||||
#elif defined(ARDUINO_ENC_COUNTER)
|
||||
volatile long left_enc_pos = 0L;
|
||||
volatile long right_enc_pos = 0L;
|
||||
@ -51,6 +189,29 @@
|
||||
|
||||
right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
|
||||
}
|
||||
|
||||
/* Wrap the encoder initializeing to be run in the main setup() function */
|
||||
void initEncoders() {
|
||||
//set as inputs
|
||||
DDRD &= ~(1<<LEFT_ENC_PIN_A);
|
||||
DDRD &= ~(1<<LEFT_ENC_PIN_B);
|
||||
DDRC &= ~(1<<RIGHT_ENC_PIN_A);
|
||||
DDRC &= ~(1<<RIGHT_ENC_PIN_B);
|
||||
|
||||
// Enable pull up resistors
|
||||
PORTD |= (1<<LEFT_ENC_PIN_A);
|
||||
PORTD |= (1<<LEFT_ENC_PIN_B);
|
||||
PORTC |= (1<<RIGHT_ENC_PIN_A);
|
||||
PORTC |= (1<<RIGHT_ENC_PIN_B);
|
||||
|
||||
// Tell pin change mask to listen to left encoder pins
|
||||
PCMSK2 |= (1 << LEFT_ENC_PIN_A)|(1 << LEFT_ENC_PIN_B);
|
||||
// Tell pin change mask to listen to right encoder pins
|
||||
PCMSK1 |= (1 << RIGHT_ENC_PIN_A)|(1 << RIGHT_ENC_PIN_B);
|
||||
|
||||
// Enable PCINT1 and PCINT2 interrupt in the general interrupt mask
|
||||
PCICR |= (1 << PCIE1) | (1 << PCIE2);
|
||||
}
|
||||
|
||||
/* Wrap the encoder reading function */
|
||||
long readEncoder(int i) {
|
||||
@ -68,15 +229,16 @@
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
/* Wrap the encoder reset function */
|
||||
void resetEncoders() {
|
||||
resetEncoder(LEFT);
|
||||
resetEncoder(RIGHT);
|
||||
}
|
||||
#else
|
||||
#error A encoder driver must be selected!
|
||||
#endif
|
||||
|
||||
/* Wrap the encoder reset function */
|
||||
void resetEncoders() {
|
||||
resetEncoder(LEFT);
|
||||
resetEncoder(RIGHT);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
@ -42,3 +42,4 @@ class SweepServo
|
||||
SweepServo servos [N_SERVOS];
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@ -76,3 +76,4 @@ Servo SweepServo::getServo()
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@ -17,25 +17,29 @@ class SweepServo2
|
||||
|
||||
void initServo(
|
||||
int servoPin,
|
||||
int stepDelayMs);
|
||||
unsigned long stepDelayMs);
|
||||
|
||||
void setServoDelay(
|
||||
int servoPin,
|
||||
int stepDelayMs);
|
||||
unsigned long stepDelayMs);
|
||||
|
||||
void moveServo();
|
||||
void setTargetPosition(int position);
|
||||
void setServoDelay(int delay);
|
||||
int getCurrentPosition();
|
||||
|
||||
void enable();
|
||||
void disable();
|
||||
bool isEnabled();
|
||||
|
||||
Servo getServo();
|
||||
|
||||
private:
|
||||
Servo servo;
|
||||
int stepDelayMs;
|
||||
unsigned long stepDelayMs;
|
||||
int currentPositionDegrees;
|
||||
int targetPositionDegrees;
|
||||
long lastSweepCommand;
|
||||
unsigned long lastSweepCommand;
|
||||
bool enabled;
|
||||
};
|
||||
|
||||
SweepServo2 myServos [MAX_N_SERVOS];
|
||||
@ -43,3 +47,4 @@ SweepServo2 myServos [MAX_N_SERVOS];
|
||||
int myServoPins [MAX_N_SERVOS];
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@ -26,7 +26,7 @@ SweepServo2::SweepServo2()
|
||||
// Init
|
||||
void SweepServo2::initServo(
|
||||
int servoPin,
|
||||
int stepDelayMs)
|
||||
unsigned long stepDelayMs)
|
||||
{
|
||||
this->stepDelayMs = stepDelayMs;
|
||||
this->currentPositionDegrees = 90;
|
||||
@ -38,7 +38,7 @@ void SweepServo2::initServo(
|
||||
// Init
|
||||
void SweepServo2::setServoDelay(
|
||||
int servoPin,
|
||||
int stepDelayMs)
|
||||
unsigned long stepDelayMs)
|
||||
{
|
||||
this->stepDelayMs = stepDelayMs;
|
||||
}
|
||||
@ -48,7 +48,7 @@ void SweepServo2::moveServo()
|
||||
{
|
||||
|
||||
// Get ellapsed time
|
||||
int delta = millis() - this->lastSweepCommand;
|
||||
unsigned long delta = millis() - this->lastSweepCommand;
|
||||
|
||||
// Check if time for a step
|
||||
if (delta > this->stepDelayMs) {
|
||||
@ -80,13 +80,16 @@ int SweepServo2::getCurrentPosition()
|
||||
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;
|
||||
void SweepServo2::enable() {
|
||||
this->enabled = true;
|
||||
}
|
||||
|
||||
void SweepServo2::disable() {
|
||||
this->enabled = false;
|
||||
}
|
||||
|
||||
bool SweepServo2::isEnabled() {
|
||||
return this->enabled;
|
||||
}
|
||||
|
||||
// Accessor for servo object
|
||||
@ -96,4 +99,14 @@ Servo SweepServo2::getServo()
|
||||
}
|
||||
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@ -14,19 +14,23 @@ add_message_files(FILES
|
||||
add_service_files(FILES
|
||||
AnalogWrite.srv
|
||||
AnalogSensorWrite.srv
|
||||
AnalogFloatSensorWrite.srv
|
||||
AnalogFloatSensorWrite.srv
|
||||
AnalogPinMode.srv
|
||||
AnalogRead.srv
|
||||
AnalogSensorRead.srv
|
||||
AnalogFloatSensorRead.srv
|
||||
DigitalPinMode.srv
|
||||
DigitalRead.srv
|
||||
DigitalSensorRead.srv
|
||||
DigitalSetDirection.srv
|
||||
DigitalSensorSetDirection.srv
|
||||
DigitalSensorPinMode.srv
|
||||
DigitalWrite.srv
|
||||
DigitalSensorWrite.srv
|
||||
Enable.srv
|
||||
Relax.srv
|
||||
AnalogSensorRead.srv
|
||||
ServoAttach.srv
|
||||
ServoDetach.srv
|
||||
ServoRead.srv
|
||||
ServoWrite.srv
|
||||
SetSpeed.srv
|
||||
|
||||
3
ros_arduino_msgs/srv/AnalogPinMode.srv
Executable file
3
ros_arduino_msgs/srv/AnalogPinMode.srv
Executable file
@ -0,0 +1,3 @@
|
||||
uint8 pin
|
||||
bool direction
|
||||
---
|
||||
3
ros_arduino_msgs/srv/DigitalSetDirection.srv
Executable file
3
ros_arduino_msgs/srv/DigitalSetDirection.srv
Executable file
@ -0,0 +1,3 @@
|
||||
uint8 pin
|
||||
bool direction
|
||||
---
|
||||
3
ros_arduino_msgs/srv/ServoAttach.srv
Executable file
3
ros_arduino_msgs/srv/ServoAttach.srv
Executable file
@ -0,0 +1,3 @@
|
||||
uint8 id
|
||||
---
|
||||
|
||||
3
ros_arduino_msgs/srv/ServoDetach.srv
Executable file
3
ros_arduino_msgs/srv/ServoDetach.srv
Executable file
@ -0,0 +1,3 @@
|
||||
uint8 id
|
||||
---
|
||||
|
||||
@ -1,3 +1,3 @@
|
||||
uint8 id
|
||||
---
|
||||
float32 value
|
||||
uint8 position
|
||||
|
||||
@ -1,3 +1,3 @@
|
||||
uint8 id
|
||||
float32 value
|
||||
uint8 position
|
||||
---
|
||||
|
||||
@ -1,3 +1,3 @@
|
||||
uint8 id
|
||||
float32 value
|
||||
float32 speed
|
||||
---
|
||||
|
||||
@ -1,2 +1,2 @@
|
||||
float32 value
|
||||
float32 speed
|
||||
---
|
||||
|
||||
@ -35,7 +35,7 @@ class SweepServo():
|
||||
self.node_name = "sweep_servo"
|
||||
|
||||
# Initialize the node
|
||||
rospy.init_node(self.node_name, anonymous=True)
|
||||
rospy.init_node(self.node_name)
|
||||
|
||||
# Set a shutdown function to clean up when termniating the node
|
||||
rospy.on_shutdown(self.shutdown)
|
||||
@ -43,11 +43,10 @@ class SweepServo():
|
||||
# Name of the joint we want to control
|
||||
joint_name = rospy.get_param('~joint', 'head_pan_joint')
|
||||
|
||||
if joint_name == '':
|
||||
if joint_name is None or joint_name == '':
|
||||
rospy.logino("Joint name for servo must be specified in parameter file.")
|
||||
os._exit(1)
|
||||
|
||||
servo_pin = rospy.get_param('/arduino/joints/' + str(joint_name) + '/pin')
|
||||
max_position = radians(rospy.get_param('/arduino/joints/' + str(joint_name) + '/max_position'))
|
||||
min_position = radians(rospy.get_param('/arduino/joints/' + str(joint_name) + '/min_position'))
|
||||
|
||||
@ -55,11 +54,11 @@ class SweepServo():
|
||||
target_min = min_position
|
||||
|
||||
# How fast should we sweep the servo
|
||||
speed = rospy.get_param('~speed', 1.0) # rad/s
|
||||
|
||||
# Time between between sweeps
|
||||
delay = rospy.get_param('~delay', 2) # seconds
|
||||
servo_speed = rospy.get_param('~servo_speed', 1.0) # rad/s
|
||||
|
||||
# Time delay between between sweeps
|
||||
delay = rospy.get_param('~delay', 0) # seconds
|
||||
|
||||
# Create a publisher for setting the joint position
|
||||
joint_pub = rospy.Publisher('/' + joint_name + '/command', Float64, queue_size=5)
|
||||
|
||||
@ -88,7 +87,7 @@ class SweepServo():
|
||||
set_speed = rospy.ServiceProxy('/' + joint_name + '/set_speed', SetSpeed)
|
||||
|
||||
# Set the initial servo speed
|
||||
set_speed(speed)
|
||||
set_speed(servo_speed)
|
||||
|
||||
rospy.loginfo('Sweeping servo...')
|
||||
|
||||
@ -121,5 +120,4 @@ if __name__ == '__main__':
|
||||
SweepServo()
|
||||
except:
|
||||
rospy.loginfo('Unexpected error: ' + str(sys.exc_info()[0]))
|
||||
raise
|
||||
|
||||
raise
|
||||
@ -1,7 +1,9 @@
|
||||
<launch>
|
||||
<arg name="joint" default="head_pan_joint" />
|
||||
|
||||
<node pkg="ros_arduino_python" name="$(anon sweep_servo)" type="sweep_servo.py" output="screen" clear_params="true">
|
||||
<node pkg="rbx3_demo" name="sweep_servo" type="sweep_servo.py" output="screen" clear_params="true">
|
||||
<param name="joint" value="$(arg joint)" />
|
||||
<param name="servo_speed" value="1.0" />
|
||||
<param name="delay" value="0.0" />
|
||||
</node>
|
||||
</launch>
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user