Lego EV3 MachineCloud Java Code

EV3DirectCommand.java

package com.andyinthecloud.legoev3force.ev3kernal;

import lejos.hardware.Button;
import lejos.hardware.motor.Motor;
import lejos.hardware.port.SensorPort;
import lejos.hardware.sensor.EV3TouchSensor;
import lejos.robotics.SampleProvider;
import lejos.robotics.filter.AbstractFilter;

/**
 * Utility class methods help move the Grabber EV3 robot around
 */
public class EV3DirectCommand {

	private static EV3TouchSensor SENSOR1 = new EV3TouchSensor(SensorPort.S1);
	
	public static void moveForward(int rotations)
	{
    	Motor.B.rotate((180 * rotations)*1, true);
    	Motor.C.rotate((180 * rotations)*1, true);
    	while (Motor.B.isMoving() || Motor.C.isMoving());
    	Motor.B.flt(true);
    	Motor.C.flt(true);
	}

	public static void moveBackwards(int rotations)
	{
    	Motor.B.rotate((180 * rotations)*-1, true);
    	Motor.C.rotate((180 * rotations)*-1, true);		
    	while (Motor.B.isMoving() || Motor.C.isMoving());
    	Motor.B.flt(true);
    	Motor.C.flt(true);
	}
	
	public static void release()
	{		
		// Grabbers already open?
		if(new SimpleTouch(SENSOR1).isPressed())
			return;
		// Release grabbers		
		Motor.A.rotate((180 * 5)*-1);
    	Motor.A.flt(true);		
	}

	public static void grab()
	{
		// Grabbers already grabbing something?
		if(new SimpleTouch(SENSOR1).isPressed()==false)
			return;
		Motor.A.rotate((180 * 5));		
    	Motor.A.flt(true);		
	}
	
	public static void turnLeft()
	{
    	Motor.B.rotate((int) (180 * 2.5)*-1, true);
    	Motor.C.rotate((int) (180 * 2.5)*1, true);
    	while (Motor.B.isMoving() || Motor.C.isMoving());
    	Motor.B.flt(true);
    	Motor.C.flt(true);
	}
	
	public static void turnRight()
	{
    	Motor.B.rotate((int) (180 * 2.5)*1, true);
    	Motor.C.rotate((int) (180 * 2.5)*-1, true);
    	while (Motor.B.isMoving() || Motor.C.isMoving());
    	Motor.B.flt(true);
    	Motor.C.flt(true);
	}
	
	public static void init()
	{
    	Motor.A.setSpeed(700);
    	Motor.B.setSpeed(700);
    	Motor.C.setSpeed(700);
    	moveForward(1);
	}
	
	public static void led(int parameter) 
	{
		Button.LEDPattern(parameter);		
	}    	

    public static void main(String[] args)
        	throws Exception
    {
    	init();
    	release();
    	moveForward(8);
    	turnLeft();
    	moveForward(9);
    	turnRight();
    	moveForward(8);
    	grab();    	
    }
    
    /**
     * See thread http://www.lejos.org/forum/viewtopic.php?f=21&t=6627
     */
    public static class SimpleTouch extends AbstractFilter {
      private float[] sample;

      public SimpleTouch(SampleProvider source) {
        super(source);
        sample = new float[sampleSize];
      }

      public boolean isPressed() {
        super.fetchSample(sample, 0);
        if (sample[0] == 0)
          return false;
        return true;
      }
    }
}

EV3ForceCommand.java

package com.andyinthecloud.legoev3force.ev3kernal;

import java.net.URL;
import java.util.HashMap;
import java.util.Map;

import lejos.hardware.Button;
import lejos.hardware.lcd.LCD;

import org.cometd.bayeux.Channel;
import org.cometd.bayeux.Message;
import org.cometd.bayeux.client.ClientSessionChannel;
import org.cometd.client.BayeuxClient;
import org.cometd.client.transport.ClientTransport;
import org.cometd.client.transport.LongPollingTransport;
import org.eclipse.jetty.client.ContentExchange;
import org.eclipse.jetty.client.HttpClient;
import org.eclipse.jetty.util.ajax.JSON;

import com.sforce.soap.partner.PartnerConnection;
import com.sforce.soap.partner.QueryResult;
import com.sforce.soap.partner.sobject.SObject;

/** 
 * Demonstration Java program to run on the Lego Mindstorms EV3 receiving commands
 *   via Salesforce using the Streaming API and records being inserted into a Custom Object
 */
public class EV3ForceCommand {
        
    /**
     * Starts listening for new commands and waits for a key press on the Lego brick to exit
     * @param sessionId
     * @param serverUrl
     * @throws Exception
     */
    public static void start(String sessionId, String serverUrl, final PartnerConnection partnerConnection, String robotId)
    	throws Exception
    {        
        // Subscribe to the Command push topic
        LCD.clear();
        LCD.drawString("Stream connect....", 0, 3);        
        final BayeuxClient client = makeStreamingAPIConnection(sessionId, serverUrl);
        LCD.clear();
        LCD.drawString("Waiting....", 0, 3);

        // Configure robot
        EV3DirectCommand.init();
        
        // Subscribe to the 'commands' topic to listen for new Command__c records
        client.getChannel("/topic/robot"+robotId).subscribe(new ClientSessionChannel.MessageListener() 
            {
                @SuppressWarnings("unchecked")
                public void onMessage(ClientSessionChannel channel, Message message) 
                {
                    try
                    {
                        HashMap<String, Object> data = (HashMap<String, Object>) JSON.parse(message.toString());
                        HashMap<String, Object> record = (HashMap<String, Object>) data.get("data");
                        HashMap<String, Object> sobject = (HashMap<String, Object>) record.get("sobject");
                        String commandName = (String) sobject.get("Name");
                        String command = (String) sobject.get("mcloud__Command__c");
                        String commandParameter = (String) sobject.get("mcloud__CommandParameter__c");
                        String programToRunId = (String) sobject.get("mcloud__ProgramToRun__c");
                        String forwardToRobotId = (String) sobject.get("mcloud__ForwardToRobot__c");
                        executeCommand(commandName, command, commandParameter, programToRunId, forwardToRobotId, partnerConnection);
                    }
                    catch (Exception e)
                    {
                    	e.printStackTrace();
                    	System.exit(1);
                    }
                }
            });          
        
        // Press button to stop
        Button.waitForAnyPress();
        System.exit(1);;
    }
    
    /**
     * Executes commands received from Salesforce via the Streaming API
     * @param commandName
     * @param command
     * @param commandParameter
     * @param programToRun
     */
    private static void executeCommand(String commandName, String command, String commandParameter, String programToRunId, String forwardToRobotId, PartnerConnection partnerConnection)
    	throws Exception
    {
        LCD.clear();    	
        LCD.drawString(forwardToRobotId!=null ? "Forwarding:" : "Running:  ", 0, 1);
        LCD.drawString(commandName, 0, 2);
        LCD.drawString("Command:  ", 0, 3);
        LCD.drawString(command, 0, 4);
        LCD.drawString("Parameter:", 0, 5);        
        LCD.drawString(commandParameter==null ? "" : commandParameter, 0, 6);
        
        // Forward this command to the specified Robot by insert it
        if(forwardToRobotId!=null)
        {        
        	SObject commandRecord = new SObject();
        	commandRecord.setType("mcloud__Command__c");
        	commandRecord.setField("mcloud__Command__c", command);
        	commandRecord.setField("mcloud__CommandParameter__c", commandParameter);
        	commandRecord.setField("mcloud__Robot__c", forwardToRobotId);
        	if(programToRunId!=null)
        		commandRecord.setField("mcloud__ProgramToRun__c", programToRunId);
        	partnerConnection.create(new SObject[] { commandRecord });
        	return;
        }
        
        // Execute this command on this robot
        int parameter = 1;
        if(commandParameter!=null && commandParameter.length()>0)
        	try { parameter = Integer.parseInt(commandParameter); } catch (Exception e) { }
        if(command.equals("Forward"))
        	EV3DirectCommand.moveForward(parameter);
        else if(command.equals("Backward"))
        	EV3DirectCommand.moveBackwards(parameter);
        else if(command.equals("Rotate Left"))
        	EV3DirectCommand.turnLeft();
        else if(command.equals("Rotate Right"))
        	EV3DirectCommand.turnRight();
        else if(command.equals("Grab"))
        	EV3DirectCommand.grab();
        else if(command.equals("Release"))
        	EV3DirectCommand.release();
        else if(command.equals("LED"))
        	EV3DirectCommand.led(parameter);
        else if(command.equals("Shutdown"))
        	System.exit(1);
        else if(command.equals("Run Program"))
        {
        	// Program to run?
        	if(programToRunId==null)
        		return;
        	// Query for the given program commands and execute them as above
			QueryResult result = 
				partnerConnection.query(
					"select Name, mcloud__Command__c, mcloud__CommandParameter__c, mcloud__ProgramToRun__c, mcloud__ForwardToRobot__c " +
					  "from mcloud__Command__c " +
					  "where mcloud__Program__c = '" + programToRunId + "' order by mcloud__ProgramSequence__c");
			SObject[] commands = result.getRecords();
			for(int loop=0; loop<parameter; loop++)
				for(SObject commandRecord : commands)
					executeCommand(
							(String) commandRecord.getField("Name"),
							(String) commandRecord.getField("mcloud__Command__c"),
							(String) commandRecord.getField("mcloud__CommandParameter__c"),
							(String) commandRecord.getField("mcloud__ProgramToRun__c"),
							(String) commandRecord.getField("mcloud__ForwardToRobot__c"),
							partnerConnection);
        }
    }
    
    /**
     * Uses the Jetty HTTP Client and Cometd libraries to connect to Saleforce Streaming API
     * @param config
     * @return
     * @throws Exception
     */
	private static BayeuxClient makeStreamingAPIConnection(final String sessionid, String serverUrl) 
	        throws Exception 
	{
	    HttpClient httpClient = new HttpClient();
	    httpClient.setConnectTimeout(20 * 1000); // Connection timeout
	    httpClient.setTimeout(120 * 1000); // Read timeout
	    httpClient.start();
	
	    // Determine the correct URL based on the Service Endpoint given during logon
	    URL soapEndpoint = new URL(serverUrl);
	    StringBuilder endpointBuilder = new StringBuilder()
	        .append(soapEndpoint.getProtocol())
	        .append("://")
	        .append(soapEndpoint.getHost());
	    if (soapEndpoint.getPort() > 0) endpointBuilder.append(":")
	        .append(soapEndpoint.getPort());
	    String endpoint = endpointBuilder.toString();
	    
	    // Ensure Session ID / oAuth token is passed in HTTP Header
	    Map<String, Object> options = new HashMap<String, Object>();
	    options.put(ClientTransport.TIMEOUT_OPTION, httpClient.getTimeout());
	    LongPollingTransport transport = new LongPollingTransport(options, httpClient) 
	            {
	                    @Override    
	                protected void customize(ContentExchange exchange) 
	                    {
	                    super.customize(exchange);
	                    exchange.addRequestHeader("Authorization", "OAuth " + sessionid);
	                }
	            };
	
	    // Construct Cometd BayeuxClient
	    BayeuxClient client = new BayeuxClient(new URL(endpoint + "/cometd/29.0").toExternalForm(), transport);
	            
	    // Add listener for handshaking
	    client.getChannel(Channel.META_HANDSHAKE).addListener
	        (new ClientSessionChannel.MessageListener() {
	
	        public void onMessage(ClientSessionChannel channel, Message message) {
	
	            boolean success = message.isSuccessful();
	            if (!success) {
	                String error = (String) message.get("error");
	                if (error != null) {
	                    System.out.println("Error during HANDSHAKE: " + error);
	                    System.out.println("Exiting...");
	                    System.exit(1);
	                }
	
	                Exception exception = (Exception) message.get("exception");
	                if (exception != null) {
	                    System.out.println("Exception during HANDSHAKE: ");
	                    exception.printStackTrace();
	                    System.out.println("Exiting...");
	                    System.exit(1);
	
	                }
	            }
	        }
	
	    });
	
	    // Add listener for connect
	    client.getChannel(Channel.META_CONNECT).addListener(
	        new ClientSessionChannel.MessageListener() {
	        public void onMessage(ClientSessionChannel channel, Message message) {
	
	            boolean success = message.isSuccessful();
	            if (!success) {
	                String error = (String) message.get("error");
	                if (error != null) {
	                    System.out.println("Error during CONNECT: " + error);
	                    System.out.println("Exiting...");
	                    System.exit(1);
	                }
	            }
	        }
	
	    });
	
	    // Add listener for subscribe
	    client.getChannel(Channel.META_SUBSCRIBE).addListener(
	        new ClientSessionChannel.MessageListener() {
	
	        public void onMessage(ClientSessionChannel channel, Message message) {
	
	            boolean success = message.isSuccessful();
	            if (!success) {
	                String error = (String) message.get("error");
	                if (error != null) {
	                    System.out.println("Error during SUBSCRIBE: " + error);
	                    System.out.println("Exiting...");
	                    System.exit(1);
	                }
	            }
	        }
	    });
	
	    // Begin handshaking
	    client.handshake();
	    boolean handshaken = client.waitFor(60 * 1000, BayeuxClient.State.CONNECTED);
	    if (!handshaken) {
	        System.out.println("Failed to handshake: " + client);
	        System.exit(1);
	    }
	
	    return client;
	}    
}

Main.java

package com.andyinthecloud.legoev3force.ev3kernal;

import java.io.File;
import java.io.FileReader;
import java.io.FileWriter;
import java.util.Map;
import java.util.Properties;

import org.eclipse.jetty.client.ContentExchange;
import org.eclipse.jetty.client.HttpClient;
import org.eclipse.jetty.io.ByteArrayBuffer;
import org.eclipse.jetty.util.ajax.JSON;

import com.sforce.soap.partner.Connector;
import com.sforce.soap.partner.PartnerConnection;
import com.sforce.soap.partner.QueryResult;
import com.sforce.soap.partner.sobject.SObject;
import com.sforce.ws.ConnectorConfig;

import lejos.hardware.lcd.LCD;
import lejos.utility.Delay;

/**
 * Main kernal entry point for connection to Salesforce and routing commands to the robot
 */
public class Main {
	
	// Connected App Details 
	private static final String CLIENT_ID = "bla";
	private static final String CLIENT_SECRET = "bla";

	// Local config file for storing refresh token, robot id and instance url
	private static String CONFIG_FILE = "ev3force.properites";
	
	/**
	 * Connects to Saleforce by reading pre determined oAuth token from properties file
	 *    or starts a pairing process with the Heroku Canvas App, then listens for commands
	 * @param args
	 * @throws Exception
	 */
	public static void main(String[] args)
		throws Exception
	{
		// Reconnect or begin pairing process?
		RobotConnection robotConnection = null;
		File configFile = new File(CONFIG_FILE);
		if(configFile.exists())
			robotConnection = loadConnection(configFile);
		else
			robotConnection = pairWithSalesforce();
		
        // Display Robot name
        LCD.clear();
        LCD.drawString("Querying Robot", 0, 3);
        PartnerConnection partnerConnection = Connector.newConnection(robotConnection.connectorConfig);
        QueryResult result = partnerConnection.query("select Name, mcloud__Paired__c from mcloud__Robot__c where Id = '"+robotConnection.robotId+"'");
        SObject[] robots = result.getRecords();
        
        // If the robot record no longer exists or has become unpaired repair with it
        if(robots.length==0 || ((String)robots[0].getField("mcloud__Paired__c")).equals("false"))
        {
        	// Repair with the robot record
        	robotConnection = pairWithSalesforce();
        	if(robots.length==0) // Query the new robot record?
        		robots = partnerConnection.query("select Name from mcloud__Robot__c where Id = '"+robotConnection.robotId+"'").getRecords();
        }
        LCD.clear();
        LCD.drawString("Welcome " + robots[0].getField("Name") + "!", 0, 3);
        Delay.msDelay(5000);
        LCD.clear();
        
        // Start listening
        EV3ForceCommand.start(
        		robotConnection.connectorConfig.getSessionId(), 
        		robotConnection.connectorConfig.getServiceEndpoint(), 
        		partnerConnection, 
        		robotConnection.robotId);
	}
	
	/**
	 * Starts a pairing process with Heroku Canvas app
	 * @return
	 * @throws Exception
	 */
	@SuppressWarnings("unchecked")
	private static RobotConnection pairWithSalesforce()
		throws Exception
	{
		LCD.clear();
		LCD.drawString("Getting Pin", 0, 3);
		
		// Http commons with pairing service
	    HttpClient httpClient = new HttpClient();
	    httpClient.setConnectTimeout(20 * 1000); // Connection timeout
	    httpClient.setTimeout(120 * 1000); // Read timeout
	    httpClient.start();
	    
	    // Get a pin number
	    ContentExchange getPin = new ContentExchange();
	    getPin.setMethod("GET");
	    getPin.setURL("https://ev3forcepairing.herokuapp.com/service/pin");
	    httpClient.send(getPin);
	    getPin.waitForDone();	
	    Map<String, Object> parsed = (Map<String, Object>) JSON.parse(getPin.getResponseContent());
	    
	    // Display pin number to enter into Salesforce
	    String pin = (String) parsed.get("pin");
	    LCD.clear();
		LCD.drawString("Pin " + pin, 0, 3);

		// Wait for refresh token for the given pin number
		Integer waitCount = 0;
		String refreshToken = null;
		String robotId = null;
		while(true)
		{
		    getPin = new ContentExchange();
		    getPin.setMethod("GET");
		    getPin.setURL("https://ev3forcepairing.herokuapp.com/service/pin?pin=" + pin);
		    httpClient.send(getPin);
		    getPin.waitForDone();
		    parsed = (Map<String, Object>) JSON.parse(getPin.getResponseContent());
		    refreshToken = (String) parsed.get("refreshToken");
			robotId = (String) parsed.get("robotId");
			if(refreshToken!=null)
				break;
			LCD.drawString("Waiting " + waitCount++, 0, 4);
			Delay.msDelay(1000);
		}
				
		// Save refresh token and robot id for next startup
		saveConfiguration(refreshToken, robotId);
				
		// Setup connector config
		ConnectorConfig config = new ConnectorConfig();
		config.setSessionId(refreshToken);
		config.setManualLogin(true);
		RobotConnection sfConnection = new RobotConnection();
		sfConnection.connectorConfig = config;
		sfConnection.robotId = robotId;
		resolveAccessToken(refreshToken, sfConnection);
		
		// Update Robot to show its been paired
		LCD.clear();
		LCD.drawString("Updating Robot", 0, 3);		
		SObject robotRecord = new SObject();
		robotRecord.setId(robotId);
		robotRecord.setType("mcloud__Robot__c");
		robotRecord.setField("mcloud__Paired__c", Boolean.TRUE);
		PartnerConnection partnerConnection = Connector.newConnection(sfConnection.connectorConfig);
		partnerConnection.update(new SObject[] { robotRecord } );
		
		return sfConnection;
	}
	
	/**
	 * Loads connection details obtained from a previous pairing process
	 * @param configFile
	 * @return
	 * @throws Exception
	 */
	private static RobotConnection loadConnection(File configFile)
		throws Exception
	{
		// Load refresh token and robot Id
		Properties configProps = new Properties();
		configProps.load(new FileReader(configFile));
		ConnectorConfig config = new ConnectorConfig();
		config.setManualLogin(true);
		RobotConnection robotConnection = new RobotConnection();
		robotConnection.connectorConfig = config;
		robotConnection.robotId = configProps.getProperty("RobotId");
		
		// Get access token
		String refreshToken = configProps.getProperty("RefreshToken");
		resolveAccessToken(refreshToken, robotConnection);
		return robotConnection;
	}
	
	/**
	 * Stores the connection details in ev3force.properites in the current folder
	 * @param robotConnection
	 * @throws Exception
	 */
	private static void saveConfiguration(String refreshToken, String robotId)
		throws Exception
	{
		Properties configProps = new Properties();
		configProps.put("RefreshToken", refreshToken);
		configProps.put("RobotId", robotId);
		configProps.store(new FileWriter(CONFIG_FILE), null);
	}

	/**
	 * Simple POJO for passing around connection details
	 */
	public static class RobotConnection
	{
		public String robotId;
		public ConnectorConfig connectorConfig;
	}
		
	/** 
	 * Helper method to obtain an access token via oAuth
	 * @param refreshToken
	 * @return
	 */
	private static void resolveAccessToken(String refreshToken, RobotConnection robotConnection)
		throws Exception
	{
		LCD.clear();
		LCD.drawString("Salesforce Login", 0, 3);				
	    HttpClient httpClient = new HttpClient();
	    httpClient.setConnectTimeout(20 * 1000); // Connection timeout
	    httpClient.setTimeout(120 * 1000); // Read timeout
	    httpClient.start();		
	    ContentExchange refershToken = new ContentExchange();
	    refershToken.setMethod("POST");
	    refershToken.setURL("https://login.salesforce.com/services/oauth2/token");
	    String formData =
	    		"grant_type=refresh_token" + "&" + 
	    		"refresh_token=" + refreshToken + "&" +
	    		"client_id=" + CLIENT_ID + "&" +
	    		"client_secret=" + CLIENT_SECRET;
	    refershToken.setRequestContent( new ByteArrayBuffer(formData));
	    refershToken.setRequestContentType( "application/x-www-form-urlencoded; charset=UTF-8" );
	    httpClient.send(refershToken);
	    refershToken.waitForDone();
	    String jsonResponse = refershToken.getResponseContent();
	    @SuppressWarnings("unchecked")
		Map<String, Object> parsed = (Map<String, Object>) JSON.parse(jsonResponse); 
	    String accessToken = (String) parsed.get("access_token");
	    String instanceUrl = (String) parsed.get("instance_url");
	    robotConnection.connectorConfig.setSessionId(accessToken);
	    robotConnection.connectorConfig.setServiceEndpoint(instanceUrl+"/services/Soap/u/29.0");
	}
}