From 91b80921e612ff48073571c39a0c313a49799d25 Mon Sep 17 00:00:00 2001 From: Patrick Goebel Date: Sat, 17 Aug 2013 16:55:57 -0700 Subject: [PATCH] Deleted extraneous ROSArduinoBridge.cpp file --- .../ROSArduinoBridge/ROSArduinoBridge.cpp | 439 ------------------ 1 file changed, 439 deletions(-) delete mode 100644 ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.cpp diff --git a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.cpp b/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.cpp deleted file mode 100644 index 50d2076..0000000 --- a/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.cpp +++ /dev/null @@ -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 - #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; - } - } - } -} - - - -