✨
This commit is contained in:
BIN
ArloRobot/.DS_Store
vendored
Normal file
BIN
ArloRobot/.DS_Store
vendored
Normal file
Binary file not shown.
31
ArloRobot/Arlo-Change-Pulse-Scale/Arlo-Change-Pulse-Scale.ino
Executable file
31
ArloRobot/Arlo-Change-Pulse-Scale/Arlo-Change-Pulse-Scale.ino
Executable file
@ -0,0 +1,31 @@
|
||||
/*
|
||||
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 <ArloRobot.h> // Include Arlo library
|
||||
#include <SoftwareSerial.h> // 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
|
||||
|
56
ArloRobot/Arlo-Distance-Maneuvers/Arlo-Distance-Maneuvers.ino
Executable file
56
ArloRobot/Arlo-Distance-Maneuvers/Arlo-Distance-Maneuvers.ino
Executable file
@ -0,0 +1,56 @@
|
||||
/*
|
||||
Arlo-Distance-Maneuvers
|
||||
|
||||
Examples that use serial communication to make the arlo travel in certain
|
||||
distances.
|
||||
*/
|
||||
|
||||
|
||||
#include <ArloRobot.h> // Include Arlo library
|
||||
#include <SoftwareSerial.h> // 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);
|
||||
}
|
15
ArloRobot/Arlo-No-Surprise-Maneuvers/Arlo-No-Surprise-Maneuvers.ino
Executable file
15
ArloRobot/Arlo-No-Surprise-Maneuvers/Arlo-No-Surprise-Maneuvers.ino
Executable file
@ -0,0 +1,15 @@
|
||||
/*
|
||||
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
|
||||
|
64
ArloRobot/Arlo-Speed-Maneuvers/Arlo-Speed-Maneuvers.ino
Executable file
64
ArloRobot/Arlo-Speed-Maneuvers/Arlo-Speed-Maneuvers.ino
Executable file
@ -0,0 +1,64 @@
|
||||
/*
|
||||
Arlo-Speed-Maneuvers
|
||||
|
||||
Examples that use ArloRobot library to make the arlo travel in certain
|
||||
speeds for certain amounts of time.
|
||||
*/
|
||||
|
||||
#include <ArloRobot.h> // Include Arlo library
|
||||
#include <SoftwareSerial.h> // 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);
|
||||
}
|
||||
|
39
ArloRobot/Arlo-Terminal-Communication/Arlo-Terminal-Communication.ino
Executable file
39
ArloRobot/Arlo-Terminal-Communication/Arlo-Terminal-Communication.ino
Executable file
@ -0,0 +1,39 @@
|
||||
/*
|
||||
Arlo-Terminal-Communication
|
||||
|
||||
IMPORTANT: Set Line Ending to Carriage return in the terminal.
|
||||
*/
|
||||
|
||||
#include <ArloRobot.h> // Include Arlo library
|
||||
#include <SoftwareSerial.h> // 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
|
||||
}
|
@ -0,0 +1,35 @@
|
||||
/*
|
||||
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 <ArloRobot.h> // Include Arlo library
|
||||
#include <SoftwareSerial.h> // 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
|
129
ArloRobot/Arlo-Test-Encoder-Connections/Arlo-Test-Encoder-Connections.ino
Executable file
129
ArloRobot/Arlo-Test-Encoder-Connections/Arlo-Test-Encoder-Connections.ino
Executable file
@ -0,0 +1,129 @@
|
||||
/*
|
||||
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 <ArloRobot.h> // Include Arlo library
|
||||
#include <SoftwareSerial.h> // 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
|
||||
|
@ -0,0 +1,31 @@
|
||||
/*
|
||||
Arlo-Test-Motor-Connections
|
||||
|
||||
Run this sketch to verify that your Arlo goes forward.
|
||||
*/
|
||||
|
||||
#include <ArloRobot.h> // Include Arlo library
|
||||
#include <SoftwareSerial.h> // 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
|
||||
|
48
ArloRobot/Arlo-Test-Ping-Sensors/Arlo-Test-Ping-Sensors.ino
Executable file
48
ArloRobot/Arlo-Test-Ping-Sensors/Arlo-Test-Ping-Sensors.ino
Executable file
@ -0,0 +1,48 @@
|
||||
/*
|
||||
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
|
||||
}
|
||||
|
@ -0,0 +1,38 @@
|
||||
/*
|
||||
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 <ArloRobot.h> // Include Arlo library
|
||||
#include <SoftwareSerial.h> // 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
|
||||
|
47
ArloRobot/Arlo-Tune-Integral-Constants/Arlo-Tune-Integral-Constants.ino
Executable file
47
ArloRobot/Arlo-Tune-Integral-Constants/Arlo-Tune-Integral-Constants.ino
Executable file
@ -0,0 +1,47 @@
|
||||
/*
|
||||
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 <ArloRobot.h> // Include Arlo library
|
||||
#include <SoftwareSerial.h> // 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
|
||||
|
302
ArloRobot/ArloRobot.cpp
Executable file
302
ArloRobot/ArloRobot.cpp
Executable file
@ -0,0 +1,302 @@
|
||||
/**
|
||||
@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;
|
||||
}
|
84
ArloRobot/ArloRobot.h
Executable file
84
ArloRobot/ArloRobot.h
Executable file
@ -0,0 +1,84 @@
|
||||
/**
|
||||
@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 <SoftwareSerial.h>
|
||||
#include <arduino.h>
|
||||
|
||||
#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
|
44
ArloRobot/ForwardLeftRightBackward/ForwardLeftRightBackward.ino
Executable file
44
ArloRobot/ForwardLeftRightBackward/ForwardLeftRightBackward.ino
Executable file
@ -0,0 +1,44 @@
|
||||
// Robotics with the BOE Shield - ForwardLeftRightBackward
|
||||
// Move forward, left, right, then backward for testing and tuning.
|
||||
|
||||
#include <Servo.h> // 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
|
||||
}
|
||||
|
18
ArloRobot/keywords.txt
Executable file
18
ArloRobot/keywords.txt
Executable file
@ -0,0 +1,18 @@
|
||||
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
|
23
ArloRobot/license.txt
Executable file
23
ArloRobot/license.txt
Executable file
@ -0,0 +1,23 @@
|
||||
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.
|
Reference in New Issue
Block a user