mirror of
https://github.com/YikeStone/ros_arduino_bridge.git
synced 2025-08-03 11:14:08 +05:30
Deleted extraneous ROSArduinoBridge.cpp file
This commit is contained in:
Patrick Goebel
parent
0a4cccca50
commit
91b80921e6
@ -1,439 +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
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
Loading…
x
Reference in New Issue
Block a user