robot.java

//http://lear.cs.okstate.edu/robot_sim.html

import java.io.IOException;
import java.io.PrintWriter;
import java.net.Socket;
import java.util.Scanner;

public class Robot {
    private static final String serverIP = "lear.cs.okstate.edu";
    private static final int serverPort = 9095;
    private static PrintWriter out;

    public static void main(String[] args) {
        try {
            Socket socket = new Socket(serverIP, serverPort);
            System.out.println("Connection to server " + serverIP + " successful");
            out = new PrintWriter(socket.getOutputStream());
        } catch (IOException e) {
            e.printStackTrace();
        }

        while (true) {
            try {
                printMenu();
                Scanner in = new Scanner(System.in);
                int choice = Integer.parseInt((in.nextLine()));
                processChoice(choice, out);
            } catch (NumberFormatException e) {
                System.out.println("You must enter a number.");
            }
        }
    }

    private static void rTakeoff(PrintWriter out) {
        String takeoff_msg = "{\"op\":\"publish\",\"topic\":\"/ardrone/takeoff\",\"msg\":{}}";
        out.write(takeoff_msg);
        out.flush();
    }

    private static void rLand(PrintWriter out) {
        String landing_msg = "{\"op\":\"publish\",\"topic\":\"/ardrone/land\",\"msg\":{}}";
        out.write(landing_msg);
        out.flush();
    }

    private static void rUp(PrintWriter out) {
        int linX = 0;
        int linY = 0;
        int linZ = 1;
        int angZ = 0;

        out.write(createMoveString(linX, linY, linZ, angZ));
        out.flush();
    }

    private static void rDown(PrintWriter out) {
        int linX = 0;
        int linY = 0;
        int linZ = -1;
        int angZ = 0;

        out.write(createMoveString(linX, linY, linZ, angZ));
        out.flush();
    }

    private static void rForward(PrintWriter out) {
        int linX = 1;
        int linY = 0;
        int linZ = 0;
        int angZ = 0;

        out.write(createMoveString(linX, linY, linZ, angZ));
        out.flush();
    }

    private static void rBackward(PrintWriter out) {
        int linX = -1;
        int linY = 0;
        int linZ = 0;
        int angZ = 0;


        out.write(createMoveString(linX, linY, linZ, angZ));
        out.flush();
    }

    private static String createMoveString(int linX, int linY, int linZ, int angZ) {
        return "{\"op\":\"publish\"," +
                "\"topic\":\"/cmd_vel\"," +
                "\"msg\":{\"linear\":{" +
                "\"x\":" + linX + "," +
                "\"y\":" + linY + "," +
                "\"z\":" + linZ + "}," +
                "\"angular\":{" +
                "\"x\":0," +
                "\"y\":0," +
                "\"z\":" + angZ + "}}}";
    }

    private static void printMenu() {
        System.out.println("Please choose a number to select an option");
        System.out.println("1: Takeoff");
        System.out.println("2: Land");
        System.out.println("3: Up");
        System.out.println("4: Down");
        System.out.println("5: Forward");
        System.out.println("6: Backward");
        System.out.print("Choice: ");
    }

    private static void processChoice(int choice, PrintWriter out) {
        switch (choice) {
            case 1:
                rTakeoff(out);
                break;
            case 2:
                rLand(out);
                break;
            case 3:
                rUp(out);
                break;
            case 4:
                rDown(out);
                break;
            case 5:
                rForward(out);
                break;
            case 6:
                rBackward(out);
                break;
            default:
                break;
        }

    }
}