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

One Major Bug Fix and separate PIDs for the two motors.

Browse Source
This commit is contained in:
yikestone 2018-01-11 01:55:19 +05:30
parent a960c8a88a
commit 53225a0990
19 changed files with 107 additions and 1044 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
README.md
View File

@ -1,3 +1,6 @@
This Fork is modified for L298N motor driver and quad encoders, with one major bug fix and other minor bug fixes.
This supports separate PID control system for two (left, right) motors.
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.

341
ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/MegaRobogaiaPololu.ino
View File

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

22
ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/commands.h
View File

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

71
ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/diff_controller.h
View File

@ -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);
}

34
ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/sensors.h
View File

@ -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);
}

16
ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/servos.h
View File

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

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

@ -1,78 +1,4 @@
/*********************************************************************
* 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
and the Home Brew Robotics Club (HBRC): http://hbrobotics.org
Authors: Patrick Goebel, James Nugen
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 the motor controller and encoder library you are using */
#ifdef USE_BASE
/* The Pololu VNH5019 dual motor driver shield */
#define POLOLU_VNH5019
/* The Pololu MC33926 dual motor driver shield */
//#define POLOLU_MC33926
/* The RoboGaia encoder shield */
#define ROBOGAIA
/* Encoders directly attached to Arduino board */
//#define ARDUINO_ENC_COUNTER
/* L298 Motor driver*/
//#define L298_MOTOR_DRIVER
#endif
#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
@ -80,65 +6,24 @@
#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
/* Motor driver function definitions */
#include "motor_driver.h"
/* Encoder driver function definitions */
#include "encoder_driver.h"
/* 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;
#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));
@ -149,7 +34,6 @@ void resetCommand() {
index = 0;
}
/* Run a command. Commands are defined in commands.h */
int runCommand() {
int i = 0;
char *p = argv1;
@ -162,40 +46,7 @@ int runCommand() {
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].setTargetPosition(arg2);
Serial.println("OK");
break;
case SERVO_READ:
Serial.println(servos[arg1].getServo().read());
break;
#endif
#ifdef USE_BASE
case READ_ENCODERS:
Serial.print(readEncoder(LEFT));
Serial.print(" ");
@ -217,20 +68,43 @@ int runCommand() {
else moving = 1;
leftPID.TargetTicksPerFrame = arg1;
rightPID.TargetTicksPerFrame = arg2;
Serial.println("OK");
break;
case UPDATE_PID:
case UPDATE_PID_L:
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];
leftPID.Kp = pid_args[0];
leftPID.Kd = pid_args[1];
leftPID.Ki = pid_args[2];
leftPID.Ko = pid_args[3];
Serial.println("OK");
break;
case UPDATE_PID_R:
while ((str = strtok_r(p, ":", &p)) != '\0') {
pid_args[i] = atoi(str);
i++;
}
rightPID.Kp = pid_args[0];
rightPID.Kd = pid_args[1];
rightPID.Ki = pid_args[2];
rightPID.Ko = pid_args[3];
Serial.println("OK");
break;
case DISP_PID_P:
Serial.println(leftPID.Kp);
Serial.println(leftPID.Kd);
Serial.println(leftPID.Ki);
Serial.println(leftPID.Ko);
Serial.println(rightPID.Kp);
Serial.println(rightPID.Kd);
Serial.println(rightPID.Ki);
Serial.println(rightPID.Ko);
Serial.println("OK");
break;
#endif
default:
Serial.println("Invalid Command");
break;
@ -241,10 +115,6 @@ int runCommand() {
void setup() {
Serial.begin(BAUDRATE);
// 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);
@ -263,28 +133,16 @@ void setup() {
// enable PCINT1 and PCINT2 interrupt in the general interrupt mask
PCICR |= (1 << PCIE1) | (1 << PCIE2);
#endif
initMotorController();
resetPID();
#endif
/* Attach servos if used */
#ifdef USE_SERVOS
int i;
for (i = 0; i < N_SERVOS; i++) {
servos[i].initServo(
servoPins[i],
stepDelay[i],
servoInitPosition[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
@ -325,8 +183,6 @@ void loop() {
}
}
// If we are using base control, run a PID calculation at the appropriate intervals
#ifdef USE_BASE
if (millis() > nextPID) {
updatePID();
nextPID += PID_INTERVAL;
@ -337,14 +193,6 @@ 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
}

13
ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h
View File

@ -5,19 +5,14 @@
#ifndef COMMANDS_H
#define COMMANDS_H
#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 UPDATE_PID_L 'L'
#define UPDATE_PID_R 'R'
#define DISP_PID_P 'z'
#define LEFT 0
#define RIGHT 1

66
ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h
View File

@ -1,30 +1,16 @@
/* 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
/*
* Using previous input (PrevInput) instead of PrevError to avoid derivative kick,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
*/
int PrevInput; // last input
//int PrevErr; // last error
/*
* Using integrated term (ITerm) instead of integrated error (Ierror),
* to allow tuning changes,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
//int Ierror;
int ITerm; //integrated term
int Kp = 20;
int Kd = 12;
int Ki = 0;
int Ko = 50;
long output; // last motor setting
}
@ -32,22 +18,9 @@ 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?
/*
* Initialize PID variables to zero to prevent startup spikes
* when turning PID on to start moving
* In particular, assign both Encoder and PrevEnc the current encoder value
* See http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
* Note that the assumption here is that PID is only turned on
* when going from stop to moving, that's why we can init everything on zero.
*/
void resetPID(){
leftPID.TargetTicksPerFrame = 0.0;
leftPID.Encoder = readEncoder(LEFT);
@ -64,7 +37,6 @@ void resetPID(){
rightPID.ITerm = 0;
}
/* PID routine to compute the next motor commands */
void doPID(SetPointInfo * p) {
long Perror;
long output;
@ -74,29 +46,18 @@ void doPID(SetPointInfo * p) {
input = p->Encoder - p->PrevEnc;
Perror = p->TargetTicksPerFrame - input;
/*
* Avoid derivative kick and allow tuning changes,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
//output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko;
// p->PrevErr = Perror;
output = (Kp * Perror - Kd * (input - p->PrevInput) + p->ITerm) / Ko;
output = ((p->Kp) * Perror + (p->Kd) * (input - p->PrevInput) + p->ITerm) / (p->Ko);
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
/*
* allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/
*/
p->ITerm += Ki * Perror;
p->ITerm += (p->Ki) * Perror;
p->output = output;
p->PrevInput = input;
@ -108,23 +69,14 @@ void updatePID() {
leftPID.Encoder = readEncoder(LEFT);
rightPID.Encoder = readEncoder(RIGHT);
/* If we're not moving there is nothing more to do */
if (!moving){
/*
* Reset PIDs once, to prevent startup spikes,
* see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
* PrevInput is considered a good proxy to detect
* whether reset has already happened
*/
if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID();
return;
}
/* Compute PID update for each motor */
doPID(&rightPID);
doPID(&leftPID);
/* Set the motor speeds accordingly */
doPID(&rightPID);
setMotorSpeeds(leftPID.output, rightPID.output);
}

10
ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h
View File

@ -1,18 +1,8 @@
/* *************************************************************
Encoder driver function definitions - by James Nugen
************************************************************ */
#ifdef ARDUINO_ENC_COUNTER
//below can be changed, but should be PORTD pins;
//otherwise additional changes in the code are required
#define LEFT_ENC_PIN_A PD2 //pin 2
#define LEFT_ENC_PIN_B PD3 //pin 3
//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
#endif
long readEncoder(int i);
void resetEncoder(int i);

39
ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino
View File

@ -1,38 +1,7 @@
/* *************************************************************
Encoder definitions
Add an "#ifdef" block to this file to include support for
a particular encoder board or library. Then add the appropriate
#define near the top of the main ROSArduinoBridge.ino file.
************************************************************ */
#ifdef USE_BASE
#ifdef ROBOGAIA
/* The Robogaia Mega Encoder shield */
#include "MegaEncoderCounter.h"
/* Create the encoder shield object */
MegaEncoderCounter encoders = MegaEncoderCounter(4); // Initializes the Mega Encoder Counter in the 4X Count mode
/* 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();
}
#elif defined(ARDUINO_ENC_COUNTER)
volatile long left_enc_pos = 0L;
volatile long right_enc_pos = 0L;
static const int8_t ENC_STATES [] = {0,1,-1,0,-1,0,0,1,1,0,0,-1,0,-1,1,0}; //encoder lookup table
/* Interrupt routine for LEFT encoder, taking care of actual counting */
ISR (PCINT2_vect){
static uint8_t enc_last=0;
@ -42,7 +11,6 @@
left_enc_pos += ENC_STATES[(enc_last & 0x0f)];
}
/* Interrupt routine for RIGHT encoder, taking care of actual counting */
ISR (PCINT1_vect){
static uint8_t enc_last=0;
@ -52,13 +20,11 @@
right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
}
/* Wrap the encoder reading function */
long readEncoder(int i) {
if (i == LEFT) return left_enc_pos;
else return right_enc_pos;
}
/* Wrap the encoder reset function */
void resetEncoder(int i) {
if (i == LEFT){
left_enc_pos=0L;
@ -68,15 +34,10 @@
return;
}
}
#else
#error A encoder driver must be selected!
#endif
/* Wrap the encoder reset function */
void resetEncoders() {
resetEncoder(LEFT);
resetEncoder(RIGHT);
}
#endif

6
ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h
View File

@ -1,15 +1,9 @@
/***************************************************************
Motor driver function definitions - by James Nugen
*************************************************************/
#ifdef L298_MOTOR_DRIVER
#define RIGHT_MOTOR_BACKWARD 5
#define LEFT_MOTOR_BACKWARD 6
#define RIGHT_MOTOR_FORWARD 9
#define LEFT_MOTOR_FORWARD 10
#define RIGHT_MOTOR_ENABLE 12
#define LEFT_MOTOR_ENABLE 13
#endif
void initMotorController();
void setMotorSpeed(int i, int spd);

75
ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino
View File

@ -1,61 +1,3 @@
/***************************************************************
Motor driver definitions
Add a "#elif defined" block to this file to include support
for a particular motor driver. Then add the appropriate
#define near the top of the main ROSArduinoBridge.ino file.
*************************************************************/
#ifdef USE_BASE
#ifdef POLOLU_VNH5019
/* Include the Pololu library */
#include "DualVNH5019MotorShield.h"
/* Create the motor driver object */
DualVNH5019MotorShield drive;
/* 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);
}
#elif defined POLOLU_MC33926
/* Include the Pololu library */
#include "DualMC33926MotorShield.h"
/* Create the motor driver object */
DualMC33926MotorShield drive;
/* 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);
}
#elif defined L298_MOTOR_DRIVER
void initMotorController() {
digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
digitalWrite(LEFT_MOTOR_ENABLE, HIGH);
@ -73,21 +15,18 @@
spd = 255;
if (i == LEFT) {
if (reverse == 0) { analogWrite(RIGHT_MOTOR_FORWARD, spd); analogWrite(RIGHT_MOTOR_BACKWARD, 0); }
else if (reverse == 1) { analogWrite(RIGHT_MOTOR_BACKWARD, spd); analogWrite(RIGHT_MOTOR_FORWARD, 0); }
}
else /*if (i == RIGHT) //no need for condition*/ {
if (reverse == 0) { analogWrite(LEFT_MOTOR_FORWARD, spd); analogWrite(LEFT_MOTOR_BACKWARD, 0); }
if(spd == 0) { digitalWrite(LEFT_MOTOR_FORWARD, 1); digitalWrite(LEFT_MOTOR_BACKWARD, 1); }
else if (reverse == 0) { analogWrite(LEFT_MOTOR_FORWARD, spd); analogWrite(LEFT_MOTOR_BACKWARD, 0); }
else if (reverse == 1) { analogWrite(LEFT_MOTOR_BACKWARD, spd); analogWrite(LEFT_MOTOR_FORWARD, 0); }
}
else if (i == RIGHT){
if(spd == 0) { digitalWrite(RIGHT_MOTOR_FORWARD, 1); digitalWrite(RIGHT_MOTOR_BACKWARD, 1); }
else if (reverse == 0) { analogWrite(RIGHT_MOTOR_FORWARD, spd); analogWrite(RIGHT_MOTOR_BACKWARD, 0); }
else if (reverse == 1) { analogWrite(RIGHT_MOTOR_BACKWARD, spd); analogWrite(RIGHT_MOTOR_FORWARD, 0); }
}
}
void setMotorSpeeds(int leftSpeed, int rightSpeed) {
setMotorSpeed(LEFT, leftSpeed);
setMotorSpeed(RIGHT, rightSpeed);
}
#else
#error A motor driver must be selected!
#endif
#endif

34
ros_arduino_firmware/src/libraries/ROSArduinoBridge/sensors.h
View File

@ -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);
}

44
ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h
View File

@ -1,44 +0,0 @@
#ifndef SERVOS_H
#define SERVOS_H
#define N_SERVOS 2
// 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

78
ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino
View File

@ -1,78 +0,0 @@
/***************************************************************
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

13
ros_arduino_python/config/arduino_params.yaml
View File

@ -24,10 +24,15 @@ base_frame: base_link
#motors_reversed: True
# === PID parameters
#Kp: 10
#Kd: 12
#Ki: 0
#Ko: 50
#lKp: 10
#lKd: 12
#lKi: 0
#lKo: 50
#rKp: 10
#rKd: 12
#rKi: 0
#rKo: 50
#accel_limit: 1.0
# === Sensor definitions. Examples only - edit for your robot.

6
ros_arduino_python/src/ros_arduino_python/arduino_driver.py
View File

@ -248,11 +248,13 @@ class Arduino:
self.mutex.release()
return ack == 'OK'
def update_pid(self, Kp, Kd, Ki, Ko):
def update_pid(self, lKp, lKd, lKi, lKo, rKp, rKd, rKi, rKo):
''' Set the PID parameters on the Arduino
'''
print "Updating PID parameters"
cmd = 'u ' + str(Kp) + ':' + str(Kd) + ':' + str(Ki) + ':' + str(Ko)
cmd = 'L ' + str(lKp) + ':' + str(lKd) + ':' + str(lKi) + ':' + str(lKo)
self.execute_ack(cmd)
cmd = 'R ' + str(rKp) + ':' + str(rKd) + ':' + str(rKi) + ':' + str(rKo)
self.execute_ack(cmd)
def get_baud(self):

36
ros_arduino_python/src/ros_arduino_python/base_controller.py
View File

@ -44,10 +44,15 @@ class BaseController:
pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "")
pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
pid_params['Kp'] = rospy.get_param("~Kp", 20)
pid_params['Kd'] = rospy.get_param("~Kd", 12)
pid_params['Ki'] = rospy.get_param("~Ki", 0)
pid_params['Ko'] = rospy.get_param("~Ko", 50)
pid_params['lKp'] = rospy.get_param("~lKp", 20)
pid_params['lKd'] = rospy.get_param("~lKd", 12)
pid_params['lKi'] = rospy.get_param("~lKi", 0)
pid_params['lKo'] = rospy.get_param("~lKo", 50)
pid_params['rKp'] = rospy.get_param("~rKp", 20)
pid_params['rKd'] = rospy.get_param("~rKd", 12)
pid_params['rKi'] = rospy.get_param("~rKi", 0)
pid_params['rKo'] = rospy.get_param("~rKo", 50)
self.accel_limit = rospy.get_param('~accel_limit', 0.1)
self.motors_reversed = rospy.get_param("~motors_reversed", False)
@ -90,7 +95,7 @@ class BaseController:
# Set up the odometry broadcaster
self.odomPub = rospy.Publisher('odom', Odometry, queue_size=5)
self.odomBroadcaster = TransformBroadcaster()
self.cameraBroadcaster = TransformBroadcaster()
rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev")
rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame")
@ -110,12 +115,17 @@ class BaseController:
self.encoder_resolution = pid_params['encoder_resolution']
self.gear_reduction = pid_params['gear_reduction']
self.Kp = pid_params['Kp']
self.Kd = pid_params['Kd']
self.Ki = pid_params['Ki']
self.Ko = pid_params['Ko']
self.lKp = pid_params['lKp']
self.lKd = pid_params['lKd']
self.lKi = pid_params['lKi']
self.lKo = pid_params['lKo']
self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko)
self.rKp = pid_params['rKp']
self.rKd = pid_params['rKd']
self.rKi = pid_params['rKi']
self.rKo = pid_params['rKo']
self.arduino.update_pid(self.lKp, self.lKd, self.lKi, self.lKo,self.rKp, self.rKd, self.rKi, self.rKo)
def poll(self):
now = rospy.Time.now()
@ -171,7 +181,11 @@ class BaseController:
self.base_frame,
"odom"
)
self.odomBroadcaster.sendTransform((self.x - 0.15, self.y - 0.055, 0.225),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
"depth_camera_frame",
self.base_frame)
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = self.base_frame