Can not control two motors at the same time because of sequencing

Hey Guys, i have a problem with a project. The goal is to use a pixy cam to control 2 wheel hub motors. Both motors are connected to a single controller which uses simple commands for the control. We can just send serial commands such as “f41” to start the motors “d0-255” for block speed “l0-255” and “r0-255” for left and right motors individually. Below you can see my code. So basically its the origignal pixy code for zumoMotors which i tried to adapt for my case. Dont mind all the stuff in the code i dont need for my projcect, i just want to see it work before i clean that up.

As u can see commands are sent via softwareserial.

#include <SoftwareSerial.h>
SoftwareSerial mySerial(3, 2); // RX, TX


#include <Pixy2.h>
#include <PIDLoop.h>
#include <ZumoMotors.h>

// this limits how fast Zumo travels forward (400 is max possible for Zumo)
#define MAX_TRANSLATE_VELOCITY  250

Pixy2 pixy;
ZumoMotors motors;

PIDLoop panLoop(350, 0, 600, true);
PIDLoop tiltLoop(500, 0, 700, true);
PIDLoop rotateLoop(300, 600, 300, false);
PIDLoop translateLoop(400, 800, 300, false);

void setup()
{
  Serial.begin(115200);
  mySerial.begin(38400);
  Serial.print("Starting...\n");
 
  
  // initialize motor objects
  motors.setLeftSpeed(0);
  motors.setRightSpeed(0);
  
  // need to initialize pixy object
  pixy.init();
  // user color connected components program
  pixy.changeProg("color_connected_components");
}

// Take the biggest block (blocks[0]) that's been around for at least 30 frames (1/2 second)
// and return its index, otherwise return -1
int16_t acquireBlock()
{
  if (pixy.ccc.numBlocks && pixy.ccc.blocks[0].m_age>30)
    return pixy.ccc.blocks[0].m_index;

  return -1;
}

// Find the block with the given index.  In other words, find the same object in the current
// frame -- not the biggest object, but he object we've locked onto in acquireBlock()
// If it's not in the current frame, return NULL
Block *trackBlock(uint8_t index)
{
  uint8_t i;

  for (i=0; i<pixy.ccc.numBlocks; i++)
  {
    if (index==pixy.ccc.blocks[i].m_index)
      return &pixy.ccc.blocks[i];
  }

  return NULL;
}


void loop()
{  
  static int16_t index = -1;
  int32_t panOffset, tiltOffset, headingOffset, left, right;
  Block *block=NULL;
  
  pixy.ccc.getBlocks();

  if (index==-1) // search....
  {
    
    Serial.println("Searching for block...");
    index = acquireBlock();
    if (index>=0)
      Serial.println("Found block!");
      mySerial.println("f45");
      Serial.println("f45");
      
 }
  // If we've found a block, find it, track it
  if (index>=0)
     block = trackBlock(index);

  // If we're able to track it, move motors
  if (block)
  {
    // calculate pan and tilt errors
    panOffset = (int32_t)pixy.frameWidth/2 - (int32_t)block->m_x;
    tiltOffset = (int32_t)block->m_y - (int32_t)pixy.frameHeight/2;  

    // calculate how to move pan and tilt servos
    panLoop.update(panOffset);
    tiltLoop.update(tiltOffset);

    // move servos
    pixy.setServos(panLoop.m_command, tiltLoop.m_command);

    // calculate translate and rotate errors
    panOffset += panLoop.m_command - PIXY_RCS_CENTER_POS;
    tiltOffset += tiltLoop.m_command - PIXY_RCS_CENTER_POS - PIXY_RCS_CENTER_POS/2 + PIXY_RCS_CENTER_POS/8;

    rotateLoop.update(panOffset);
    translateLoop.update(-tiltOffset);

    // keep translation velocity below maximum
    if (translateLoop.m_command>MAX_TRANSLATE_VELOCITY)
      translateLoop.m_command = MAX_TRANSLATE_VELOCITY;

    // calculate left and right wheel velocities based on rotation and translation velocities
    left = -rotateLoop.m_command + translateLoop.m_command;
    right = rotateLoop.m_command + translateLoop.m_command;

    // set wheel velocities
    
  //  Serial.println(left);
  //  Serial.println(right);
    

    
    int anReadl = left;
    anReadl = anReadl/12;
    int anReadr = right;
    anReadr = anReadr/12;
   
    motors.setRightSpeed(right);
    motors.setLeftSpeed(left);
   
 
    Serial.print("r");
    Serial.println(anReadr);
    Serial.print("l");
    Serial.println(anReadl);
    
    mySerial.print("r");
    mySerial.println(anReadr);
    mySerial.print("l");
    mySerial.println(anReadl);
    
   

    // print the block we're tracking -- wait until end of loop to reduce latency
    block->print();
  }  
  else // no object detected, stop motors, go into search state
  {
    rotateLoop.reset();
    translateLoop.reset();
    motors.setLeftSpeed(0);
    motors.setRightSpeed(0);
    index = -1; // set search state
    mySerial.println("d0");
    Serial.println("d0");
  }
}

Here u can see whats problematic:

// calculate left and right wheel velocities based on rotation and translation velocities
    left = -rotateLoop.m_command + translateLoop.m_command;
    right = rotateLoop.m_command + translateLoop.m_command;

    // set wheel velocities
    
  //  Serial.println(left);
  //  Serial.println(right);
    

    
    int anReadl = left;
    anReadl = anReadl/12;
    int anReadr = right;
    anReadr = anReadr/12;
   
    motors.setRightSpeed(right);
    motors.setLeftSpeed(left);
   
 
    Serial.print("r");
    Serial.println(anReadr);
    Serial.print("l");
    Serial.println(anReadl);
    
    mySerial.print("r");
    mySerial.println(anReadr);
    mySerial.print("l");
    mySerial.println(anReadl);

The anRead/12 is just to slow the motors down a bit, we will map that later to meet our requirements. The problem is that we are only able to operate one motor with this code. Although we can see commands for left AND right motor in the serial monitor, only one of the motors will actually work when testing. As it is right now, only the right motor works. If we switch the sequence so that the left motor is sequenced before the right motor only the left motor works.

What causes that? Why is only one motor responding? How could we change it so both motors can be controlled at the same time.

I see your Serial.print instructions, but I don't see any that are displaying the actual command being sent to the controller. Why guess at what is being sent when you can actually show the values?

Paul

Vashet: Dont mind all the stuff in the code i dont need for my projcect, i just want to see it work before i clean that up.

It's very much easier to sort out problems (and to give help) with a short program. Do the clean up now. And then post the shorter version.

...R

Paul_KD7HB: I see your Serial.print instructions, but I don't see any that are displaying the actual command being sent to the controller. Why guess at what is being sent when you can actually show the values?

Paul

We are using softwareserial called mySerial to send to commands, we are using that also for direct control via smartphone which works flawless

Robin2: It's very much easier to sort out problems (and to give help) with a short program. Do the clean up now.

And who knows, in the process of doing the cleanup, the problem might resolve.

It looks like the problem MUST be in the ZumoMotors library. How does the library know you want to use “mySerial” for output? Does the library sent the right commands? Is it waiting for a reply?

SoftwareSerial mySerial(3, 2); // RX, TX
#include <ZumoMotors.h>
ZumoMotors motors;

void setup()
{
  mySerial.begin(38400);

    motors.setRightSpeed(right);
    motors.setLeftSpeed(left);
}