commit a0c1548ac66418e70d01bd61151cce32d49ca437 Author: NikolajDanger Date: Fri Sep 2 10:35:07 2022 +0200 'init commit' diff --git a/__pycache__/robot.cpython-310.pyc b/__pycache__/robot.cpython-310.pyc new file mode 100644 index 0000000..322cc71 Binary files /dev/null and b/__pycache__/robot.cpython-310.pyc differ diff --git a/robot.py b/robot.py new file mode 100644 index 0000000..7a0377b --- /dev/null +++ b/robot.py @@ -0,0 +1,176 @@ +# Arlo Robot Controller + +from time import sleep +import serial + + + +class Robot(object): + """ + Defines the Arlo robot API + + DISCLAIMER: This code does not contain error checking - it is the + responsibility of the caller to ensure proper parameters and not to send + commands to the Arduino too frequently (give it time to process the command + by adding a short sleep wait statement). Failure to do some may lead to + strange robot behaviour. + + In case you experience trouble - consider using only commands that do not + use the wheel encoders. + """ + def __init__(self, port = '/dev/ttyACM0'): + """ + The constructor port parameter can be changed from default value if you + want to control the robot directly from your labtop (instead of from + the on-board raspberry pi). The value of port should point to the USB + port on which the robot Arduino is connected. + """ + self.port = port + + # 1 sec. timeout, wait until data is received or until timeout + # self.serialRead = serial.Serial(self.port,9600, timeout=1) + + # No timeout, wait forever or until data is received + self.serial_read = serial.Serial(self.port,9600, timeout=None) + + # Wait if serial port is not open yet + while not self.serial_read.isOpen(): + sleep(1) + + print("Waiting for serial port connection ...") + sleep(2) + + print("Running ...") + + def __del__(self): + print("Shutting down the robot ...") + + sleep(0.05) + print(self.stop()) + sleep(0.1) + + cmd='k\n' + print((self.send_command(cmd))) + self.serial_read.close() + + def send_command(self, cmd, sleep_ms=0.0): + """Sends a command to the Arduino robot controller""" + self.serial_read.write(cmd.encode('ascii')) + sleep(sleep_ms) + str_val=self.serial_read.readline() + return str_val + + + def _power_checker(self, power): + """ + Checks if a power value is in the set {0, [40;127]}. + This is an internal utility function. + """ + return (power == 0) or (power >=40 and power <=127) + + def go_diff(self, power_left, power_right, dir_left, dir_right): + """ + Start left motor with motor power powerLeft (in {0, [40;127]} and the + numbers must be integer) and direction dirLeft (0=reverse, 1=forward) + and right motor with motor power powerRight (in {0, [40;127]} and the + numbers must be integer) and direction dirRight (0=reverse, 1=forward). + + The Arlo robot may blow a fuse if you run the motors at less than 40 in + motor power, therefore choose either power = 0 or 40 < power <= 127. + + This does NOT use wheel encoders. + """ + + if ( + (not self._power_checker(power_left)) or + (not self._power_checker(power_right)) + ): + print("WARNING: Read the docstring of Robot.go_diff()!") + return "" + else: + pl_int = int(power_left) + pr_int = int(power_right) + dl_int = int(dir_left) + dr_int = int(dir_right) + cmd = f"d{pl_int},{pr_int},{dl_int},{dr_int}\n" + return self.send_command(cmd) + + + def stop(self): + """ + Send a stop command to stop motors. Sets the motor power on both wheels + to zero. + + This does NOT use wheel encoders. + """ + cmd='s\n' + return self.send_command(cmd) + + + + def read_sensor(self, sensorid): + """ + Send a read sensor command with sensorid and return sensor value. + + Will return -1, if error occurs. + """ + cmd=str(sensorid) + '\n' + str_val=self.send_command(cmd) + if len(str_val) > 0: + return int(str_val) + else: + return -1 + + def read_front_ping_sensor(self): + """ + Read the front sonar ping sensor and return the measured range in + milimeters [mm]. + """ + return self.read_sensor(0) + + def read_back_ping_sensor(self): + """ + Read the back sonar ping sensor and return the measured range in + milimeters [mm]. + """ + return self.read_sensor(1) + + def read_left_ping_sensor(self): + """ + Read the left sonar ping sensor and return the measured range in + milimeters [mm]. + """ + return self.read_sensor(2) + + def read_right_ping_sensor(self): + """ + Read the right sonar ping sensor and return the measured range in + milimeters [mm]. + """ + return self.read_sensor(3) + + + def read_left_wheel_encoder(self): + """ + Reads the left wheel encoder counts since last reset_encoder_counts + command. + + The encoder has 144 counts for one complete wheel revolution. + """ + cmd='e0\n' + return self.send_command(cmd, 0.045) + + def read_right_wheel_encoder(self): + """ + Reads the right wheel encoder counts since last clear + reset_encoder_counts command. + + The encoder has 144 counts for one complete wheel revolution. + """ + cmd='e1\n' + return self.send_command(cmd, 0.045) + + def reset_encoder_counts(self): + """Reset the wheel encoder counts.""" + cmd='c\n' + return self.send_command(cmd) diff --git a/serialRobotArlo.ino b/serialRobotArlo.ino new file mode 100644 index 0000000..f45a40b --- /dev/null +++ b/serialRobotArlo.ino @@ -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 +#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/simple_arlo.py b/simple_arlo.py new file mode 100644 index 0000000..55bdc7b --- /dev/null +++ b/simple_arlo.py @@ -0,0 +1,85 @@ +from time import sleep + +import robot + +def main(): + # Create a robot object and initialize + arlo = robot.Robot() + + print("Running ...") + + + # send a go_diff command to drive forward + left_speed = 64 + right_speed = 64 + print(arlo.go_diff(left_speed, right_speed, 1, 1)) + + # Wait a bit while robot moves forward + sleep(3) + + # send a stop command + print(arlo.stop()) + + # Wait a bit before next command + sleep(0.041) + + # send a go_diff command to drive backwards the same way we came from + print(arlo.go_diff(left_speed, right_speed, 0, 0)) + + # Wait a bit while robot moves backwards + sleep(3) + + # send a stop command + print(arlo.stop()) + + # Wait a bit before next command + sleep(0.041) + + + + # request to read Front sonar ping sensor + print("Front sensor = ", arlo.read_front_ping_sensor()) + sleep(0.041) + + + # request to read Back sonar ping sensor + print("Back sensor = ", arlo.read_back_ping_sensor()) + sleep(0.041) + + # request to read Right sonar ping sensor + print("Right sensor = ", arlo.read_right_ping_sensor()) + sleep(0.041) + + # request to read Left sonar ping sensor + print("Left sensor = ", arlo.read_left_ping_sensor()) + sleep(0.041) + + + + # send a go_diff command to drive forward in a curve turning right + left_speed = 64 + right_speed = 32 + print(arlo.go_diff(left_speed, right_speed, 1, 1)) + + # Wait a bit while robot moves forward + sleep(3) + + # send a stop command + print(arlo.stop()) + + # Wait a bit before next command + sleep(0.041) + + # send a go_diff command to drive backwards the same way we came from + print(arlo.go_diff(left_speed, right_speed, 0, 0)) + + # Wait a bit while robot moves backwards + sleep(3) + + # send a stop command + print(arlo.stop()) + + print("Finished") + +if __name__ == "__main__": + main()