Tetrix Mecanum code freezes and locks up

Projects Programming
1

Hi everyone,

I'm the programmer for my school's robotics club. This year, we've built a bot using Tetrix Mecanum wheels, running on Torquenado motors and programmed with a Prizm board. To control the wheels, we're using Tetrix's example tele-op Mecanum code (which can be found here and which requires the DC motor expansion controller library). For our controller, we're using a PS4 controller with tetrix's tele-op library.

One problem we've been encountering is that the code, after a certain period of driving, will stop responding, causing the bot to either stop driving or to keep driving in the direction that the stick was held as it locked up (usually leading to it driving until it hits a wall). I've asked many of the previous year's programmers about this, and I've tried debugging the values and whatnot, but nothing comes up. Keep in mind that the code fully stops responding, even other functions/lines in the code will not respond. Not sure if this is a hardware or software problem. The previous year's programmer told me that if the Prizm boards are powered by an undercharged battery it can affect the code execution, but if I take the driver's word for it this issue has happened a few times on charged batteries.

TLDR; mecanum wheels being controlled with PS4 controller. bug happens where code randomly stops in the middle of driving, causing bot to stop moving or to keep driving in one direction uncontrollably.

2

Can you share your Arduino code?

3

Yes, here:

#include <PRIZM.h>      // Include the PRIZM library in sketch
#include <TELEOP.h>     // TETRIX Tele-Op Arduino Library
#include <AsyncTimer.h>

AsyncTimer t;

PRIZM prizm;            // Instantiate an object in the PRIZM class named "prizm"
EXPANSION exc;          // Instantiate an object in the EXPANSION class named "exc"
PS4 ps4;                // Instantiate an object in the PS4 class named "ps4"


float highSpeed = 1;                  // variable to set the motor percentage (100%) for turbo mode
float lowSpeed = highSpeed*0.25;       // variable to set the motor percentage (25%) for crawl mode
float medSpeed = highSpeed*1;       // variable to set the motor percentage (40%) for normal mode
float powerMultiplier = medSpeed;     // variable to set the motor power percentage (currently at medPower or 40%)

int mPowers[] = {0,0,                 // array to track motor powers for the 4 different motors (front left, front right...
                 0,0};                //                                                         back left,  back right)

void setup() {
  
  prizm.PrizmBegin();                 // Initiates the PRIZM controller
  ps4.setDeadZone(LEFT,30);           // set the left joystick dead zone range to +/- 10 
  ps4.setDeadZone(RIGHT,30);          // set the right joystick dead zone range to +/- 10
  prizm.setMotorInvert(1,1);          // invert front left motor
  exc.setMotorInvert(1,1,1);          // invert back left motor
  prizm.setServoSpeed(1,100);
  prizm.setServoPosition(1,10);
  prizm.setCRServoState(1,0);
  prizm.setServoPosition(1,40);

  Serial.begin(9600);
  Serial.println("hello");
}

void loop() {
  ps4.getPS4();                                                     // get the status of the PS4 button states                                                               // end while loop for when cross button is pressed
  t.handle();

  prizm.setCRServoState(1,100); //Turn intake Servo
      
  //Push Ball through shooter
  if (ps4.Button(R1)) {
    prizm.setServoPosition(1,80);
    t.setTimeout([]() {
      prizm.setServoPosition(1,40);
    }, 500
    );
 } 

  adjustMotorPowers();                                              // call function to adjust powerMultiplier based on what buttons are pressed

/* The next two lines of code set the mPowers array to the motor power values needed for the four different motors. 
 *  The array is set up in the following pattern {left front motor connected to PRIZM motor 1, right front motor connected to PRIZM motor 2,
 *                                                left back motor connected to exc motor 1, right back motor connected to exc motor 2}
 *                                                
 *  Equations are used to determine the power level of each motor based on PS4 joystick positions. Here are the equations for each motor:
 *  Front Left Motor:  (LY + LX - RX) * powerMultiplier         (Left Stick Y Position + Left Stick X Position - Right Stick X Position)
 *  Front Right Motor: (LY - LX + RX) * powerMultiplier         (Left Stick Y Position - Left Stick X Position + Right Stick X Position)
 *  Back Left Motor:   (LY - LX - RX) * powerMultiplier         (Left Stick Y Position - Left Stick X Position - Right Stick X Position)
 *  Back Right Motor:  (LY + LX + RX) * powerMultiplier         (Left Stick Y Position + Left Stick X Position + Right Stick X Position)
 *  Each motor power is then constrained between -100 and 100 so that when both sticks are used at the same time the power never exceeds 100.
 */
  
  mPowers[0] = constrain((ps4.Motor(LY)+ps4.Motor(LX)-ps4.Motor(RX))*powerMultiplier,-100,100);
  mPowers[1] = constrain((ps4.Motor(LY)-ps4.Motor(LX)+ps4.Motor(RX))*powerMultiplier,-100,100);
  mPowers[2] = constrain((ps4.Motor(LY)-ps4.Motor(LX)-ps4.Motor(RX))*powerMultiplier,-100,100);
  mPowers[3] = constrain((ps4.Motor(LY)+ps4.Motor(LX)+ps4.Motor(RX))*powerMultiplier,-100,100);
  
  prizm.setMotorPowers(mPowers[0], mPowers[1]);                     // set PRIZM motor powers based on the values from the mPowers array
  exc.setMotorPowers(1,mPowers[2], mPowers[3]);                     // set EXC motor powers based on the values from the mPowers array

} // close loop void

void adjustMotorPowers(){                                           // called function to change the low, med, and high speed motor powers
  if(ps4.Button(L1)==1){                                              // if the L1 button is pressed: 
    powerMultiplier = lowSpeed;                                         // set powerMultiplier to lowSpeed for crawl mode
    ps4.setRumble(SLOW);                                                // set the PS4 ruble speed to slow
    ps4.setLED(RED);}                                                   // set the game pad LED to red to indicate crawl mode
  else {                                                              // otherwise, the first two conditions are not true, so:
    powerMultiplier = medSpeed;                                         // set the powerMultiplier to medSpeed for normal mode
    ps4.setRumble(STOP);                                                // set the PS4 ruble speed to off
    ps4.setLED(YELLOW);}                                                // set the game pad LED to yellow to indicate normal mode
}
4

Can you run it on blocks so the serial monitor can be used for debugging?

5

I'm not at school right now so I'll let you know tomorrow. Earlier this year I was debugging motor speeds using the monitor, and when it locked up it would just display the same value over and over.

6

Did you find the reason?

7

No, that's why I made this thread.

8

Oh, ok.
Then can you show the code for that last attempt and the messages you got? Just so we don't plow the same ground that you have already gone though.

9

Is ps4.Button(R1) set to true only once, or could it be true on each loop pass?
If your loop try to create a new timeout each time, you will loose some timeouts. It would be better if you "reset" the timeout instead of creating new ones if it is still active.

10

ps4.button(R1) is read every loop because it needs to read the state of the controller trigger button to see if it's pressed. As for the timeouts, I'll try calling the reset function instead of redeclaring it, but I doubt that has any issue to do with code freezing as those timer functions are asynchronous (using the AsyncTimer library).

11

I don't have those logs, those are from months ago. The logs essentially looked like this:

// simulated driving
motor power:0.0
motor power:50.0
motor power:100.0
motor power:50.0
motor power:0.0
motor power:0.0
// driving again
motor power:0.0
motor power:50.0
motor power:100.0
// gets stuck, at this point moving sticks and everything else stops working. the only way to stop is to re-run the code by pressing the green button. keep in mind that it didn't always get stuck at 100, this is just an example.
motor power:100.0
motor power:100.0
motor power:100.0
motor power:100.0
motor power:100.0
motor power:100.0
motor power:100.0
12

The problem has been happening with all iterations of this code.

13

Did you NOT add debug messages to track down the failure?

14

I did, above I have posted a rough example of what the logs looked like. I will post a direct copy of the logs tomorrow.

15

ps4.button(R1) is read every loop because it needs to read the state of the controller trigger button to see if it's pressed.

Let me explain it better:
Your loop need probably 1ms to run.
So, once you press the button, you will create 500 timers in 500ms.
But since you can create only 10 timers, the other 490 timers will not be called.
After 500ms, your code will set the Servo to 80 and at the same time it will be set to 40 (it will be toggled 10 times, then the servo will be set again to 80).

I don't think this is what you want. Maybe it has nothing to do with the motor freezing, but you are getting the wrong result.

16

Okay, here's an actual log I just got from the board.

Motor1: -100, Motor2: -48, Motor3: -48, Motor4: -100
Motor1: -100, Motor2: -45, Motor3: -45, Motor4: -100
Motor1: -100, Motor2: -45, Motor3: -45, Motor4: -100
Motor1: -100, Motor2: -45, Motor3: -45, Motor4: -100
Motor1: -100, Motor2: -45, Motor3: -45, Motor4: -100
Motor1: -100, Motor2: -45, Motor3: -45, Motor4: -100
Motor1: -100, Motor2: -45, Motor3: -45, Motor4: -100
Motor1: -100, Motor2: -49, Motor3: -49, Motor4: -100
Motor1: -100, Motor2: -56, Motor3: -56, Motor4: -100
Motor1: -100, Motor2: -100, Motor3: -100, Motor4: -100
Motor1: -100, Motor2: -100, Motor3: -100, Motor4: -100
Motor1: -100, Motor2: -100, Motor3: -100, Motor4: -100
Motor1: -13, Motor2: -100, Motor3: -100, Motor4: -13
Motor1: 47, Motor2: -100, Motor3: -100, Motor4: 47
// the line BELOW is where it froze. it maintained these values for several seconds before returning to zero.
Motor1: 68, Motor2: -100, Motor3: -100, Motor4: 68
Motor1: 0, Motor2: 0, Motor3: 0, Motor4: 0
Motor1: 0, Motor2: 0, Motor3: 0, Motor4: 0
Motor1: 0, Motor2: 0, Motor3: 0, Motor4: 0

It froze when the joystick was moved too quickly, this is an issue I've encountered even on example code. I have a feeling it has to do with the board not being able to process data that fast. The only solution I've found to limiting data speeds is to delay when the data is read, which is not optimal as it limits the amount of precision the driver has to move the bot.

In last week's testing, when it kept permanently locking up, I don't think the driver had actually checked that we were on full batteries. I've been told by last year's programmer that batteries below 90 to 85 percent charge tend to cause issues with the board.

17

You do not show any debug messages that allow you to track the LOGIC of your program. Display a message before and after any function calls, so you can see the last one called before the problem.

18

I would also like to add that this problem happens even with the stock manufacturer code (from Studica and/or Tetrix, they're both identical anyway).

19

I guess either the stock manufacturer code has the same bug, or it is a hardware issue.

20

Okay, I need to do more extensive testing but as far as my testing yesterday had gone the issue is almost certainly resolved:

(Unless it fails in future testing and proves me wrong), it was indeed a hardware issue. I had been testing with different levels of battery charge, different code revisions (including stock manufacturer code), and different combinations of deactivating the front/back pairs of wheels to see if it was a voltage issue. I'm not exactly sure why, but the following is what has seemingly fixed it: the back wheels were on a Tetrix DC expander module, while the front wheels were connected to the Prizm board. Connecting the front wheels to their own DC expander has seemingly fixed the issue.

Will update tomorrow if anything arises.