diff --git a/ArloRobot/.DS_Store b/ArloRobot/.DS_Store deleted file mode 100644 index 816d33f..0000000 Binary files a/ArloRobot/.DS_Store and /dev/null differ diff --git a/ArloRobot/Arlo-Change-Pulse-Scale/Arlo-Change-Pulse-Scale.ino b/ArloRobot/Arlo-Change-Pulse-Scale/Arlo-Change-Pulse-Scale.ino deleted file mode 100755 index bd1ab56..0000000 --- a/ArloRobot/Arlo-Change-Pulse-Scale/Arlo-Change-Pulse-Scale.ino +++ /dev/null @@ -1,31 +0,0 @@ -/* - Arlo-Change-Pulse-Scale - - Change pulse scale from 1000...2000 us to 1300 to 1700 us so that - Robotics with the BOE Shield-Bot examples can run the Arlo at top speed. -*/ - -#include // Include Arlo library -#include // Include SoftwareSerial library - -// Arlo and serial objects required -ArloRobot Arlo; // Arlo object -SoftwareSerial ArloSerial(12, 13); // Serial in I/O 12, out I/O 13 - -void setup() // Setup function -{ - tone(4, 3000, 2000); // Piezospeaker beep - Serial.begin(9600); // Start terminal serial port - - ArloSerial.begin(19200); // Start DHB-10 serial com - Arlo.begin(ArloSerial); // Pass to Arlo object - - Arlo.writeConfig("SCALE", 200); // Change pulse to speed response - //Arlo.writeConfig("SCALE", 1000); // Want default back? Un-comment - Arlo.storeConfig("SCALE"); // Save setting in EEPROM - - Arlo.writePulseMode(); // Get ready to receive pulses -} - -void loop() {} // Nothing for main loop - diff --git a/ArloRobot/Arlo-Distance-Maneuvers/Arlo-Distance-Maneuvers.ino b/ArloRobot/Arlo-Distance-Maneuvers/Arlo-Distance-Maneuvers.ino deleted file mode 100755 index bd34b46..0000000 --- a/ArloRobot/Arlo-Distance-Maneuvers/Arlo-Distance-Maneuvers.ino +++ /dev/null @@ -1,56 +0,0 @@ -/* - Arlo-Distance-Maneuvers - - Examples that use serial communication to make the arlo travel in certain - distances. -*/ - - -#include // Include Arlo library -#include // Include SoftwareSerial library - -// Arlo and serial objects required -ArloRobot Arlo; // Arlo object -SoftwareSerial ArloSerial(12, 13); // Serial in I/O 12, out I/O 13 - -int countsLeft, countsRight; // Encoder counting variables -void setup() // Setup function -{ - tone(4, 3000, 2000); // Piezospeaker beep - Serial.begin(9600); // Start terminal serial port - Serial.println("Program running..."); // Display starting message - - ArloSerial.begin(19200); // Start DHB-10 serial com - Arlo.begin(ArloSerial); // Pass to Arlo object - - Arlo.clearCounts(); // Clear encoder counts - - Arlo.writeCounts(144, 144); // Go forward 144 counts - delay(3000); // Wait three seconds - displayDistances(); // Display encoder counts - - Arlo.writeCounts(-72, 72); // Turn left 72 counts - delay(2000); // Wait two seconds - displayDistances(); // Display encoder counts - - Arlo.writeCounts(72, -72); // Turn right 72 counts - delay(2000); // Wait 2 seconds - displayDistances(); // Display encoder counts - - Arlo.writeCounts(-144, -144); // Back up 144 counts - delay(3000); // Wait three seconds - displayDistances(); // Display encoder counts -} - -void loop() {} // Nothing for main loop - -void displayDistances() -{ - countsLeft = Arlo.readCountsLeft(); // Get left & right encoder counts - countsRight = Arlo.readCountsRight(); - - Serial.print("countsLeft = "); // Display encoder measurements - Serial.print(countsLeft, DEC); - Serial.print(", countsRight = "); - Serial.println(countsRight, DEC); -} diff --git a/ArloRobot/Arlo-No-Surprise-Maneuvers/Arlo-No-Surprise-Maneuvers.ino b/ArloRobot/Arlo-No-Surprise-Maneuvers/Arlo-No-Surprise-Maneuvers.ino deleted file mode 100755 index 96548b5..0000000 --- a/ArloRobot/Arlo-No-Surprise-Maneuvers/Arlo-No-Surprise-Maneuvers.ino +++ /dev/null @@ -1,15 +0,0 @@ -/* - Arlo-No-Surprise-Maneuvers.ino - - Run this before ever turning on power to the Arlo's motors to prevent any - unexpected motions. -*/ - -void setup() // Setup function -{ - tone(4, 3000, 2000); // Piezospeaker beep - Serial.begin(9600); // Start terminal serial port - Serial.print("Your Arlo will stay still."); // Message -} -void loop() {} // Nothing for main loop - diff --git a/ArloRobot/Arlo-Speed-Maneuvers/Arlo-Speed-Maneuvers.ino b/ArloRobot/Arlo-Speed-Maneuvers/Arlo-Speed-Maneuvers.ino deleted file mode 100755 index 18db78a..0000000 --- a/ArloRobot/Arlo-Speed-Maneuvers/Arlo-Speed-Maneuvers.ino +++ /dev/null @@ -1,64 +0,0 @@ -/* - Arlo-Speed-Maneuvers - - Examples that use ArloRobot library to make the arlo travel in certain - speeds for certain amounts of time. -*/ - -#include // Include Arlo library -#include // Include SoftwareSerial library - -// Arlo and serial objects required -ArloRobot Arlo; // Arlo object -SoftwareSerial ArloSerial(12, 13); // Serial in I/O 12, out I/O 13 - -int countsLeft, countsRight; // Encoder counting variables -void setup() // Setup function -{ - tone(4, 3000, 2000); // Piezospeaker beep - Serial.begin(9600); // Start terminal serial port - Serial.println("Sketch running..."); // Display starting message - - ArloSerial.begin(19200); // Start DHB-10 serial com - Arlo.begin(ArloSerial); // Pass to Arlo object - - Arlo.clearCounts(); // Clear encoder counts - - Arlo.writeSpeeds(144, 144); // Go forward 144 counts/sec - delay(3000); // for three seconds - Arlo.writeSpeeds(0, 0); // Stop - delay(1000); // for one second - displayDistances(); // Display encoder counts - - Arlo.writeSpeeds(-72, 72); // Rotate seft 72 counts/sec - delay(2000); // for two seconds - Arlo.writeSpeeds(0, 0); // Stop - delay(1000); // ...for one second - displayDistances(); // Display encoder counts - - Arlo.writeSpeeds(72, -72); // Rotate right counts/sec - delay(2000); // for two seconds - Arlo.writeSpeeds(0, 0); // Stop - delay(1000); // for one second - displayDistances(); // Display encoder counts - - Arlo.writeSpeeds(-144, -144); // Go backward 144 counts/sec - delay(3000); // for three seconds - Arlo.writeSpeeds(0, 0); // Stop - delay(1000); // for one second - displayDistances(); // Display encoder counts -} - -void loop() {} // Nothing for main loop - -void displayDistances() -{ - countsLeft = Arlo.readCountsLeft(); // Get left & right encoder counts - countsRight = Arlo.readCountsRight(); - - Serial.print("countsLeft = "); // Display encoder measurements - Serial.print(countsLeft, DEC); - Serial.print(", countsRight = "); - Serial.println(countsRight, DEC); -} - diff --git a/ArloRobot/Arlo-Terminal-Communication/Arlo-Terminal-Communication.ino b/ArloRobot/Arlo-Terminal-Communication/Arlo-Terminal-Communication.ino deleted file mode 100755 index 2db0666..0000000 --- a/ArloRobot/Arlo-Terminal-Communication/Arlo-Terminal-Communication.ino +++ /dev/null @@ -1,39 +0,0 @@ -/* - Arlo-Terminal-Communication - - IMPORTANT: Set Line Ending to Carriage return in the terminal. -*/ - -#include // Include Arlo library -#include // Include SoftwareSerial library - -// Arlo and serial objects required -ArloRobot Arlo; // Arlo object -SoftwareSerial ArloSerial(12, 13); // Serial in I/O 12, out I/O 13 - -char str[64]; - -void setup() // Setup function -{ - tone(4, 3000, 2000); // Piezospeaker beep - Serial.begin(9600); // Start terminal serial port - - ArloSerial.begin(19200); // Start DHB-10 serial com - Arlo.begin(ArloSerial); // Pass to Arlo object - - Serial.println("Arlo Terminal"); // Display heading -} - -void loop() // Main loop -{ - memset(str, 0, 64); // Clear the buffer - Serial.print("> "); // Display prompt - while(Serial.available() == 0); // Wait for terminal input - Serial.readBytesUntil('\r', str, 64); // Read until carriage return - Serial.println(str); // Display what was typed - ArloSerial.print(str); // Send to Arlo's DHB-10 - ArloSerial.write('\r'); // Append with a carriage return - memset(str, 0, 64); // Clear the buffer again - ArloSerial.readBytesUntil('\r', str, 64); // Get Arlo's reply - Serial.println(str); // Display Arlo's reply -} diff --git a/ArloRobot/Arlo-Test-Arduino-DHB-10-Communication/Arlo-Test-Arduino-DHB-10-Communication.ino b/ArloRobot/Arlo-Test-Arduino-DHB-10-Communication/Arlo-Test-Arduino-DHB-10-Communication.ino deleted file mode 100755 index 6ea6ade..0000000 --- a/ArloRobot/Arlo-Test-Arduino-DHB-10-Communication/Arlo-Test-Arduino-DHB-10-Communication.ino +++ /dev/null @@ -1,35 +0,0 @@ -/* - Arlo-Test-Arduino-DHB-10-Communication - - Insert web address for instructions here. - - You'll re ready for the next step when the Arduino Terminal displays: - - fwver = 10 - hwver = 1 -*/ - -#include // Include Arlo library -#include // Include SoftwareSerial library - -ArloRobot Arlo; // Declare Arlo object -SoftwareSerial ArloSerial(12, 13); // Declare SoftwareSerial object - // DHB-10 -> I/O 12, DHB-10 <- I/O 13 -void setup() // Setup function -{ - tone(4, 3000, 2000); // Piezospeaker beep - Serial.begin(9600); // Start terminal serial port - - ArloSerial.begin(19200); // Start DHB-10 serial communication - Arlo.begin(ArloSerial); // Pass to Arlo object - - int fwver = Arlo.readFirmwareVer(); // Check DHB-10 firmware - Serial.print("fwver = "); // Display firmware version - Serial.println(fwver, DEC); - - int hwver = Arlo.readHardwareVer(); // Check DHB-10 hardware - Serial.print("hwver = "); // Display hardware version - Serial.println(hwver, DEC); -} - -void loop() {} // Nothing for main loop diff --git a/ArloRobot/Arlo-Test-Encoder-Connections/Arlo-Test-Encoder-Connections.ino b/ArloRobot/Arlo-Test-Encoder-Connections/Arlo-Test-Encoder-Connections.ino deleted file mode 100755 index 0df4bb3..0000000 --- a/ArloRobot/Arlo-Test-Encoder-Connections/Arlo-Test-Encoder-Connections.ino +++ /dev/null @@ -1,129 +0,0 @@ -/* - Arlo-Test-Encoder-Connections - - This sketch tests to make sure the Arlo's wheel encoder connections - are correct. The Arlo will not be ready for the next step until you - have verified that the number of encoder transitions (ticks) for both - wheels are positive when the wheels roll forward. - - If you have not already completed Test Arlo Motor Connections.c, - complete it first, then continue from here. - - Use the Arduino IDE’s Upload button to run this sketch. If the - Terminal displays the "Encoder connections are correct!..." - message, your Arlo is ready for the next step, which is running - navigation sketches. - - If the Terminal instead displays one or more "ERROR..." - messages, those encoder encoder connections will need to be - corrected. For example, if the messages says, "ERROR: Motor 1 - encoder connections are reversed!", you will need to unplug and - swap the two 3-wire encoder cables next to the Motor 1 terminal - on the DHB-10, swap them, and plug them back in. - - Make sure to test between each adjustment. Your arlo will not be - ready for the next step until you get the success message from - this test. -*/ - -#include // Include Arlo library -#include // Include SoftwareSerial library - -// Arlo and serial objects required -ArloRobot Arlo; // Arlo object -SoftwareSerial ArloSerial(12, 13); // Serial in I/O 12, out I/O 13 - -int countsLeft, countsRight; // Encoder counting variables -void setup() // Setup function -{ - tone(4, 3000, 2000); // Piezospeaker beep - Serial.begin(9600); // Start terminal serial port - - ArloSerial.begin(19200); // Start DHB-10 serial com - Arlo.begin(ArloSerial); // Pass to Arlo object - - Serial.println("Testing..."); // Display testing message - - Arlo.clearCounts(); // Clear encoder counts - - Arlo.writeMotorPower(32, 32); // Go forward very slowly - delay(4000); // for three seconds - Arlo.writeMotorPower(0, 0); // then stop - - countsLeft = Arlo.readCountsLeft(); // Get left & right encoder counts - countsRight = Arlo.readCountsRight(); - - Serial.print("countsLeft = "); // Display encoder measurements - Serial.print(countsLeft, DEC); - Serial.print(", countsRight = "); - Serial.println(countsRight, DEC); - Serial.println(); - - // Both distances positive? - if((countsLeft > 175) && (countsLeft < 325) - && (countsRight > 175) && (countsRight < 325)) - { - // Success message - Serial.println("Encoder connections are correct!"); - Serial.println("Your Arlo is ready for the next step."); - Serial.println(); - } - else - { - // Left encoders cables correct? - if(countsLeft > 175 && countsLeft < 325) - { - // Correct encoder message - Serial.println("Motor 1 encoder cables are connected"); - Serial.println(" correctly."); - Serial.println(); - } - // Left encoders cables swapped? - else if(countsLeft > -325 && countsLeft < -125) - { - // Swapped encoder message - Serial.println("ERROR: Motor 1 encoder connections"); - Serial.println(" are reversed!"); - Serial.println(); - } - else // Other problem - { - // Other encoder error message - Serial.println("ERROR: Motor 1 encoder values out of "); - Serial.println("range. Recheck encoder connections "); - Serial.println("and assemblies."); - Serial.println(); - } - // Right encoders cables correct? - if(countsRight > 175 && countsRight < 325) - { - // Correct encoder message - Serial.println("Motor 2 encoder cables are "); - Serial.println("connected correctly."); - Serial.println(); - } - // Right encoders cables swapped? - else if(countsRight > -325 && countsRight < -125) - { - // Swapped encoder message - Serial.println("ERROR: Motor 2 encoder connections "); - Serial.println("are reversed!"); - Serial.println(); - } - else // Other problem - { - // Other encoder error message - Serial.println("ERROR: Motor 2 encoder values "); - Serial.println("out of range. Recheck encoder "); - Serial.println("connections and assemblies."); - Serial.println(); - } - } - - Arlo.writePulseMode(); // Get ready for pulse control - - Serial.print("Test done.\n\n"); // Display status -} - -void loop() {} // Nothing for main loop - diff --git a/ArloRobot/Arlo-Test-Motor-Connections/Arlo-Test-Motor-Connections.ino b/ArloRobot/Arlo-Test-Motor-Connections/Arlo-Test-Motor-Connections.ino deleted file mode 100644 index bf35df9..0000000 --- a/ArloRobot/Arlo-Test-Motor-Connections/Arlo-Test-Motor-Connections.ino +++ /dev/null @@ -1,31 +0,0 @@ -/* - Arlo-Test-Motor-Connections - - Run this sketch to verify that your Arlo goes forward. -*/ - -#include // Include Arlo library -#include // Include SoftwareSerial library - -ArloRobot Arlo; // Declare Arlo object -SoftwareSerial ArloSerial(12, 13); // Declare SoftwareSerial object - // DHB-10 -> I/O 12, DHB-10 <- I/O 13 -void setup() // Setup function -{ - tone(4, 3000, 2000); // Piezospeaker beep - Serial.begin(9600); // Start terminal serial port - - ArloSerial.begin(19200); // Start DHB-10 serial communication - Arlo.begin(ArloSerial); // Pass to Arlo object - - Arlo.writeMotorPower(20, 20); // Go forward very slowly - delay(3000); // for three seconds - Arlo.writeMotorPower(0, 0); // then stop - - Arlo.writeMotorPower(-20, -20); // Go forward very slowly - delay(3000); // for three seconds - Arlo.writeMotorPower(0, 0); // then stop -} - -void loop() {} // Nothing for main loop - diff --git a/ArloRobot/Arlo-Test-Ping-Sensors/Arlo-Test-Ping-Sensors.ino b/ArloRobot/Arlo-Test-Ping-Sensors/Arlo-Test-Ping-Sensors.ino deleted file mode 100755 index 5f859c5..0000000 --- a/ArloRobot/Arlo-Test-Ping-Sensors/Arlo-Test-Ping-Sensors.ino +++ /dev/null @@ -1,48 +0,0 @@ -/* - Arlo-Test-Ping-Sensors -*/ - -String directions[4] = {"Front: ", // Directions for display - "Back: ", - "Left: ", - "Right: "}; -int pingPin[4] = {11, 10, 9, 8}; // Ping pins -int cmDist[4]; // Cm distances -int i = 0; // Index, stat at 0 - -void setup() // Setup function -{ - Serial.begin(9600); // Start terminal communication -} - -void loop() // Main loop -{ - cmDist[i] = pingCm(pingPin[i]); // Get distance - - Serial.print(directions[i]); // Display direction - Serial.print("cmDist["); // Display variable name - Serial.print(i); // Display variable index - Serial.print("] = "); // Display ] & = - Serial.println(cmDist[i]); // Display value - i++; // Increase index by 1 - if(i == 4) // If index is 4 - { - i = 0; // Reset index to 0 - delay(1000); // Wait a second - Serial.println(); // Print a blank line - } -} - -int pingCm(int pin) // Ping measurement function -{ - digitalWrite(pin, LOW); // Pin to output-low - pinMode(pin, OUTPUT); - delayMicroseconds(200); // Required between successive - digitalWrite(pin, HIGH); // Send high pulse - delayMicroseconds(5); // Must be at least 2 us - digitalWrite(pin, LOW); // End pulse to start ping - pinMode(pin, INPUT); // Change to input - long microseconds = pulseIn(pin, HIGH); // Wait for echo to reflect - return microseconds / 29 / 2; // Convert us echo to cm -} - diff --git a/ArloRobot/Arlo-Troubleshoot-Arduino-DHB-10-Communication/Arlo-Troubleshoot-Arduino-DHB-10-Communication.ino b/ArloRobot/Arlo-Troubleshoot-Arduino-DHB-10-Communication/Arlo-Troubleshoot-Arduino-DHB-10-Communication.ino deleted file mode 100755 index 679f42f..0000000 --- a/ArloRobot/Arlo-Troubleshoot-Arduino-DHB-10-Communication/Arlo-Troubleshoot-Arduino-DHB-10-Communication.ino +++ /dev/null @@ -1,38 +0,0 @@ -/* - Arlo-Troubleshoot-Arduino-DHB-10-Communication - - Insert web address for instructions here. - - You'll re ready for the next step when the Arduino Terminal displays: - - fwver = 10 - hwver = 1 -*/ - -#include // Include Arlo library -#include // Include SoftwareSerial library - -ArloRobot Arlo; // Declare Arlo object -SoftwareSerial ArloSerial(12, 13); // Declare SoftwareSerial object - // DHB-10 -> I/O 12, DHB-10 <- I/O 13 -void setup() // Setup function -{ - tone(4, 3000, 2000); // Piezospeaker beep - Serial.begin(9600); // Start terminal serial port - - ArloSerial.begin(19200); // Start DHB-10 serial communication - Arlo.begin(ArloSerial); // Pass to Arlo object - - int fwver = Arlo.readFirmwareVer(); // Check DHB-10 firmware - Serial.print("fwver = "); // Display firmware version - Serial.println(fwver, DEC); - Serial.println(Arlo.lastExchange); - - int hwver = Arlo.readHardwareVer(); // Check DHB-10 hardware - Serial.print("hwver = "); // Display hardware version - Serial.println(hwver, DEC); - Serial.println(Arlo.lastExchange); -} - -void loop() {} // Nothing for main loop - diff --git a/ArloRobot/Arlo-Tune-Integral-Constants/Arlo-Tune-Integral-Constants.ino b/ArloRobot/Arlo-Tune-Integral-Constants/Arlo-Tune-Integral-Constants.ino deleted file mode 100755 index 2a6790d..0000000 --- a/ArloRobot/Arlo-Tune-Integral-Constants/Arlo-Tune-Integral-Constants.ino +++ /dev/null @@ -1,47 +0,0 @@ -/* - Arlo-Tune-Integral-Constants - - Set control system constants that give the extra push to get to the - final position and control the allowable positional error. -*/ - -#include // Include Arlo library -#include // Include SoftwareSerial library - -// Arlo and serial objects required -ArloRobot Arlo; // Arlo object -SoftwareSerial ArloSerial(12, 13); // Serial in I/O 12, out I/O 13 - -void setup() // Setup function -{ - tone(4, 3000, 2000); // Piezospeaker beep - Serial.begin(9600); // Start terminal serial port - - ArloSerial.begin(19200); // Start DHB-10 serial com - Arlo.begin(ArloSerial); // Pass to Arlo object - - int ki = Arlo.readConfig("KI"); // Check KI - Serial.print("KI = "); // Display KI - Serial.println(ki); - - int dz = Arlo.readConfig("DZ"); // Check DZ - Serial.print("DZ = "); // Display DZ - Serial.println(dz); - - Arlo.writeConfig("KI", 65); // Change KI - Arlo.writeConfig("DZ", 1); // Change DZ - - // Uncomment code below to store new settings to the DHB-10's EEPROM. - // Tip: To uncomment, replace the two * characters with / characters. - - /* - Arlo.storeConfig("KI"); // New KI to DHB-10 EEPROM - Serial.println(Arlo.lastExchange); - - Arlo.storeConfig("DZ"); // New DZ to DHB-10 EEPROM - Serial.println(Arlo.lastExchange); - */ -} - -void loop() {} // Nothing for main loop - diff --git a/ArloRobot/ArloRobot.cpp b/ArloRobot/ArloRobot.cpp deleted file mode 100755 index 324db2b..0000000 --- a/ArloRobot/ArloRobot.cpp +++ /dev/null @@ -1,302 +0,0 @@ -/** - @file ArloRobot.cpp - - @author Parallax Inc. - - @brief Arlo Robot (with DHB-10 motor controller) functions for Arduino. - - @version 0.5 - - @copyright - Copyright (c) Parallax Inc 2016. All rights MIT licensed; - see license.txt. -*/ - -#include "ArloRobot.h" - -// Set up/tear down - -void ArloRobot::begin(SoftwareSerial &serial) -{ - dhb10 = &serial; - dhb10->print("txpin ch2\r"); // Receive on different pin - delay(20); // Wait for reply - dhb10->flush(); // Clear input buffer -} - -// -void ArloRobot::end() -{ - dhb10->end(); -} -// -// Movements - -void ArloRobot::writeCounts(long left, long right) -{ - paramVal[0] = left; - paramVal[1] = right; - paramVal[2] = topSpeed; - com("MOVE", 3, 0); -} - -void ArloRobot::writeSpeeds(int left, int right) -{ - left = constrain(left, -topSpeed, topSpeed); - right = constrain(right, -topSpeed, topSpeed); - paramVal[0] = left; - paramVal[1] = right; - com("GOSPD", 2, 0); -} - -void ArloRobot::writeMotorPower(int left, int right) -{ - left = constrain(left, -DHB10_MAX_MOTOR_PWR, DHB10_MAX_MOTOR_PWR); - right = constrain(right, -DHB10_MAX_MOTOR_PWR, DHB10_MAX_MOTOR_PWR); - paramVal[0] = left; - paramVal[1] = right; - com("GO", 2, 0); -} - - -// Measurements - -int ArloRobot::readCountsLeft() -{ - - #ifdef DISPLAY_ACTIVITY - Serial.print("left strcmp "); - Serial.println(strcmp("DIST", lastCommand)); - Serial.print("left millis "); - Serial.println(millis() - lastReadCount); - #endif - - if(((millis() - lastReadCount) > DT_PREV_ENCODER_CHECK || (strcmp("DIST", lastCommand)))) - { - com("DIST", 0, 2); - } - lastReadCount = millis(); - return returnVal[0]; -} - -int ArloRobot::readCountsRight() -{ - #ifdef DISPLAY_ACTIVITY - Serial.print("right strcmp "); - Serial.println(strcmp("DIST", lastCommand)); - Serial.print("right millis "); - Serial.println(millis() - lastReadCount); - #endif - - if(((millis() - lastReadCount) > DT_PREV_ENCODER_CHECK || (strcmp("DIST", lastCommand)))) - { - com("DIST", 0, 2); - } - lastReadCount = millis(); - return returnVal[1]; -} - -int ArloRobot::readSpeedLeft() -{ - if(((millis() - lastReadCount) > DT_PREV_ENCODER_CHECK || (strcmp("SPD", lastCommand)))) - { - com("SPD", 0, 2); - } - lastReadCount = millis(); - return returnVal[0]; -} - -int ArloRobot::readSpeedRight() -{ - if(((millis() - lastReadCount) > DT_PREV_ENCODER_CHECK || (strcmp("SPD", lastCommand)))) - { - com("SPD", 0, 2); - } - lastReadCount = millis(); - return returnVal[1]; -} - -void ArloRobot::writePulseMode() -{ - com("PULSE", 0, 0); -} - - -// Information - -int ArloRobot::readFirmwareVer() -{ - com("VER", 0, 1); - return returnVal[0]; -} - -int ArloRobot::readHardwareVer() -{ - com("HWVER", 0, 1); - return returnVal[0]; -} - -int ArloRobot::readSpeedLimit() -{ - return topSpeed; -} - - -// Configuration -void ArloRobot::writeConfig(char *configString, int value) -{ - paramVal[0] = value; - com(configString, 1, 0); -} - -int ArloRobot::readConfig(char *configString) -{ - com(configString, 0, 1); - return returnVal[0]; -} - -void ArloRobot::writeSpeedLimit(int countsPerSecond) -{ - topSpeed = countsPerSecond; -} - -void ArloRobot::clearCounts() -{ - com("RST", 0, 0); -} - - -//Nonvolatile Configuration Storage - -void ArloRobot::storeConfig(char *configString) -{ - char temp[] = {'S','T','O','R','E', ' ' ,0,0,0,0,0,0,0,0,0,0,0} ; - strcpy(&temp[6], configString); - com(temp, 0, 0); -} - -void ArloRobot::restoreConfig() -{ - com("RESTORE", 0, 0); -} - - -//Private - -char *ArloRobot::com(char *command, int paramCount, int retCount) -{ - dhb10->listen(); - memset(c, 0, LAST_EXCG_STR_LEN); - memset(lastExchange, ' ', LAST_EXCG_STR_LEN - 1); - lastExchange[LAST_EXCG_STR_LEN - 1] = 0; - int idx = 0; - - #ifdef DISPLAY_ACTIVITY - Serial.print(command); - for(int i = 0; i < paramCount; i++) - { - Serial.print(" "); - Serial.print(paramVal[i], DEC); - } - Serial.print("\\r "); // Display message - #endif - - dhb10->print(command); // Send message to DHB-10 - strcpy(lastExchange, command); - idx = strlen(command); - for(int i = 0; i < paramCount; i++) - { - dhb10->print(" "); - lastExchange[idx++] = ' '; - //lastExchange[idx] = 0; - dhb10->print(paramVal[i], DEC); - //idx += sprintf(&lastExchange[idx], "%d", paramVal[i]); - char *p = itoa(paramVal[i], &lastExchange[idx], 10); - idx += strlen(p); - } - dhb10->print('\r'); // Send message to DHB-10 - lastExchange[idx++] = '\\'; - lastExchange[idx++] = 'r'; - //lastExchange[idx++] = 0; - if(idx < LAST_EXCG_TX_MIN) idx = LAST_EXCG_TX_MIN; - - long ti = millis(); - dhb10->readBytes(c, 1); // Get first byte of reply - long tf = millis(); - - #ifdef DISPLAY_ACTIVITY - Serial.print("millis = "); - Serial.println(tf - ti); - Serial.print("Dec = "); - Serial.println(c[0], DEC); - #endif - - if((tf - ti) > timeoutDHB10) - { - #ifdef DISPLAY_ACTIVITY - Serial.println("No reply (timeout)"); // Display timeout message - #endif - strcpy(&lastExchange[idx], "No reply (timeout)."); - } - else if(c[0] == 0) - { - #ifdef DISPLAY_ACTIVITY - Serial.println("No reply"); // Display timeout message - #endif - strcpy(&lastExchange[idx], "No reply."); - } - else if(c[0] == '\r') // If DHB-10 replied - { - #ifdef DISPLAY_ACTIVITY - Serial.println("\\r"); // Display terminating character - #endif - strcpy(&lastExchange[idx], "\\r"); - } - else if(c[0] == 'E') - { - dhb10->readBytesUntil('\r', &c[1], EXCG_STR_LEN - 2); - - #ifdef DISPLAY_ACTIVITY - Serial.write(c); - Serial.println("\\r"); - #endif - strcpy(&lastExchange[idx], c); - idx += strlen(c); - strcpy(&lastExchange[idx], "\\r"); - } - else - { - dhb10->readBytesUntil('\r', &c[1], EXCG_STR_LEN - 2); - #ifdef DISPLAY_ACTIVITY - Serial.write(c); - Serial.println(">>> Else <<<"); - Serial.println("\\r"); - #endif - strcpy(&lastExchange[idx], c); - idx += strlen(c); - strcpy(&lastExchange[idx], "\\r"); - } - - for(int i = 0; i < retCount; i++) returnVal[i] = 0; - - int j = -1; - for(int i = 0; i < retCount; i++) - { - while(isAlpha(c[++j])); - if(isdigit(c[j]) || (c[j] == '-')) - { - //returnVal[i] = atoi(&c[j]); - returnVal[i] = atol(&c[j]); - while(isDigit(c[++j])); - } - #ifdef DISPLAY_ACTIVITY - Serial.print("rv["); - Serial.print(i, DEC); - Serial.print("] = "); - Serial.println(returnVal[i], DEC); - #endif - } - strcpy(lastCommand, command); - //lastCommand = command; - return c; -} diff --git a/ArloRobot/ArloRobot.h b/ArloRobot/ArloRobot.h deleted file mode 100755 index 4105fe9..0000000 --- a/ArloRobot/ArloRobot.h +++ /dev/null @@ -1,84 +0,0 @@ -/** - @file ArloRobot.h - - @author Parallax Inc. - - @brief Arlo Robot (with DHB-10 motor controller) functions for Arduino. - - @version 0.5 - - @copyright - Copyright (c) Parallax Inc 2016. All rights MIT licensed; - see license.txt. -*/ - -#ifndef PARALLAX_ARLO_h -#define PARALLAX_ARLO_h - -//#define DISPLAY_ACTIVITY - -#include -#include - -#define DHB10_MAX_MOTOR_PWR 127 -#define DT_PREV_ENCODER_CHECK 40 -#define LAST_EXCG_STR_LEN 96 -#define EXCG_STR_LEN 96 -#define LAST_EXCG_TX_MIN 16 -#define DEFAULT_TOP_SPEED 200 - -class ArloRobot -{ - public: - char lastExchange[LAST_EXCG_STR_LEN]; - int timeoutDHB10 = 900; - public: - // Set up/tear down - void begin(SoftwareSerial &serial); - void end(); - - // Movements - void writeCounts(long left, long right); - void writeSpeeds(int left, int right); - void writeMotorPower(int left, int right); - - // Measurements - int readCountsLeft(); - int readCountsRight(); - int readSpeedLeft(); - int readSpeedRight(); - - // Communication Modes - void writePulseMode(); - - // Information - int readFirmwareVer(); - int readHardwareVer(); - int readSpeedLimit(); - - // Configuration - void writeConfig(char *configString, int value); - int readConfig(char *configString); - void writeSpeedLimit(int countsPerSecond); - void clearCounts(); - - //Nonvolatile Configuration Storage - void storeConfig(char *configString); - void restoreConfig(); - - void checkCharacter(char c, int j); - - private: - SoftwareSerial *dhb10; - long returnVal[3]; - long paramVal[3]; - char c[EXCG_STR_LEN]; - unsigned long lastReadCount = 0; - char lastCommand[12] = {0,0,0,0,0,0,0,0,0,0,0,0}; - int topSpeed = DEFAULT_TOP_SPEED; - - private: - char *com(char *command, int paramCount, int retCount); -}; - -#endif diff --git a/ArloRobot/ForwardLeftRightBackward/ForwardLeftRightBackward.ino b/ArloRobot/ForwardLeftRightBackward/ForwardLeftRightBackward.ino deleted file mode 100755 index 61f3f8d..0000000 --- a/ArloRobot/ForwardLeftRightBackward/ForwardLeftRightBackward.ino +++ /dev/null @@ -1,44 +0,0 @@ -// Robotics with the BOE Shield - ForwardLeftRightBackward -// Move forward, left, right, then backward for testing and tuning. - -#include // Include servo library - -Servo servoLeft; // Declare left and right servos -Servo servoRight; - -void setup() // Built-in initialization block -{ - tone(4, 3000, 1000); // Play tone for 1 second - delay(1000); // Delay to finish tone - - servoLeft.attach(13); // Attach left signal to pin 13 - servoRight.attach(12); // Attach right signal to pin 12 - - // Full speed forward - servoLeft.writeMicroseconds(1700); // Left wheel counterclockwise - servoRight.writeMicroseconds(1300); // Right wheel clockwise - delay(2000); // ...for 2 seconds - - // Turn left in place - servoLeft.writeMicroseconds(1300); // Left wheel clockwise - servoRight.writeMicroseconds(1300); // Right wheel clockwise - delay(600); // ...for 0.6 seconds - - // Turn right in place - servoLeft.writeMicroseconds(1700); // Left wheel counterclockwise - servoRight.writeMicroseconds(1700); // Right wheel counterclockwise - delay(600); // ...for 0.6 seconds - - // Full speed backward - servoLeft.writeMicroseconds(1300); // Left wheel clockwise - servoRight.writeMicroseconds(1700); // Right wheel counterclockwise - delay(2000); // ...for 2 seconds - - servoLeft.detach(); // Stop sending servo signals - servoRight.detach(); -} - -void loop() // Main loop auto-repeats -{ // Empty, nothing needs repeating -} - diff --git a/ArloRobot/keywords.txt b/ArloRobot/keywords.txt deleted file mode 100755 index e61416d..0000000 --- a/ArloRobot/keywords.txt +++ /dev/null @@ -1,18 +0,0 @@ -ArloRobot KEYWORD1 -begin KEYWORD2 -end KEYWORD2 -writeCounts KEYWORD2 -writeSpeeds KEYWORD2 -writeMotorPower KEYWORD2 -readCountsLeft KEYWORD2 -readCountsRight KEYWORD2 -readSpeedLeft KEYWORD2 -readSpeedRight KEYWORD2 -writePulseMode KEYWORD2 -readFirmwareVer KEYWORD2 -readHardwareVer KEYWORD2 -readSpeedLimit KEYWORD2 -writeConfig KEYWORD2 -readConfig KEYWORD2 -writeSpeedLimit KEYWORD2 -clearCounts KEYWORD2 \ No newline at end of file diff --git a/ArloRobot/license.txt b/ArloRobot/license.txt deleted file mode 100755 index 1207304..0000000 --- a/ArloRobot/license.txt +++ /dev/null @@ -1,23 +0,0 @@ -ArloRobot Library -Copyright (c) 2016, Parallax Inc. -All rights reservied. - -TERMS OF USE: MIT License - -Permission is hereby granted, free of charge, to any person obtaining a -copy of this software and associated documentation files (the "Software"), -to deal in the Software without restriction, including without limitation -the rights to use, copy, modify, merge, publish, distribute, sublicense, -and/or sell copies of the Software, and to permit persons to whom the -Software is furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL -THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER -DEALINGS IN THE SOFTWARE. diff --git a/robot/__init__.py b/robot/__init__.py new file mode 100644 index 0000000..d931080 --- /dev/null +++ b/robot/__init__.py @@ -0,0 +1 @@ +from robot import * \ No newline at end of file diff --git a/robot.py b/robot/robot.py similarity index 100% rename from robot.py rename to robot/robot.py diff --git a/serialRobotArlo.ino b/serialRobotArlo.ino deleted file mode 100644 index f45a40b..0000000 --- a/serialRobotArlo.ino +++ /dev/null @@ -1,427 +0,0 @@ -// version 1.1.1 Kim Steenstrup Pedersen, DIKU, August 2017, August 2020 -// -// This sketch converts an Arduino Uno with a BOE Shield and Arlo DBH-10 motor bridge controller -// into a serial robot controller. The sketch is inspired by Frindo serial_Robot. -// -// The sketch provides basic motion control as well as access to Arduinos analog inputs over the -// serial or USB port for robotics applications. The simple commands means that the controller can be -// interfaced to anything with a serial or USB port including a PC, Raspberry Pi, microcontroller -// or another Arduino. -// -// This sketch uses the ArloRobot library and provides -// a mix of stepped commands, continuous commands and analog measurements. The movement of your robot -// is controlled by sending a single character command over the serial port. -// -// Stepped commands make your robot move in a specific way for a fixed period of time and speed. The length of -// time is set using the "step_time" and "turn_time" variables. All times are in ms (milliseconds). -// The step commands are: -// -// f = Forward, b = Backwards, l = rotate Left, r = rotate Right -// -// Continuous commands make your robot move in the same way continuously until you send a stop or other command: -// -// s = Stop, g = Go, v = Reverse, n = rotate Left, m = rotate Right -// -// The speed of the motors during all movements is set by the "motor_speed" and "turn_speed". Motor speeds -// are measured in wheel encoder counts per seconds. There are 144 counts for a complete wheel revolution. -// -// Set the motor_speed and turn_speed with these commands followed by an integer representing the wanted speed. -// -// z = motor_speed , x = turn_speed -// -// Set the step_time used by f and b and the turn_time used by l and r followed by an integer representing the -// wanted time in miliseconds: -// -// t = step_time, y = turn_time -// -// Start motors with different speeds and directions -// -// d = Go differential -// -// followed by 4 integers separated by commas: powerLeft, powerRight, dirLeft (0 = reverse and 1 = forward) -// and dirRight (0 = reverse and 1 = forward). Example: -// -// d127,72,1,0 -// -// Make a distance measurement using the sonar ping sensors. The distance will be returned in mm. -// -// 0 = Front, 1 = Back, 2 = Left, 3 = Right -// -// Read the left wheel encoder (there are 144 counts for a complete wheel revolution and this returns the counts -// since last clear counts command) by -// -// e0 = left wheel encoder count -// -// and read the right wheel encoder by -// -// e1 = right wheel encoder count -// -// Clear the internal wheel encoder counts by -// -// c = clear the encoder counts -// -// Shutdown the DBH-10 motor bridge card and clean-up -// -// k = shutdown - - - -#include -#include - -// Set wheel encoder pins -#define encoderLA 4 // Left A channel pin -#define encoderLB 2 // Left B channel pin (interrupt) -#define encoderRA 7 // Right A channel pin -#define encoderRB 3 // Right B channel pin (interrupt) - - -// Arlo and serial objects required -ArloRobot Arlo; // Arlo object -SoftwareSerial ArloSerial(12, 13); // Serial in I/O 12, out I/O 13 - - -// define variables used -int Response; -int motor_speed = 32; // encoder counter / sec -int turn_speed = 32; // encoder counter / sec -int step_time = 1000; //mSec -int turn_time = 1000; //mSec - -volatile long left_encoder = 0; // Left wheel encoder counter -volatile long right_encoder = 0; // Right wheel encoder counter - - -// Interrupt handlers -void runEncoderLeft() -{ - if (digitalRead(encoderLA) == digitalRead(encoderLB)) - left_encoder++; // count up if both encoder pins are HIGH on pin change interrupt - else - left_encoder--; //count down if pins are HIGH and LOW -} - -void runEncoderRight() -{ - if (digitalRead(encoderRA) == digitalRead(encoderRB)) - right_encoder--; // count down if both encoder pins are HIGH on pin change interrupt - else - right_encoder++; //count up if pins are HIGH and LOW -} - - - -void setup() { - Serial.begin(9600); // set up serial port - - ArloSerial.begin(19200); // Start DHB-10 serial com - Arlo.begin(ArloSerial); // Pass to Arlo object - - // Wheel encoder pins - pinMode(encoderLA, INPUT); - pinMode(encoderLB, INPUT); - pinMode(encoderRA, INPUT); - pinMode(encoderRB, INPUT); - - // Set wheel encoder interrupt handlers - attachInterrupt(digitalPinToInterrupt(encoderLB), runEncoderLeft, RISING); - attachInterrupt(digitalPinToInterrupt(encoderRB), runEncoderRight, RISING); -} - -void loop() { - - if (Serial.available() > 0) // if something has been received - { - int incoming = Serial.read(); // go read it - - if ((char)incoming == 'f') - { - stepMove(step_time, motor_speed, motor_speed); - - Serial.println("Forward"); - } - - else if ((char)incoming == 'b') - { - stepMove(step_time, -motor_speed, -motor_speed); - Serial.println("Back"); - } - - else if ((char)incoming == 'l') - { - stepMove(turn_time, -turn_speed, turn_speed); - Serial.println("Left"); - } - - else if ((char)incoming == 'r') - { - stepMove(turn_time, turn_speed, -turn_speed); - Serial.println("Right"); - } - - else if ((char)incoming == 's') - { - //Arlo.writeSpeeds(0,0); - Arlo.writeMotorPower(0,0); - Serial.println("Stop"); - } - - else if ((char)incoming == 'g') - { - Arlo.writeSpeeds(motor_speed, motor_speed); - Serial.println("Go"); - } - - else if ((char)incoming == 'v') - { - Arlo.writeSpeeds(-motor_speed, -motor_speed); - Serial.println("Reverse"); - } - - else if ((char)incoming == 'm') - { - Arlo.writeSpeeds(turn_speed, -turn_speed); - Serial.println("Clockwise"); - } - - else if ((char)incoming == 'n') - { - Arlo.writeSpeeds(-turn_speed, turn_speed); - Serial.println("Counter Clockwise"); - } - - else if ((char)incoming == '0') - { - //Response = pingCm(11); // Front sensor - Response = pingMm(11); // Front sensor - Serial.println(Response); - } - - else if ((char)incoming == '1') - { - //Response = pingCm(10); // Back sensor - Response = pingMm(10); // Back sensor - Serial.println(Response); - } - - else if ((char)incoming == '2') - { - //Response = pingCm(9); // Left sensor - Response = pingMm(9); // Left sensor - Serial.println(Response); - } - - else if ((char)incoming == '3') - { - //Response = pingCm(8); // Right sensor - Response = pingMm(8); // Right sensor - Serial.println(Response); - } - - else if ((char)incoming == 'z') - { - String inString = ""; - int len = 4; - char inChars[len]; - Serial.readBytesUntil('\n', inChars, len); - for (int i=0; i < len+1; i++) { - if (isDigit(inChars[i])) { - inString += (char)inChars[i]; - } - } - motor_speed = inString.toInt(); - Serial.print("Setting motor speed to "); - Serial.println(motor_speed); - } - - else if ((char)incoming == 'x') - { - String inString = ""; - int len = 4; - char inChars[len]; - Serial.readBytesUntil('\n', inChars, len); - for (int i=0; i < len+1; i++) { - if (isDigit(inChars[i])) { - inString += (char)inChars[i]; - } - } - turn_speed = inString.toInt(); - Serial.print("Setting turn speed to "); - Serial.println(turn_speed); - } - - else if ((char)incoming == 't') - { - String inString = ""; - int len = 5; - char inChars[len]; - Serial.readBytesUntil('\n', inChars, len); - for (int i=0; i < len; i++) { - if (isDigit(inChars[i])) { - inString += (char)inChars[i]; - } - } - step_time = inString.toInt(); - Serial.print("Setting step_time to "); - Serial.println(step_time); - } - - else if ((char)incoming == 'y') - { - String inString = ""; - int len = 5; - char inChars[len]; - Serial.readBytesUntil('\n', inChars, len); - for (int i=0; i < len; i++) { - if (isDigit(inChars[i])) { - inString += (char)inChars[i]; - } - } - turn_time = inString.toInt(); - Serial.print("Setting turn_time to "); - Serial.println(turn_time); - } - - else if ((char)incoming == 'd') - { - int params[4]; - int idx = 0; - String inString = ""; - - int len = 13; - char inChars[len]; - int numBytes = Serial.readBytesUntil('\n', inChars, len); - for (int i=0; i < numBytes; i++) { - if (isDigit(inChars[i])) { - inString += (char)inChars[i]; - } else if (inChars[i] ==',') { - params[idx] = inString.toInt(); - inString = ""; - idx++; - } - } - - if ((char)inString[0] == 48) { // ASCII '0'=48 - params[idx] = 0; - } else { // Assuming it is '1' - params[idx] = 1; - } - - - idx++; - - if (idx == 4) { - int dirLeft = 1; - if (params[2] == 0) - dirLeft = -1; - int dirRight = 1; - if (params[3] == 0) - dirRight = -1; - - //Arlo.writeSpeeds(dirLeft*params[0], dirRight*params[1]); - - Arlo.writeMotorPower(dirLeft*params[0], dirRight*params[1]); - - Serial.print("Go diff "); - Serial.print(params[0]); - Serial.print(","); - Serial.print(params[1]); - Serial.print(","); - Serial.print(params[2]); - Serial.print(","); - Serial.println(params[3]); - } else { - Serial.print("ERROR: "); - for (int i=0; i < len; i++) { - inString += (char)inChars[i]; - } - Serial.println(inString); - } - } - - else if ((char)incoming == 'c') - { - // Clear the wheel encoder counts - //Arlo.clearCounts(); - // Critical region - noInterrupts(); - left_encoder = 0; - right_encoder = 0; - interrupts(); - Serial.println("Resetting counts"); - } - - else if ((char)incoming == 'e') - { - // Read the wheel encoder counts - - String inString = ""; - int len = 1; - char inChars[len]; - Serial.readBytesUntil('\n', inChars, len); - for (int i=0; i < len; i++) { - if (isDigit(inChars[i])) { - inString += (char)inChars[i]; - } - } - - if (inString.toInt() == 0) { - //Serial.println(Arlo.readCountsLeft()); - noInterrupts(); - // Inside critical region - long counts = left_encoder; - interrupts(); - Serial.println(counts); - } else if (inString.toInt() == 1) { - //Serial.println(Arlo.readCountsRight()); - noInterrupts(); - // Inside critical region - long counts = right_encoder; - interrupts(); - Serial.println(counts); - } else { - Serial.println("ERROR - unknown wheel encoder"); - } - } - else if ((char)incoming == 'k') - { - Arlo.end(); - Serial.println("Shutdown"); - } - } - -} - -void stepMove(unsigned int dT, int speedLeft, int speedRight) { - Arlo.writeSpeeds(speedLeft, speedRight); - delay(dT); - Arlo.writeSpeeds(0, 0); -} - - -int pingCm(int pin) // Ping measurement function -{ - digitalWrite(pin, LOW); // Pin to output-low - pinMode(pin, OUTPUT); - delayMicroseconds(200); // Required between successive - digitalWrite(pin, HIGH); // Send high pulse - delayMicroseconds(5); // Must be at least 2 us - digitalWrite(pin, LOW); // End pulse to start ping - pinMode(pin, INPUT); // Change to input - long microseconds = pulseIn(pin, HIGH); // Wait for echo to reflect - return microseconds / 29 / 2; // Convert us echo to cm -} - -int pingMm(int pin) // Ping measurement function -{ - digitalWrite(pin, LOW); // Pin to output-low - pinMode(pin, OUTPUT); - delayMicroseconds(200); // Required between successive - digitalWrite(pin, HIGH); // Send high pulse - delayMicroseconds(5); // Must be at least 2 us - digitalWrite(pin, LOW); // End pulse to start ping - pinMode(pin, INPUT); // Change to input - long microseconds = pulseIn(pin, HIGH); // Wait for echo to reflect - float fmicrosecs = microseconds / 29.0f / 2.0f * 10.0f; - return (int)fmicrosecs; // Convert US echo to mm -} diff --git a/square.py b/square.py new file mode 100644 index 0000000..8a9ee22 --- /dev/null +++ b/square.py @@ -0,0 +1,7 @@ +import robot + +def main(): + pass + +if __name__ == "__main__": + main()