mirror of
https://github.com/YikeStone/ros_arduino_bridge.git
synced 2025-08-03 19:24:09 +05:30
308 lines
7.5 KiB
C++
308 lines
7.5 KiB
C++
/*********************************************************************
|
|
* 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
|
|
|
|
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"
|
|
|
|
#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
|
|
const int PID_INTERVAL = 1000 / PID_RATE;
|
|
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();
|
|
}
|
|
|
|
void resetEncoders() {
|
|
resetEncoder(LEFT);
|
|
resetEncoder(RIGHT);
|
|
}
|
|
|
|
// 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
|
|
|
|
int mode = 0;
|
|
int index = 0;
|
|
char chr;
|
|
char cmd;
|
|
char argv1[16];
|
|
char argv2[16];
|
|
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;
|
|
mode = 0;
|
|
index = 0;
|
|
}
|
|
|
|
// Run a command
|
|
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.print(arg1);
|
|
Serial.print(arg2);
|
|
Serial.println("OK");
|
|
break;
|
|
case SERVO_READ:
|
|
Serial.println(servos[arg1].read());
|
|
break;
|
|
#endif
|
|
|
|
#ifdef USE_BASE
|
|
case READ_ENCODERS:
|
|
Serial.print(readEncoder(0));
|
|
Serial.print(" ");
|
|
Serial.println(readEncoder(1));
|
|
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;
|
|
}
|
|
}
|
|
|
|
void setup() {
|
|
Serial.begin(BAUDRATE);
|
|
|
|
#ifdef USE_BASE
|
|
drive.init();
|
|
#endif
|
|
|
|
#ifdef USE_SERVOS
|
|
int i;
|
|
for (i = 0; i < N_SERVOS; i++) {
|
|
servos[i].attach(servoPins[i]);
|
|
}
|
|
#endif
|
|
}
|
|
|
|
void loop() {
|
|
while (Serial.available() > 0) {
|
|
|
|
chr = Serial.read();
|
|
|
|
// Terminate a command with a CR
|
|
if (chr == 13) {
|
|
if (mode == 1) argv1[index] = NULL;
|
|
else if (mode == 2) argv2[index] = NULL;
|
|
runCommand();
|
|
resetCommand();
|
|
}
|
|
// Use spaces to delimit parts of the command
|
|
else if (chr == ' ') {
|
|
if (mode == 0) mode = 1;
|
|
else if (mode == 1) {
|
|
argv1[index] = NULL;
|
|
mode = 2;
|
|
index = 0;
|
|
}
|
|
continue;
|
|
}
|
|
else {
|
|
if (mode == 0) {
|
|
cmd = chr;
|
|
}
|
|
else if (mode == 1) {
|
|
argv1[index] = chr;
|
|
index++;
|
|
}
|
|
else if (mode == 2) {
|
|
argv2[index] = chr;
|
|
index++;
|
|
}
|
|
}
|
|
}
|
|
|
|
// If we using base control, run a PID loop at the appropriate intervals
|
|
#ifdef USE_BASE
|
|
if (millis() > nextPID) {
|
|
updatePID();
|
|
nextPID += PID_INTERVAL;
|
|
}
|
|
|
|
if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {;
|
|
setMotorSpeeds(0, 0);
|
|
moving = 0;
|
|
}
|
|
|
|
#endif
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|