diff --git a/src/main/java/App.java b/src/main/java/App.java index 654447b..b437ac4 100644 --- a/src/main/java/App.java +++ b/src/main/java/App.java @@ -1,16 +1,14 @@ -import swarm.configs.MQTTSettings; -import swarm.robot.Robot; - import java.io.File; import java.io.FileNotFoundException; import java.io.FileReader; import java.io.IOException; import java.util.Properties; -import Robots.samples.MazeFollowingRobot; -import Robots.samples.ObstacleAvoidRobot; -import Robots.samples.SampleRobot; +import Robots.samples.ColorRippleRobot; +import swarm.configs.MQTTSettings; +import swarm.robot.Robot; +import swarm.robot.VirtualRobot; public class App { @@ -34,24 +32,31 @@ public static void main(String[] args) { MQTTSettings.channel = props.getProperty("channel", "v1"); reader.close(); - // Start a single robot - Robot robot = new MazeFollowingRobot(10, 9, 9, 90); - new Thread(robot).start(); - // // Start a swarm of robots - // int[] robotList = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9 }; - - // int startX = 0; - // int startY = 0; - // int startHeading = 90; - - // Robot[] vr = new VirtualRobot[robotList.length]; - - // for (int i = 0; i < robotList.length; i++) { - // vr[i] = new SampleRobot(robotList[i], startX + 40 * i, startY + 50 * i, - // startHeading + 10 * i); - // new Thread(vr[i]).start(); - // } + int[] robotList = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9 }; + Robot[] vr = new VirtualRobot[robotList.length]; + + int x, y, robotHeading; + int startAngle = 0; + int deltaAngle = 360 / 10; + int radius = 50; + int headingOffset = 0; + + for (int i = 0; i < robotList.length; i++) { + double a = (startAngle + i * deltaAngle); + x = (int) (radius * Math.cos(a * Math.PI / 180)); + y = (int) (radius * Math.sin(a * Math.PI / 180)); + robotHeading = (int) (a + headingOffset); + + if (i == 0 || i == 1 || i == 2 || i == 3 | i == 4) { + // These are physical robots + System.out.println(i + "> x:" + x + " y:" + y + " heading:" + robotHeading); + } else { + // These are virtual robots + vr[i] = new ColorRippleRobot(robotList[i], x, y, robotHeading); + new Thread(vr[i]).start(); + } + } } catch (FileNotFoundException ex) { // file does not exist diff --git a/src/main/java/Robots/samples/ColorRippleRobot.java b/src/main/java/Robots/samples/ColorRippleRobot.java new file mode 100644 index 0000000..262e048 --- /dev/null +++ b/src/main/java/Robots/samples/ColorRippleRobot.java @@ -0,0 +1,70 @@ +package Robots.samples; + +import swarm.robot.VirtualRobot; + +public class ColorRippleRobot extends VirtualRobot { + + private int currentHopId; + private boolean colorUpdated; + + public ColorRippleRobot(int id, double x, double y, double heading) { + super(id, x, y, heading); + } + + public void setup() { + super.setup(); + + neoPixel.changeColor(0, 0, 0); + colorUpdated = false; + currentHopId = -1; + coordinates.publishCoordinate(); + } + + @Override + public void loop() throws Exception { + super.loop(); + // Anything specially check in continuously + } + + @Override + public void communicationInterrupt(String msg) { + System.out.println("communicationInterrupt on " + id + " with msg:" + msg); + + String[] s = msg.split(" "); + + if (s.length == 4) { + int hopId = Integer.parseInt(s[0]); + + if (colorUpdated) { + // a returning message, don't forward + neoPixel.changeColor(0, 0, 0); + + } else if (hopId > currentHopId) { + int hopR = Integer.parseInt(s[1]); + int hopG = Integer.parseInt(s[2]); + int hopB = Integer.parseInt(s[3]); + neoPixel.changeColor(hopR, hopG, hopB); + currentHopId = hopId; + colorUpdated = true; + + delay(2000); + + // Send it to the next robot + simpleComm.sendMessage((hopId + 1) + " " + hopR + " " + hopG + " " + hopB); + neoPixel.changeColor(0, 0, 0); + } + } else { + System.out.println("Invalid msg received"); + } + } + + public void start() { + super.start(); + + // Things to do when start action + neoPixel.changeColor(0, 0, 0); + colorUpdated = false; + currentHopId = -1; + } + +} \ No newline at end of file