Power Functions PWM

July 2009

 

I was programming Grab-N-Go and wanted to control the speed of the LEGO Power Function motors. The LEGO IR protocol supports a message format to set the motor speeds, and the NRLink device from Mindsensors can send the required IR communication packets. But I could not find code to control the PWM mode from Lejos.


Xander Soldaat had posted his example code showing how to set the speed of the PF motors using the HiTechnic sensor, but there were no examples on how to use the Mindsensors NRLink to set PWM mode and control the speed of the motors. After a lot of experimenting it turned out the answer is exceptionally simple (as the answer always is in hindsight).


Page 11 of the LEGO IR protocol defines the bit fields required for the IR message to set the Combo PWM mode. The trick is to assemble the message a nibble (4 bits) at a time and then compute the checksum across all of the nibbles. In all of my tests I had forgotten to compute the checksum which caused the IR receiver to simply ignore the messages I was sending.


Code

The lejos 0.8 distribution comes with a PFLink class written by Alexander Vegh. I wrote a new class PFLinkPWM that extends the PFLink class to add new methods comboPWM() and singlePWM(). I also created some convenient definitions for the motor speeds.


The Java code for the PWM extensions looks as follows (click PowerFuncPWM.java to download the file):


import lejos.nxt.*;

import lejos.nxt.addon.*;



/**

* Class for controlling PF Motors with MindSensors NRLink-Nx

* Extended to handle PWM control of motors

*

* @author Mark Crosbie  <mark@mastincrosbie.com>

* @author Alexander Vegh <alex@vegh.ch>

*/

class PFLinkPWM extends PFLink {


private static final byte NR_TRANSMIT = 0x40;

    private static final byte NR_TXDATA = 0x42;


    public static final byte PWM_FORWARD_1 = 1;

    public static final byte PWM_FORWARD_2 = 2;

    public static final byte PWM_FORWARD_3 = 3;

    public static final byte PWM_FORWARD_4 = 4;

    public static final byte PWM_FORWARD_5 = 5;

    public static final byte PWM_FORWARD_6 = 6;

    public static final byte PWM_FORWARD_7 = 7;

   

    public static final byte PWM_FLOAT = 0;

    public static final byte PWM_BRAKE = 8;


    public static final byte PWM_REVERSE_1 = 15;

    public static final byte PWM_REVERSE_2 = 14;

    public static final byte PWM_REVERSE_3 = 13;

    public static final byte PWM_REVERSE_4 = 12;

    public static final byte PWM_REVERSE_5 = 11;

    public static final byte PWM_REVERSE_6 = 10;

    public static final byte PWM_REVERSE_7 = 9;

   

    public static final byte OUTPUT_A = 0;

    public static final byte OUTPUT_B = 1;

   

    public static final byte CHANNEL_1 = 0;

    public static final byte CHANNEL_2 = 1;

    public static final byte CHANNEL_3 = 2;

    public static final byte CHANNEL_4 = 3;

   


    public PFLinkPWM(I2CPort _Port) {

        super(_Port);

    }


     /**

     * Constructs and send a Power Function IR message in raw

     * format to the IR receiver. Sends the message to the IR

     * receiver.

     *

     * @param nibble1 - first nibble of command

     * @param nibble2 - second nibble

     * @param nibble3 - third nibble

     * @return Returns non-zero on error, zero on success

     */

    private int sendIRMessage(int nibble1, int nibble2, int nibble3) {

        byte[] buf = new byte[2];


        // Store the two byte command in the transmit buffer by writing to the

        // transmit pseudo-register

        buf[0] = (byte) ((byte)nibble1 << 4 | (byte)(nibble2));

        buf[1] = (byte) (nibble3 << 4);

        buf[1] += (0xf ^ (byte)nibble1 ^ (byte)nibble2 ^ (byte)nibble3);

       

        sendData(NR_TXDATA, buf, 2);

       

        Sound.pause(10);


        // Now send the bytes by writing to the pseudo-register

        // telling it how many bytes to send

        buf[0] = 2;

        return sendData(NR_TRANSMIT, buf, 1);

    }

   

    /**

     * Combo PWM mode - set both motors to a fixed speed

     *

     * @param channel - channel IR receiver is set to. Range 0 to 3 corresponding to Channels 1 to 4

     * @param motorASpeed - speed and direction to set motorA

     * @param motorBSpeed - speed and direction to set motorB

     * @return Non-zero for error, zero for success

     */

    public int comboPWM(int channel, byte motorASpeed, byte motorBSpeed) {

   

    byte nibble1 = (byte) (0x4 | (byte)channel);

    byte nibble2 = motorBSpeed;

    byte nibble3 = motorASpeed;

   

    return sendIRMessage(nibble1, nibble2, nibble3);

    }


    /**

     * Single PWM mode - set a single motor to a fixed speed

     *

     * @param channel - channel IR receiver is set to. Range 0 to 3 corresponding to Channels 1 to 4

     * @param output - 0 for Channel A, 1 for Channel B

     * @param motorSpeed - speed and direction to set motorB

     * @return Non-zero for error, zero for success

     */

    public int singlePWM(int channel, int output, byte motorSpeed) {

   

    byte nibble1 = (byte) (channel);

    byte nibble2 = (byte) ((byte) 0x4 | (byte)output);

    byte nibble3 = motorSpeed;

      

    return sendIRMessage(nibble1, nibble2, nibble3);

    }


}


To test this code here is a simple main program that calls the methods. This assumes that you have  pair of power function motors attached to a receiver on Channel 1.


public class PowerFuncPWM {


public static final SensorPort NRlinkPort = SensorPort.S1;


/**

* @param args

*/

public static void main(String[] args) {


  LCD.clearDisplay();


PFLinkPWM link = new PFLinkPWM(NRlinkPort);

link.initialize(PFLinkPWM.NR_RANGE_LONG);

        LCD.drawString(link.getVersion(), 0, 0);

        LCD.drawString(link.getProductID(), 0, 1);

        LCD.drawString(link.getSensorType(), 0, 2);


        for(byte i=0; i < 16; i++) {

        //link.comboPWM(PFLinkPWM.CHANNEL_1, i, i);

        link.singlePWM(PFLinkPWM.CHANNEL_1, PFLinkPWM.OUTPUT_B, i);

        Sound.pause(1000);

        }

       

Sound.pause(5000);

LCD.drawString("Done", 0, 4);

}

}

 

Last updated 24 May, 2009


All content © 2008 2009 Mark Crosbie  mark@mastincrosbie.com


LEGO® is a trademark of the LEGO Group of companies which does not sponsor, authorize or endorse this site. This site, its owner and contents are in no way affiliated with or endorsed by the LEGO Group. For more please read the LEGO Fair Play policy.