diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.cpp b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.cpp new file mode 100644 index 0000000..50d2076 --- /dev/null +++ b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.cpp @@ -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 + #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; + } + } + } +} + + + + diff --git a/ros_arduino_python/nodes/arduino_node.py b/ros_arduino_python/nodes/arduino_node.py index e9c65f0..15f0ac5 100755 --- a/ros_arduino_python/nodes/arduino_node.py +++ b/ros_arduino_python/nodes/arduino_node.py @@ -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