mirror of
https://github.com/YikeStone/ros_arduino_bridge.git
synced 2025-08-03 19:24:09 +05:30
Fixed spelling typo in arduino_node.py
This commit is contained in:
Patrick Goebel
parent
71b8c5876e
commit
0a4cccca50
@ -0,0 +1,439 @@
|
||||
/*********************************************************************
|
||||
* 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
|
||||
#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
|
||||
#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
|
||||
/* 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));
|
||||
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];
|
||||
|
||||
if (cmd == SET_PID_TARGET) {
|
||||
arg1 = atol(argv1);
|
||||
arg2 = atol(argv2);
|
||||
} else {
|
||||
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;
|
||||
case SET_PID_TARGET:
|
||||
resetEncoders();
|
||||
leftMotorPIDTarget = arg1;
|
||||
rightMotorPIDTarget = arg2;
|
||||
leftPIDTargetComplete = 0;
|
||||
rightPIDTargetComplete = 0;
|
||||
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
|
||||
}
|
||||
|
||||
// Variables to hold Encoder Targets - Keep moving until at this target
|
||||
long leftMotorTarget;
|
||||
long rightMotorTarget;
|
||||
long targetError = 100; // How far away can I be before it is okay? Keeps jitter on low
|
||||
|
||||
long targetLeftErrorMin;
|
||||
long targetRightErrorMin;
|
||||
long targetLeftErrorMax;
|
||||
long targetRightErrorMax;
|
||||
long rightToGo; // How far do we have left to go?
|
||||
long leftToGo;
|
||||
long leftSpeed; // How fast do we need to go to get there?
|
||||
long rightSpeed;
|
||||
long leftTargetComplete = 0;
|
||||
long rightTargetComplete = 0;
|
||||
long startSpeed = 35; // Acceleration starting value to break traction.
|
||||
long endSpeed = 25; // Won't decelerate below this number
|
||||
float accel = 0.05; // Acceleartion ramp value to get to top speed.
|
||||
float decel = 0.01; // Deceleration value to slow down and stop without overshooting.
|
||||
|
||||
void checkTargets() {
|
||||
long leftEncoder = readEncoder(LEFT);
|
||||
long rightEncoder = readEncoder(RIGHT);
|
||||
|
||||
targetLeftErrorMin = leftMotorTarget - targetError;
|
||||
targetRightErrorMin = rightMotorTarget - targetError;
|
||||
targetLeftErrorMax = leftMotorTarget + targetError;
|
||||
targetRightErrorMax = rightMotorTarget + targetError;
|
||||
|
||||
if (leftTargetComplete == 0) {
|
||||
if (leftEncoder <= targetLeftErrorMax && leftEncoder >= targetLeftErrorMin) {
|
||||
if (leftTargetComplete == 0) {
|
||||
leftTargetComplete = 1;
|
||||
leftSpeed = 0;
|
||||
leftPID.TargetTicksPerFrame = 0;
|
||||
moving = 0;
|
||||
}
|
||||
} else {
|
||||
if (leftEncoder >= leftMotorTarget) {
|
||||
leftToGo = leftEncoder - leftMotorTarget;
|
||||
leftSpeed = abs(leftToGo) * decel;
|
||||
if (leftSpeed < EndSpeed) { leftSpeed = EndSpeed; }
|
||||
if (leftEncoder <= 10000) {
|
||||
if (leftEncoder < (StartSpeed * 20)) {
|
||||
leftSpeed = StartSpeed;
|
||||
} else {
|
||||
leftSpeed = leftEncoder * accel;
|
||||
}
|
||||
}
|
||||
if (leftSpeed > 500) { leftSpeed = 500; }
|
||||
leftPID.TargetTicksPerFrame = leftSpeed;
|
||||
moving = 1;
|
||||
} else {
|
||||
leftToGo = leftMotorTarget - leftEncoder;
|
||||
leftSpeed = abs(leftToGo) * decel;
|
||||
if (leftSpeed < EndSpeed) { leftSpeed = EndSpeed; }
|
||||
if (leftEncoder < 10000) {
|
||||
if (leftEncoder < (StartSpeed * 20)) {
|
||||
leftSpeed = StartSpeed;
|
||||
} else {
|
||||
leftSpeed = leftEncoder * accel;
|
||||
}
|
||||
}
|
||||
if (leftSpeed > 500) { leftSpeed = 500; }
|
||||
leftPID.TargetTicksPerFrame = -leftSpeed;
|
||||
moving = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (RightTargetComplete == 0) {
|
||||
if (rightEncoder <= targetRightErrorMax && rightEncoder >= targetRightErrorMin) {
|
||||
if (RightTargetComplete == 0) {
|
||||
RightTargetComplete = 1;
|
||||
rightspeed = 0;
|
||||
rightPID.TargetTicksPerFrame = 0;
|
||||
moving = 0;
|
||||
}
|
||||
} else {
|
||||
if (rightEncoder >= rightMotorTarget) {
|
||||
rightToGo = rightEncoder - rightMotorTarget;
|
||||
rightspeed = abs(rightToGo) * decel;
|
||||
if(rightspeed < EndSpeed) { rightspeed = EndSpeed; }
|
||||
if(rightEncoder <= 10000) {
|
||||
if(rightEncoder < (StartSpeed * 20)) {
|
||||
rightspeed = StartSpeed;
|
||||
} else {
|
||||
rightspeed = rightEncoder * accel;
|
||||
}
|
||||
}
|
||||
if(rightspeed > 500) { rightspeed = 500; }
|
||||
rightPID.TargetTicksPerFrame = rightspeed;
|
||||
moving = 1;
|
||||
} else {
|
||||
rightToGo = rightMotorTarget - rightEncoder;
|
||||
rightspeed = abs(rightToGo) * decel;
|
||||
if (rightspeed < EndSpeed) { rightspeed = EndSpeed; }
|
||||
if (rightEncoder < 10000) {
|
||||
if (rightEncoder < (StartSpeed * 20)) {
|
||||
rightspeed = StartSpeed;
|
||||
} else {
|
||||
rightspeed = rightEncoder * accel;
|
||||
}
|
||||
}
|
||||
if (rightspeed > 500) { rightspeed = 500; }
|
||||
rightPID.TargetTicksPerFrame = -rightspeed;
|
||||
moving = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@ -85,7 +85,7 @@ class ArduinoROS():
|
||||
|
||||
rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud")
|
||||
|
||||
# Reservce a thread lock
|
||||
# Reserve a thread lock
|
||||
mutex = thread.allocate_lock()
|
||||
|
||||
# Initialize any sensors
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user