CAN Jaguar enhancements

[JasonStern]

Our team has noticed a couple of areas in the (Java) CAN Jaguar code we would like to modify:

* Always send speed and position references no matter the control type
* Adding the ability to switch control types on the fly

Are there any hardware/firmware/software reasons that would prevent these changes?

The first change is because we discovered the Jags will not return a speed value in open loop control mode unless code had previously been loaded to the crio prior to rebooting that put the Jags in closed loop control mode (speed control). We have motor calibration code that depends on receiving speed values in open loop mode.

The second change is so that we can switch between position and speed control for different operations (autonomous and teleoperated).

We can just make the changes and try it out, but we have already bricked our spare Jags and don't want to risk it. (We also don't have any Black Jags for recovery.)

Thanks,
Jason

[JasonStern]

Quick update:

Change #1 worked and we can now get position and speed information while operating in the default open loop mode (voltage).

Change #2 worked briefly for a few runs but then abruptly stopped for no apparent reason and without significant code changes. Out test sequence cycled from voltage to position to speed control modes.

We also tried adding sync method to the Java code. Similar to #2, it worked for a few runs and then stopped working.

Anyone have any thoughts/suggestions?

Thanks,
Jason

[taichichuan]

What chages did you have to make to enable reading the position/speed info in voltage mode? Any changes to the CAN Jaguar library code or just to your initialization sequence? Posting a snippet of your code showing what you did would be most appreciated.

TIA,

Mike

[Bryscus]

Originally Posted by JasonStern

Our team has noticed a couple of areas in the (Java) CAN Jaguar code we would like to modify:

* Always send speed and position references no matter the control type
* Adding the ability to switch control types on the fly

Jason,

First of all, I don't see what would keep you from switching control types on the fly. The control method of the Jag is set AFTER initialization. In fact, the messages sent to the Jag have the control mode as part of the data packet (if one could call it that). You have to be sure to enable and disable your closed loop controls though, if you switch between voltage mode and one of the closed loop modes. I wonder to what modification are you referring?

Secondly, why do you need feedback if you're in open loop mode? This is a contradiction. If you receive feedback your are by definition in a closed loop mode. If you need both position and speed in open loop mode, then just connect your encoder to the a set of digital inputs and calculate it all on the cRio. There is a handy encoder class that will keep track of everything for you.

- Bryce

[JasonStern]

Originally Posted by Bryscus

First of all, I don't see what would keep you from switching control types on the fly. The control method of the Jag is set AFTER initialization.

In the Java code, there is no method to set the control type.

Originally Posted by Bryscus

Secondly, why do you need feedback if you're in open loop mode? This is a contradiction. If you receive feedback your are by definition in a closed loop mode.

Receiving feedback in and of itself does not place you in closed loop; you have to be using the feedback to change the input value before you have closed the loop. For example, if you write your current speed to a spreadsheet but don't use the value anywhere else (like we are doing for the motor calibration code), you are still in an open loop.

Wiring the feedback values into the Jags as opposed to the crio provides more versatility in that you can change control modes without having to rewire. You might also be in a position where you want the Jag itself to run in an open loop and want to implement the controller on the crio instead.

Originally Posted by Bryscus

I wonder to what modification are you referring?

I made three main changes:
1. Removed the checks for which control type you are in when setting the references
2. Add a method to change the control type
3. Added code to add sync functionality

Note: this worked a few times and then stopped working, so I would not recommend using this code without testing!!! The diff is based off of rev 50.

CANJaguar.java:

--- C:/temp/CANJaguarOld.java	Thu Feb 25 13:28:27 2010
+++ C:/temp/CANJaguar.java	Thu Feb 25 13:38:01 2010
@@ -268,6 +268,10 @@
      * @param outputValue The set-point to sent to the motor controller.
      */
     public void set(double outputValue) {
+        this.set(outputValue, LM_API_SYNC_GROUP_NONE);
+    }
+
+    public void set(double outputValue, short syncGroup) {
         int messageID = 0;
         byte[] dataBuffer = new byte[8];
         byte dataSize = 0;
@@ -296,6 +300,12 @@
             default:
                 return;
         }
+
+        if(syncGroup != LM_API_SYNC_GROUP_NONE){
+            dataBuffer[dataSize] = (byte) syncGroup;
+            dataSize += 1;
+        }
+
         setTransaction(messageID, dataBuffer, dataSize);
     }
 
@@ -510,6 +520,19 @@
     }
 
     /**
+     * Sets the controller mode. Control will be automatically disabled when this method is called.
+     * 
+     * @param controlMode
+     */
+    public void setControlMode(CANJaguar.ControlMode controlMode ){
+        if(!m_controlMode.equals(controlMode)){
+            disableControl();
+            m_controlMode = controlMode;
+            initJaguar();
+        }
+    }
+
+    /**
      * Set the reference source device for speed controller mode.
      *
      * Choose encoder as the source of speed feedback when in speed control mode.
@@ -517,18 +540,10 @@
      *
      * @param reference Specify a SpeedReference.
      */
-    private void setSpeedReference(SpeedReference reference) {
+    public void setSpeedReference(SpeedReference reference) {
         byte[] dataBuffer = new byte[8];
-
-        switch (m_controlMode.value) {
-            case ControlMode.kSpeed_val:
                 dataBuffer[0] = reference.value;
                 setTransaction(LM_API_SPD_REF, dataBuffer, (byte) 1);
-                break;
-            default:
-                // TODO: Error, Invalid
-                return;
-        }
     }
 
     /**
@@ -541,16 +556,14 @@
      */
     public void setPositionReference(PositionReference reference) {
         byte[] dataBuffer = new byte[8];
-
-        switch (m_controlMode.value) {
-            case ControlMode.kPosition_val:
                 dataBuffer[0] = reference.value;
                 setTransaction(LM_API_POS_REF, dataBuffer, (byte) 1);
-                break;
-            default:
-                // TODO: Error, Invalid
-                return;
         }
+
+    public void syncUpdate(short syncGroup){
+        byte[] dataBuffer = new byte[8];
+        dataBuffer[0] = (byte) syncGroup;
+        sendMessage(CAN_MSGID_API_SYNC, dataBuffer, (byte) 1);
     }
 
     /**

JaguarCANProtocol.java:

--- C:/temp/JaguarCANProtocolOld.java	Thu Feb 25 13:29:27 2010
+++ C:/temp/JaguarCANProtocol.java	Mon Feb 22 15:09:27 2010
@@ -131,6 +131,16 @@
     //
     //*****************************************************************************
     public static final int LM_API_SYNC_PEND_NOW   = 0;
+    public static final short LM_API_SYNC_GROUP_NONE   = 0x00;
+    public static final short LM_API_SYNC_GROUP_1   = 0x01;
+    public static final short LM_API_SYNC_GROUP_2   = 0x02;
+    public static final short LM_API_SYNC_GROUP_3   = 0x04;
+    public static final short LM_API_SYNC_GROUP_4   = 0x08;
+    public static final short LM_API_SYNC_GROUP_5   = 0x10;
+    public static final short LM_API_SYNC_GROUP_6   = 0x20;
+    public static final short LM_API_SYNC_GROUP_7   = 0x40;
+    public static final short LM_API_SYNC_GROUP_8   = 0x80;
+    public static final short LM_API_SYNC_GROUP_ALL   = 0xFF;
 
     //*****************************************************************************
     //

[Bryscus]

Jason,

Let me apologize first. I was apparently having a long day and misspoke a number of times. I see what you are trying to do now and also must say that the mode is also set at initialization for C++ as well - I remembered incorrectly. I must warn you that I do not have any Java experience, so if there are any weird errors that are logical because of syntax I probably won't be able to spot them.

To preface my post, I am referencing Rev. 50 and 29 of the CANJaguar.java and JaguarCANDriver.java files, respectively located on the firstforge page.

Well, then. Let me begin:

1.) I'm not quite sure why you chose to make your LM_API_SYNC_GROUP_...(s) of type short and not byte. I do see that you cast them to a byte, but why not just keep them as bytes to begin with. Either way, unless there's some weird casting issue in Java, the group ID portion should still work just fine.

2.) In public void setControlMode(), you disableControl as you should. Do you make sure you re-enable control? You should also set up your encoder references again just to be safe and probably the number of lines too (couldn't hurt).

At this point, your code seems to look pretty good to me at least for switching modes. Next is my 2 bits for why your synchronous updates don't work.

1.) Your code looks good to me. I've delved into the bit masks and everything seems fine. Here's where it gets interesting. I'm willing to bet that the problem lies within the JaguarCANDriver.sendMessage() method that we can't see. Here is "protected void sendMessage(int messageID, byte[] data, int dataSize)":

 protected void sendMessage(int messageID, byte[] data, int dataSize) {
        final int[] kTrustedMessages = {
            LM_API_VOLT_T_EN, LM_API_VOLT_T_SET, LM_API_SPD_T_EN, LM_API_SPD_T_SET,
            LM_API_POS_T_EN, LM_API_POS_T_SET, LM_API_ICTRL_T_EN, LM_API_ICTRL_T_SET};

        byte i;
        for (i = 0; i < kTrustedMessages.length; i++) {
            if ((kFullMessageIDMask & messageID) == kTrustedMessages[i]) {
                sendTrustedDataBuffer[0] = 0;
                sendTrustedDataBuffer[1] = 0;
                // Make sure the data will still fit after adjusting for the token.
                if (dataSize > JaguarCANDriver.kMaxMessageDataSize - 2) {
                    // TODO: Error
                    return;
                }

                byte j;
                for (j = 0; j <
                        dataSize; j++) {
                    sendTrustedDataBuffer[j + 2] = data[j];
                }

                JaguarCANDriver.sendMessage(messageID, sendTrustedDataBuffer, dataSize + 2);
                return;
            }
        }
        JaguarCANDriver.sendMessage(messageID, data, dataSize);
    }

If you notice, the trusted messages are matched with constant values. This means that for trusted messages, the messageID does not contain the Device ID, otherwise the if statement with kTrustedMessages[i] would never be true. This means that JaguarCANDriver.sendMessage() must add the Device ID to the message that it sends (from m_deviceNumber). Normally this is fine, but this is BAD when you want to send a SYNC message that probably requires a Device ID value of 0. You might want to try setting m_deviceNumber to 0 before you send the sync command. So try:

    public void syncUpdate(short syncGroup){
        byte[] dataBuffer = new byte[8];
        dataBuffer[0] = (byte) syncGroup;
        //temporarily store m_deviceNumber;
        //set m_deviceNumber = 0;
        sendMessage(CAN_MSGID_API_SYNC, dataBuffer, (byte) 1);
        //restore m_deviceNumber to previous value;
    }

Again, I don't know the Java syntax so there's the pseudo-code.

Anyway, I think these are your problems. I have not implemented these changes in C++ yet, but I do have the plan to do so. I'll email Joe Hershberger about these potential issues. I've been looking at this code for 2-3 hours now so I think I'll just give it a rest for now.

I'm sorry if some of this doesn't make sense, but I'll try to answer any other questions that I can.

- Bryce

P.S. What is the status of the LEDs when your system stops working?

[imac256]

A couple of things:

1st:
The device number is set in the setTransaction method in the sendMessage call. Here is the code for that from Rev 50.

    byte setTransaction(int messageID, byte[] data, byte dataSize) {
        int ackMessageID = LM_API_ACK | m_deviceNumber;

        // Make sure we don't have more than one transaction with the same Jaguar outstanding.
        synchronized (m_transactionSemaphore) {
            // Throw away any stale acks.
//            receiveMessage(ackMessageID, kNoData, 10.0);
            // Send the message with the data.
            sendMessage(messageID | m_deviceNumber, data, dataSize);
            // Wait for an ack.
//            dataSize = receiveMessage(ackMessageID, kNoData, 0.0);
        }
        return dataSize;

The Bitwise OR operation in the sendMessage call appends the device number to the messageID field, so the sync routine should work since sendMessage is called directly.

The trusted messages feature works because they use a bitwise AND statement to determine the messageID for comparison, so this line:

if ((kFullMessageIDMask & messageID) == kTrustedMessages[i]) {

basically removes the device number from the ID temporarily for the check against the message headers to determine if it is trusted or not and then goes from there.

2nd:
I think that you might want to include more than just the API call in the messageID. For the syncronization program I am writing for my team in C++ I include every field so that my final message ID is this:

#define CAN_MSGID_MFR_BCAST         0x00000000    //Define the Broadcast manufacturer
//Define the Syncronization message Header
#define LM_API_SYNC       CAN_MSGID_DTYPE_BCAST | CAN_MSGID_MFR_BCAST| CAN_MSGID_API_SYNC

Unfortunately I have not had much time to actually test it thoroughly, but I believe it works. I will have to fire up the cRIO and some Jaguars that we kept this week to see what happens.

That is all that I notice right now, but with more information about what the Jaguars are doing we should be able to help more.

-Ian McInerney
Programmer, Team 2022

[Bryscus]

Ian,

Thanks for your feedback!

Originally Posted by imac256

A couple of things:

1st:
The device number is set in the setTransaction method in the sendMessage call. Here is the code for that from Rev 50.

You are correct about the setTransaction() method and the kTrustedMessages statement. I just missed it. I had been staring at it for a while and was multitasking as well.

However, because the SYNC message is not destined for just one device, the device number should NOT be sent. It is a broadcast message, as you have shown below. This could cause some issues. I think my setting of the m_deviceNumber to zero before sending the SYNC message is still the best way to go. OR, one could modify setTransaction().

2nd:
I think that you might want to include more than just the API call in the messageID. For the syncronization program I am writing for my team in C++ I include every field so that my final message ID is this:

#define CAN_MSGID_MFR_BCAST         0x00000000    //Define the Broadcast manufacturer
//Define the Syncronization message Header
#define LM_API_SYNC       CAN_MSGID_DTYPE_BCAST | CAN_MSGID_MFR_BCAST | CAN_MSGID_API_SYNC

You can do this, but it is redundant. Because CAN_MSGID_DTYPE_BCAST and CAN_MSGID_MFR_BCAST are both 0, there is no added information to the LM_API_SYNC other than CAN_MSGID_API_SYNC. The compiler will pre-compile with the same end result. It IS however, nicer code and more future friendly should anything change.

- Bryce

[JasonStern]

Thank you for the information, feedback, and suggestions!

Originally Posted by Bryscus

...because the SYNC message is not destined for just one device, the device number should NOT be sent. It is a broadcast message, as you have shown below. This could cause some issues. I think my setting of the m_deviceNumber to zero before sending the SYNC message is still the best way to go. OR, one could modify setTransaction().

I haven't looked at the C code, but in Java m_deviceNumber is only used in setTransaction and not sendMessage. However, I didn't realize before that JaguarCANDriver.sendMessage is static (CANJaguar.sendMessage is not). Because the sync should be send to all devices, I recommend modifying syncUpdate to

public static void syncUpdate(short syncGroup){
        byte[] dataBuffer = new byte[8];
        dataBuffer[0] = (byte) syncGroup;
        JaguarCANDriver.sendMessage(CAN_MSGID_API_SYNC, dataBuffer, (byte) 1);
}

This removes any potential tie to the device id since you won't have to have created an object first and better matches how the CAN bus actually operates.

Originally Posted by imac256

For the syncronization program I am writing for my team in C++ I include every field so that my final message ID is this:

#define CAN_MSGID_MFR_BCAST         0x00000000    //Define the Broadcast manufacturer
//Define the Syncronization message Header
#define LM_API_SYNC       CAN_MSGID_DTYPE_BCAST | CAN_MSGID_MFR_BCAST| CAN_MSGID_API_SYNC

Thank you for the suggestion!

Originally Posted by Bryscus

1.) I'm not quite sure why you chose to make your LM_API_SYNC_GROUP_...(s) of type short and not byte.

Java doesn't have unsigned numbers and makes it tricky to interface with outside systems that do. Stepping up the variable size to the next level was partly force of habit and partly because the IDE was complaining otherwise. I agree the 'final' version should be using byte instead, but this code was written an hour before crating and I wanted something I knew would (should? ) work. :-)

Originally Posted by Bryscus

In public void setControlMode(), you disableControl as you should. Do you make sure you re-enable control? You should also set up your encoder references again just to be safe and probably the number of lines too (couldn't hurt).

I thought about this and decided it was better to re-enable control and perform the config outside of the method to avoid potential issues. This way, the setControlMode() method should be functionally equivalent to creating a new CANJaguar object.

Originally Posted by Bryscus

At this point, your code seems to look pretty good to me at least for switching modes.

You would think that, but it only worked a few times before stopping randomly and failing to work again!

Originally Posted by Bryscus

Anyway, I think these are your problems. I have not implemented these changes in C++ yet, but I do have the plan to do so. I'll email Joe Hershberger about these potential issues.

On a semi-related note, shouldn't the sync code be trusted as well? Otherwise I see a potential case where you define the set point when the robot is enabled, the robot then gets disabled, and then the sync command gets sent. Or am I missing something else that would prevent this from happening?

Originally Posted by Bryscus

What is the status of the LEDs when your system stops working?

I honestly couldn't tell you at this point. I know it worked a few times before stopping, but I was testing this an hour before shipping so everyone was breathing down my neck to give them back the robot . We have a spare cRio and I think we still have some old CIMs with encoders on them, so I will try to get the team to do some more testing before the competition on Thursday. (The joys of being in a first week regional. ) However, at this point it is more for my interest and to help the FIRST community than for us to put into production use. I gave guidance to our programmer to only operate in speed control mode using the current functionality due to the issues we experienced. It would have been nice if he was able to say "drive forward 10 feet" in autonomous mode, but I guess that is something to look forward to next year!

[jhersh]

Originally Posted by JasonStern

On a semi-related note, shouldn't the sync code be trusted as well? Otherwise I see a potential case where you define the set point when the robot is enabled, the robot then gets disabled, and then the sync command gets sent. Or am I missing something else that would prevent this from happening?

From looking at the jag source it appears that it would indeed enable while the robot is disabled, but it would time out after 100ms and turn back off. While it is a shortcoming of the jag firmware, it isn't a significant safety risk.

Joe

[Bryscus]

Jason,

When you do get this working, please share what the problems were with the original code, as we're also interested in implementing this in C++. Thanks.

- Bryce