Script feature

Output.txt

@@ -0,0 +1,10 @@
+HIIIIIIIHIIIIIIIHIIIIIIIGot: {"COLOR_SENSOR":{"data":0}}
+
+Got: {"COLOR_SENSOR":{"data":0},"name":{"data":"INVALID: Abstract Sensor"}}
+
+Got: {"COLOR_SENSOR":{"data":0},"sensor1":{"data":0}}
+
+Got: {"COLOR_SENSOR":{"data":0}}
+
+Got: {"COLOR_SENSOR":{"data":0}}
+

cs-minibot-platform.iml

@@ -6,12 +6,28 @@
     <content url="file://$MODULE_DIR$">
       <sourceFolder url="file://$MODULE_DIR$/src/main/java" isTestSource="false" />
       <sourceFolder url="file://$MODULE_DIR$/src/main/resources" type="java-resource" />
-      <sourceFolder url="file://$MODULE_DIR$/python-interface" type="java-resource" />
       <excludeFolder url="file://$MODULE_DIR$/target" />
     </content>
     <orderEntry type="inheritedJdk" />
     <orderEntry type="sourceFolder" forTests="false" />
-    <orderEntry type="library" name="Maven: org.jbox2d:jbox2d-library:2.2.1.1" level="project" />
+    <orderEntry type="module-library">
+      <library>
+        <CLASSES>
+          <root url="jar://$MODULE_DIR$/lib/jaw.jar!/" />
+        </CLASSES>
+        <JAVADOC />
+        <SOURCES />
+      </library>
+    </orderEntry>
+    <orderEntry type="module-library">
+      <library>
+        <CLASSES>
+          <root url="jar://$MODULE_DIR$/lib/XboxController.jar!/" />
+        </CLASSES>
+        <JAVADOC />
+        <SOURCES />
+      </library>
+    </orderEntry>
     <orderEntry type="library" name="Maven: org.jbox2d:jbox2d-library:2.2.1.1" level="project" />
     <orderEntry type="module" module-name="cs-base-station-interface" />
     <orderEntry type="library" name="Maven: org.apache.commons:commons-math3:3.5" level="project" />
@@ -38,23 +54,5 @@
     <orderEntry type="library" name="Maven: org.apache.velocity:velocity:1.7" level="project" />
     <orderEntry type="library" name="Maven: commons-collections:commons-collections:3.2.1" level="project" />
     <orderEntry type="library" name="Maven: commons-lang:commons-lang:2.4" level="project" />
-    <orderEntry type="module-library">
-      <library>
-        <CLASSES>
-          <root url="jar://$MODULE_DIR$/lib/jaw.jar!/" />
-        </CLASSES>
-        <JAVADOC />
-        <SOURCES />
-      </library>
-    </orderEntry>
-    <orderEntry type="module-library">
-      <library>
-        <CLASSES>
-          <root url="jar://$MODULE_DIR$/lib/XboxController.jar!/" />
-        </CLASSES>
-        <JAVADOC />
-        <SOURCES />
-      </library>
-    </orderEntry>
   </component>
 </module>

script.py

@@ -0,0 +1,163 @@
+import socket
+import sys
+import time
+
+HOST = "localhost"
+PORT = 11111
+
+sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+
+class BaseMiniBot:
+    """
+    Abstract class defining the base functions of the MiniBot. More customized MiniBots may
+    subclass this.
+    """
+    def __init__(self):
+        server_address = (HOST, PORT)
+        sock.connect(server_address)
+
+    def move_forward(self, power):
+        """
+        Moves the bot forward at a percentage of its full power
+
+        :param power The percentage of the bot's power to use from 0-100
+        :return True if the action is supported
+        """
+        message = "FORWARD:" + str(power) + '\n'
+        sock.sendall(message)
+        return
+
+    def move_backward(self, power):
+        """
+        Moves the bot backward at a percentage of its full power
+
+        :param power The percentage of the bot's power to use from 0-100
+        :return True if the action is supported
+        """
+        # print("Unimplemented: Moving backward "+str(power))
+        message = "BACKWARD:" + str(power) + '\n'
+        sock.sendall(message)
+        return
+
+    def turn_clockwise(self, power):
+        """
+        Moves the bot clockwise  at a percentage of its full power
+
+        :param power The percentage of the bot's power to use from 0-100
+        :return True if the action is supported
+        """
+        # print("Unimplemented: Turning clockwise "+str(power))
+        message = "RIGHT:" + str(power) + '\n'
+        sock.sendall(message)
+        return
+
+    def turn_counter_clockwise(self, power):
+        """
+        Moves the bot counter-clockwise at a percentage of its full power
+
+        :param power The percentage of the bot's power to use from 0-100
+        :return True if the action is supported
+        """
+        # print("Unimplemented: Turning counter clockwise "+str(power))
+        message = "LEFT:" + str(power) + '\n'
+        sock.sendall(message)
+        return
+
+    def set_wheel_power(self, front_left, front_right, back_left, back_right):
+        """
+        Sets the power of the bot's wheels as a percentage from -100 to 100. If a wheel
+        specified does not exist, the power for that wheel is ignored.
+        :param front_left power to deliver to the front_left wheel
+        :param front_right power to deliver to the front_right wheel
+        :param back_left power to deliver to the back_left wheel
+        :param back_right power to deliver to the back_right wheel
+        :return True if the action is supported
+        """
+        # print("Unimplemented: Setting wheel power to %d,%d,%d,%d" % (front_left, front_right, back_left, back_right))
+        message = "WHEELS:" + str(front_left) + ',' + str(front_right) + ',' + str(back_left) + ',' \
+                  + str(back_right) + '\n';
+        sock.sendall(message)
+        return
+
+    def wait(self, t):
+        """
+        Waits for a duration in seconds.
+        :param t The duration in seconds
+        """
+        message = "WAIT:" + str(t) + '\n'
+        sock.sendall(message)
+        time.sleep(t)
+        return
+
+    def stop(self):
+        """
+        Waits for a duration in seconds.
+        :param t The duration in seconds
+        """
+        message = "STOP:0" + '\n'
+        sock.sendall(message)
+        return
+
+    # def getSensorData(self):
+    #     message = "GET:1" + '\n'
+    #     sock.sendall(message)
+    #     with open('output.txt', 'a') as file:
+    #         file.write("Got: " + sock.recv(1024) + "\n")
+    #     file.close()
+    #     return
+
+    def register_sensor(self, name):
+        message = "REGISTER:" + name + '\n'
+        sock.sendall(message)
+        return
+
+    def kill(self):
+        """
+        Kills TCP connection
+        """
+        message = "KILL:-1" + '\n'
+        sock.sendall(message)
+        sock.close()
+        return
+
+
+class Sensor:
+    def __init__(self, bot, name):
+        self.name = name
+        bot.register_sensor(name)
+
+    def read(self):
+        return "Invalid: Abstract Sensor Class Reading"
+
+
+class GPIOSensor(Sensor):
+    def __init__(self, bot, name, pin_number):
+        Sensor.__init__(self, bot, name)
+        self.pin_number = pin_number
+
+    def readAll(self):
+        """
+        Get Sensor Data
+        """
+        message = "GET:ALL" + '\n'
+        sock.sendall(message)
+        with open('output.txt', 'a') as file:
+            file.write("Got: " + sock.recv(1024) + "\n")
+        file.close()
+        return
+
+    def read(self, name):
+        """
+        Get Sensor Data
+        """
+        message = "GET:" + name + '\n'
+        sock.sendall(message)
+        with open('output.txt', 'a') as file:
+            file.write("Got: " + sock.recv(1024) + "\n")
+        file.close()
+        return
+
+bot = BaseMiniBot()
+bot.turn_clockwise(50)
+bot.wait(2)
+bot.move_forward(100)

src/main/java/minibot/BaseHTTPInterface.java

@@ -14,7 +14,9 @@
 import com.google.gson.JsonObject;
 import com.google.gson.JsonParser;
 import simulator.physics.PhysicalObject;
+import simulator.simbot.ColorIntensitySensor;
 import simulator.simbot.SimBotConnection;
+import simulator.simbot.SimBotSensorCenter;
 import spark.route.RouteOverview;
 
 import java.util.ArrayList;
@@ -89,13 +91,17 @@ public static void main(String[] args) {
             }
                else {
                 SimBotConnection sbc = new SimBotConnection();
-                newBot = new SimBot(sbc, name);
-
                 PhysicalObject po = new PhysicalObject("TESTBOT", 50, simvs.getWorld(), 0.4f, 0.0f, 1f, 1f, true);
+                SimBot simba;
+                simba = new SimBot(sbc, name, po);
+                newBot = simba;
 
                 ArrayList<PhysicalObject> pObjs = new ArrayList<>();
                 pObjs.add(po);
                 simvs.processPhysicalObjects(pObjs);
+
+                // Color sensor TODO put somewhere nice
+                ColorIntensitySensor colorSensor = new ColorIntensitySensor((SimBotSensorCenter) simba.getSensorCenter(),"COLOR_SENSOR",simba);
             }
 
 
@@ -145,6 +151,11 @@ public static void main(String[] args) {
             String name = commandInfo.get("name").getAsString();
             String script = commandInfo.get("script").getAsString();
 
+            ((SimBot)BaseStation.getInstance()
+                    .getBotManager()
+                    .getBotByName(name)
+                    .orElseThrow(NoSuchElementException::new)).resetServer();
+
             return BaseStation.getInstance()
                     .getBotManager()
                     .getBotByName(name)

src/main/java/simulator/simbot/ColorIntensitySensor.java

@@ -0,0 +1,86 @@
+package simulator.simbot;
+
+import basestation.BaseStation;
+import basestation.vision.VisionCoordinate;
+import basestation.vision.VisionObject;
+import com.google.gson.JsonObject;
+
+import javax.imageio.ImageIO;
+import java.awt.image.BufferedImage;
+import java.io.File;
+import java.io.IOException;
+import java.util.List;
+
+/**
+ * A sensor that measures color intensity based on an image provided to the simulator.
+ */
+public class ColorIntensitySensor extends Sensor {
+
+    transient BufferedImage img;
+    private transient SimBot parent;
+
+    public ColorIntensitySensor(SimBotSensorCenter myCenter, String name, SimBot parent) {
+        super(myCenter, name);
+        this.parent = parent;
+        try {
+            img = ImageIO.read(new File("./src/main/resources/public/img/line.png"));
+        } catch (IOException e) {
+            e.printStackTrace();
+        }
+    }
+
+    public JsonObject read() {
+        // Get the x,y coordinates of the bot.
+        // TODO: Make it so we can do this line, not the one below. VisionCoordinate parentCoordinate = BaseStation.getInstance().getVisionManager().getBotCoordinate(parent);
+        List<VisionObject> allLocs = BaseStation.getInstance().getVisionManager().getAllLocationData();
+        JsonObject jo = new JsonObject();
+
+        if (allLocs.size() == 0) {
+            jo.addProperty("error","no object");
+            return jo;
+        }
+
+        VisionCoordinate parentCoordinate = allLocs.get(0).coord;
+
+        if (parentCoordinate == null) {
+            jo.addProperty("error","null object");
+            return jo;
+        }
+
+        // TODO: Coordinate transform, for now just read at the middle
+
+        int[] transformed = transformToPixels(parentCoordinate);
+        // Get pixel values
+        if (img == null) {
+            jo.addProperty("error","no image");
+            return jo;
+        }
+        int rgb = img.getRGB(transformed[0], transformed[1]);
+        if (rgb <= -16777216) {
+            rgb=1;
+        } else {
+            rgb=0;
+        }
+        jo.addProperty("data",rgb);
+
+//        System.out.printf("%d, %d\n",transformed[0], transformed[1]);
+
+        return jo;
+    }
+
+    private int[] transformToPixels(VisionCoordinate vc) {
+        double X_SCALE = 100;
+        double Y_SCALE = 100;
+        double medial_offset = 62.5; //offset along the medial axis of the robot, positive values indicate a "forwards" offset. 62.5 corresponds with offsetting it to the front of the bot
+        double lateral_offset = 0; //offset along lateral axis of robot, positive values indicate a "leftwards" offset, 0 indicates that there is no left/right offset
+        int[] ret = new int[2];
+
+        double angle = vc.getThetaOrZero();
+        double angle_offset = Math.atan(lateral_offset/medial_offset)+angle; //intermediate step for calculating coordinates
+        double total_offset = Math.sqrt(medial_offset*medial_offset+lateral_offset*lateral_offset); //intermediate step for calculating coordinates
+
+        ret[0] = (int) Math.floor(vc.x * X_SCALE + total_offset*Math.cos(angle_offset));
+        ret[1] = (int) Math.floor(vc.y * Y_SCALE + total_offset*Math.sin(angle_offset));
+        return ret;
+    }
+}