From 8492de19918047b695e9031420f4a0debe77a77b Mon Sep 17 00:00:00 2001
From: Felix Agner <felix.agner@control.lth.se>
Date: Wed, 9 Nov 2022 08:59:55 +0100
Subject: [PATCH] added initial code, and created some basic documents
---
Manifest.toml | 7 ++++
Project.toml | 2 +
src/Omnibot.jl | 11 ++++++
src/connectomnibot.jl | 19 +++++++++
src/getcoordinates.jl | 92 +++++++++++++++++++++++++++++++++++++++++++
src/setspeeds.jl | 41 +++++++++++++++++++
test/testserver.py | 74 ++++++++++++++++++++++++++++++++++
7 files changed, 246 insertions(+)
create mode 100644 Manifest.toml
create mode 100644 Project.toml
create mode 100644 src/Omnibot.jl
create mode 100644 src/connectomnibot.jl
create mode 100644 src/getcoordinates.jl
create mode 100644 src/setspeeds.jl
create mode 100644 test/testserver.py
diff --git a/Manifest.toml b/Manifest.toml
new file mode 100644
index 0000000..ecef8aa
--- /dev/null
+++ b/Manifest.toml
@@ -0,0 +1,7 @@
+# This file is machine-generated - editing it directly is not advised
+
+julia_version = "1.7.3"
+manifest_format = "2.0"
+
+[[deps.Sockets]]
+uuid = "6462fe0b-24de-5631-8697-dd941f90decc"
diff --git a/Project.toml b/Project.toml
new file mode 100644
index 0000000..5b47cb6
--- /dev/null
+++ b/Project.toml
@@ -0,0 +1,2 @@
+[deps]
+Sockets = "6462fe0b-24de-5631-8697-dd941f90decc"
diff --git a/src/Omnibot.jl b/src/Omnibot.jl
new file mode 100644
index 0000000..e27cb96
--- /dev/null
+++ b/src/Omnibot.jl
@@ -0,0 +1,11 @@
+module Omnibot
+
+export connectomnibot
+export getx
+export gety
+export getz
+export gettheta
+
+import connectomnibot.jl
+import setspeeds.jl
+import getcoordinates.jl
\ No newline at end of file
diff --git a/src/connectomnibot.jl b/src/connectomnibot.jl
new file mode 100644
index 0000000..9c27ea8
--- /dev/null
+++ b/src/connectomnibot.jl
@@ -0,0 +1,19 @@
+using Sockets
+
+"""
+ connectomnibot(ip::String;port::Int=9998)
+
+Connect to the omnibot with `ip` and `port`. Return a `TCPSocket` with open connection.
+"""
+function connectomnibot(ip::String;port::Int=9998)
+ return connect(ip,port)
+end
+
+"""
+ connectomnibot(ip::IPv4;port::Int=9998)
+
+Also callable with an IPv4 type argument
+"""
+function connectomnibot(ip::IPv4;port::Int=9998)
+ return connect(ip,port)
+end
diff --git a/src/getcoordinates.jl b/src/getcoordinates.jl
new file mode 100644
index 0000000..83d15c2
--- /dev/null
+++ b/src/getcoordinates.jl
@@ -0,0 +1,92 @@
+using Sockets
+
+"""
+getx(bot::TCPSocket)
+
+Returns the x-coordinate of the omnibot as `Float64`.
+"""
+function getx(bot::TCPSocket;)
+msg = "rx"
+print(bot,msg)
+ret = readline(bot)
+if msg[1] == 'e'
+ error(ret)
+end
+try
+ return parse(ret,Float64)
+catch e
+ if isa(e, ArgumentError)
+ throw(ArgumentError("Received $ret from the bot, which is not a valid float."))
+ end
+end
+
+
+end
+"""
+gety(bot::TCPSocket)
+
+Returns the y-coordinate of the omnibot as `Float64`.
+"""
+function gety(bot::TCPSocket;)
+msg = "ry"
+print(bot,msg)
+ret = readline(bot)
+if msg[1] == 'e'
+ error(ret)
+end
+try
+ return parse(ret,Float64)
+catch e
+ if isa(e, ArgumentError)
+ throw(ArgumentError("Received $ret from the bot, which is not a valid float."))
+ end
+end
+
+
+end
+
+"""
+getz(bot::TCPSocket)
+
+Returns the z-coordinate of the omnibot as `Float64`.
+"""
+function getz(bot::TCPSocket;)
+msg = "rz"
+print(bot,msg)
+ret = readline(bot)
+if msg[1] == 'e'
+ error(ret)
+end
+try
+ return parse(ret,Float64)
+catch e
+ if isa(e, ArgumentError)
+ throw(ArgumentError("Received $ret from the bot, which is not a valid float."))
+ end
+end
+
+
+end
+
+"""
+gettheta(bot::TCPSocket)
+
+Returns the ``θ``-coordinate of the omnibot as `Float64`.
+"""
+function gettheta(bot::TCPSocket;)
+msg = "rtheta"
+print(bot,msg)
+ret = readline(bot)
+if msg[1] == 'e'
+ error(ret)
+end
+try
+ return parse(ret,Float64)
+catch e
+ if isa(e, ArgumentError)
+ throw(ArgumentError("Received $ret from the bot, which is not a valid float."))
+ end
+end
+
+
+end
\ No newline at end of file
diff --git a/src/setspeeds.jl b/src/setspeeds.jl
new file mode 100644
index 0000000..e1c8486
--- /dev/null
+++ b/src/setspeeds.jl
@@ -0,0 +1,41 @@
+using Sockets
+
+"""
+setspeed(bot::TCPSocket,i::Int,v::Int)
+
+sets the speed of servo `i` to `v`.
+
+# Arguments
+- `bot::TCPSocket`: the socket-connection to the robot.
+- `i::Int`: index of the servo, takes values 1-3.
+- `v::Int`: integer servo speed set-point.
+"""
+function setspeed(bot::TCPSocket,i::Int,v::Int)
+msg = 'w'*string(i)*string(v)
+print(bot,msg)
+ret = readline(bot)
+if msg[1] == 'e'
+ error(ret)
+end
+
+return ret
+end
+
+"""
+setspeeds(bot::TCPSocket,v::Vector{Int})
+
+Set the speed-setpoint of servos to the values in vector v.
+"""
+function setspeed(bot::TCPSocket,v::Int)
+fullret = ""
+for (i,s) in enumerate(v)
+ msg = 'w'*string(i)*string(s)
+ print(bot,msg)
+ ret = readline(bot)
+ if msg[1] == 'e'
+ error(ret)
+ end
+ fullret = fullret*ret*"\n"
+end
+return fullret
+end
\ No newline at end of file
diff --git a/test/testserver.py b/test/testserver.py
new file mode 100644
index 0000000..5ca5ece
--- /dev/null
+++ b/test/testserver.py
@@ -0,0 +1,74 @@
+"""
+A python script that can run some tests on the julia client which mimics the behavior of the real system.
+"""
+
+from time import sleep
+import logging
+
+# TCP connection
+import socket
+
+from random import random
+
+
+def run_server(HOST,PORT=9998):
+
+
+ # Create socket s
+ with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
+ s.bind((HOST, PORT)) # Bind the socket to specified port and ip
+ s.listen(1) # Start listening for connections
+
+ # Loop "while true" to keep receiving new connections until the application closes.
+ while True:
+ logging.info("Waiting for connection")
+ conn, addr = s.accept() # Wait for connection
+ logging.info("Connection received from"+str(addr))
+ # Connection received. Closes automatically once the session ends, and then waits for a new connection.
+ with conn:
+ # Log info about the connection
+
+
+ while True:
+
+ # Receive a message
+ data = conn.recv(1024)
+
+ # If the message is empty, close the connection.
+ if not data:
+ logging.debug("Empty input, stopping connection")
+ break
+
+ try:
+ inp = data.decode('utf-8')
+ logging.debug(f"Received {inp}")
+ command_type = inp[0]
+
+ if command_type == 'w':
+ # Handle writing speed to servos
+ message = "Operation " + inp + " successful.\n"
+ conn.sendall(message.encode('utf-8'))
+ elif command_type == 'r':
+ # Handle reading from the crazyflie
+ message = str(round(random(),5))+"\n"
+ conn.sendall(message.encode('utf-8'))
+ except Exception as e:
+ # If an exception is encountered, send it to the client.
+ logging.exception(f"Exception encountered: {e}")
+ message = "e:"+str(e)
+ conn.sendall(message.encode('utf-8'))
+
+
+
+if __name__ == "__main__":
+
+ # Set IP (HOST) and port number (PORT)
+ # "" means listening to any available IP
+ HOST, PORT = "", 9998
+
+ # Define logging format
+ format = "%(asctime)s: %(message)s"
+ logging.basicConfig(format=format, level=logging.DEBUG,datefmt="%H:%M:%S")
+
+ run_server(HOST,PORT=PORT)
+
\ No newline at end of file
--
GitLab