From a0c1548ac66418e70d01bd61151cce32d49ca437 Mon Sep 17 00:00:00 2001 From: NikolajDanger Date: Fri, 2 Sep 2022 10:35:07 +0200 Subject: [PATCH] 'init commit' --- __pycache__/robot.cpython-310.pyc | Bin 0 -> 5810 bytes robot.py | 176 ++++++++++++ serialRobotArlo.ino | 427 ++++++++++++++++++++++++++++++ simple_arlo.py | 85 ++++++ 4 files changed, 688 insertions(+) create mode 100644 __pycache__/robot.cpython-310.pyc create mode 100644 robot.py create mode 100644 serialRobotArlo.ino create mode 100644 simple_arlo.py diff --git a/__pycache__/robot.cpython-310.pyc b/__pycache__/robot.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..322cc711a945895cfc9de9042f857b94ec4fa4fc GIT binary patch literal 5810 zcmcIo%WvGq8Rugk6tz-p$FIbRJxQ9to7#G~Zqp`l6U9zb1Bp^dDN<|@5EN%tON-0d z3`winYS9Amt>~pd4^2?OLJJf=w5MMBmlPm+>ml&jw;Y=Oz8UU%^@sxmH5WTW4!;?G z-*0~3gOqb~zJ=qpksH-dS=N8(V*04z;!XUsi)gsT?ZoQa9owdJCviH?I~I3&?T*E3 z!u{0k)VQ;5wd?mGZME&Po+UyIRwKHcK3rV9iGOwnjj%d4w>l2DJ1&1tFK1(+I&a*u zI(7a$_xQ{myVKx4pT$U%xA+`-kI(Zb(9iH={5X1_pWsiTpXDd{DfBIVnx8>G$ItSo z(9iRy`8o7Y@Mrk5=#TO9`~v#poI&Lm%KDnzka_rT+p>IyG!NsH38kcBQBOo$ak|Mavp8oE z#;ST1C9;7`vv?y;;(Uk6u6|w=CW%limrSHtp#&Q!Il#amRAFD_LS-yWxqc30GLdq< zSS0&>jAM0Ksm+Tx#krKMtHdXTNb_WeUEGXs3(_Nx`+{V#dL%MqYPE)5Z)1mroRiui z%X(7fOz$V#4ly~)!(8;SVtax8B#aYM6KiqkBe85h++m3bImYx_S+2r#Q&cE!h+cR* zmW5g{VBSnw6lQ|$$byMq!qix#5!O?(*hmCW1DsjR0YQt_{M ziMDOKCqx1WX=hZnpznRzTIAyPVxI4;Tz_vVazMcGLFaY+uj8L}(Cpf~R$%4!Eoaxd z1@d!;yT5ZW=H@kC`xI_~arbV0&&n?34PKufQKuo(pb^|@>b}N3>a~l^gfEYY2CjiX zLYtVYBFe#+0f9Z)ZD5nK4M_9|LXN$Zea1yMERvkv4wC}A*(L0>g|yywm|}aSW}%Xa zM#J!z$4W%R0=)?4D3*j9xg4;Iahm0{V;~{joGeCA%JlMv3>9Z8%my1E^$r`v*m+Xi zXn3E0?q#U`dgFLw2(}(IdRmRQfx&TvT9FCQz$98*)lRH{>_V)wc55j zO(Lx<^0JnTEWZyBt>H@_f{;XuE}+Xmo-i@GfHkyGjK?i3EZnCJ9?q^6X-dKvX*bIn z=~Fh$tWINH&n{=pb)bMpSsy6a(rLum`vZ|GTBYrjwM-;k<>BI2 zOD5s1#p__*;*Bs56S)~LURk;#%d^JK%b zeHZ_)JhUxmIIrW>t?~Ur@pKlDEEx+ARvrs^z18hL3 zvPFR@sJv8BqABZ9A~SK%Q7yW1qdwQrnP#5GnnA!t5(Epl$Y{i~J*Q=RcFmc$=k4L# z7!ED<1qAdFQA!yf1j)paDRYtg-nDn_yGSy7_`xaNT@8@FQ`@sf?yd{d-QnUolty?R zM;G!UqVSlW#)~Mr!>_g;Y6pdAASIdaz4<*fzG2stzcS4 zHlD2yPgaE3ClJy0j}Kzr4E`FB9!|i0fU;*RU!OU9c6KZWNZLD4z~s&@&PR3hAjznK z9-cF5qDRypdEDKte~yx8x1rmnZauX1Kh2D0v}eui5x~u`1uZ9`Z#@qY))UdqSzl6^ z-;VQMb!srz?Yks^XuKb7o$s()t=gKF_j1?VI|@d6WA9jx3nKzV@nKsh54^n zBd=d$aDaWG)dRfaukkHRpOmQ7OmD4A0P+ z8Egy^Bp+n8figX>uDQO}A?lzPHY>G$3MDd%EZ)5HmFGDB^=dUlX+&!L&Lc{VqjV*I zC}+8tOWyc^0z9`zsI!bm>Ja>g$u3bW#uigHnU2T$+U)THTNk;hT&K1a^)DMi@00S9?>{i3KP2wLOH$YBF?V}XslQEtTXe!Z( zGQFl#GQn!%aVT$5oMti&;pF6a6>?J;Apo$x28x5t$PaFE%BMm($ z^^Jxe!C@W7AIp~BCtrGmrG~%y`O~KUeq!o0dk=~2vz6#h(eOI{4|D*XBm?g3(YK`9 z@NfIrY+}9M<$bUBhgjz|~f z8SNOF3EGgcqMs2ICWW-@;j>fjaai_iu~b*F3b6af#9oSh20KHXZvrpE#~U@7eH4`A zJo7jpT=v!XVZkHdJFDR{?AnLlUkLD5&Dhxx))ItEHU)x%0^#YghKGrYZykzQfbKMPC+KTIim!h`Q2Ig8m%K>mycGnW z6k$@`p|@ul1%Y}3a}<46F!hQq8^1%Bv>+&d&X;dbLi>kd49{+((O7v)*xN#Zv%?n#L9-;xP=ch Z`u0&zDZST?g+Hd;o>7izp}ss>`yV)~1h4=A literal 0 HcmV?d00001 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()