'init commit'
This commit is contained in:
NikolajDanger
427
serialRobotArlo.ino
Normal file
427
serialRobotArlo.ino
Normal file
@ -0,0 +1,427 @@
|
||||
// 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 <ArloRobot.h>
|
||||
#include <SoftwareSerial.h>
|
||||
|
||||
// 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
|
||||
}
|
||||
Reference in New Issue
Block a user