Tilt robot motor code not working

Hi everyone. I am working on a wirelessly controlled robot, that responds and moves according to the movement of an accelerometer on a wireless controller. Basically, you tilt the controller in the direction you want the robot to move. The communication between the robot and the controller are 2 XBees. The robot uses an Arduino Leonardo, and the controller uses a 3.3V Pro Mini. The accelerometer is the 3-axis analog ADXL335. I also have an LCD connected to the Leonardo for debugging. I am also using a library called EasyTransfer for communication between the XBees, which is just serial communication.

The robot has 2 wheels, and are able to be drive in either direction, with a separate PWM speed control.

The robot uses differential drive, so when turning in direction, the wheel on that side of the robot spins slower to allow it to veer in that direction. Also, it can simply spin one motor one way and the other the other way to spin in place.

Here's how my basic program flow is:

  1. Pro Mini on controller reads analog values from ADXL335 accelerometer
  2. Pro Mini does some simple math, converts raw analog values to degrees of rotation for each axis. (0 to 360).
  3. Pro Mini sends a packet of data over serial to local XBee, which transmits data to remote XBee on robot
  4. Leonardo receives serial data from local XBee containing accelerometer data
  5. Leonardo uses accelerometer data to control 2 motors
  6. Leonardo displays X, Y, Z data on LCD
  7. Leonardo comes up with how to drive the motors based on X, Y, Z data.

Now, I need the motors to spin based on the amount of tilt either forward or backwards (X axis). And when turning, the turning motor has to spin slower than the regular speed motor while maintaining a relationship with amount of tilt.

Now, just for clarification, when tilting backward, the X values wrap back around to 0, and increase as tilting farther back, looping back around to 360. So when sitting level, making a 360 degree flip backwards reads X values rising from 0 to 360. Same with Y axis. Flipping leftward all the way makes values rise from 0 back around to 360.

Also, the farther you tilt forward/backward, the faster the general speed of the motors is, but the farther you tilt to the side, the slower the turning motor is in relation to the other one.

Now, my problem lies in implementing turning. I have the code down for moving forward and backward, but whenever I try to implement turning, the code doesn't do anything. As in, it doesn't 'recognize' that I'm turning or gets stuck or something. I don't know. Here's my working code on the Leonardo, with only forward/backward movement implemented:

EDIT: Because of the character limit on posts, I wasn't able to include the whole code here. Instead, I broke it into 2 parts and posted it in the following replies. I also attached a copy of my whole code. Please refer to below posts when I'm talking about stuff here.

Now, this only takes into account forward/backward X tilt, and it works great. I am using a mathematical equation to figure out PWM values instead of map() so I can modify it to figure out turning PWM values. Now, with the code above, If I modify the line:

if (driveForward == true && driveBackward == false) { // If forward
    motorForward();
    
    constXVal = constrain(mydata.xVal, forwardRangeMin, forwardRangeMax); // Constrain values that will be converted
    int xPWM = (pwmFullMin + (pwmFullRange * ((forwardRangeMax - constXVal)/forwardRange) ) ); // Convert amount of tilt to PWM values
    
    analogWrite(motorAPWM, xPWM);
    analogWrite(motorBPWM, xPWM);}    
   
  if (driveForward == false && driveBackward == true){ // If backward
    motorBackward();
   
    constXVal = constrain(mydata.xVal, backwardRangeMin, backwardRangeMax); // Constrain values that will be converted
    int xPWM = (pwmFullMin + (pwmFullRange * (constXVal/backwardRange) ) ); // Convert amount of tilt to PWM values
    
    analogWrite(motorAPWM, xPWM); // Set both motors to same speed
    analogWrite(motorBPWM, xPWM);}

To this:

if ((driveForward == true && driveBackward == false) && (driveRight == false && driveLeft == false)) { // If forward
    motorForward();
    
    constXVal = constrain(mydata.xVal, forwardRangeMin, forwardRangeMax); // Constrain values that will be converted
    int xPWM = (pwmFullMin + (pwmFullRange * ((forwardRangeMax - constXVal)/forwardRange) ) ); // Convert amount of tilt to PWM values
    
    analogWrite(motorAPWM, xPWM);
    analogWrite(motorBPWM, xPWM);}    
   
  if ((driveForward == false && driveBackward == true) && (driveRight == false && driveLeft == false)) { // If backward
    motorBackward();
   
    constXVal = constrain(mydata.xVal, backwardRangeMin, backwardRangeMax); // Constrain values that will be converted
    int xPWM = (pwmFullMin + (pwmFullRange * (constXVal/backwardRange) ) ); // Convert amount of tilt to PWM values
    
    analogWrite(motorAPWM, xPWM); // Set both motors to same speed
    analogWrite(motorBPWM, xPWM);}

To check to see if I am only tilting forward, it doesn't work. It simply doesn't recognize any movement forward, renders it obsolete, and only runs the backward tilt code that's further on.

Also, any implementation of the motorTurn() function renders the motors useless, as they don't spin. It seems that my variable xPWM is getting lost, and because of that, yPWM gets screwed up. I wish I could post a video, but my camera's dead. Also, I'll attach a few drawings I made that might help explain it more.

Basically, in short, any implemenation of recognizing Y tilt, and turning the robot fail. I simply can't implement proper turning, the way I want.

PLEASE, If you think you can help, I'll be MORE than happy to provide further information about my code/robot/dilemma!

Also, look at following posts for whole code. It's cut in the middle and divided over two posts.

LeonardoETReceiveTest2.ino (6.48 KB)

WHOLE CODE --- PART 1:

// * INCLUDE LIBRARIES * //

#include <EasyTransfer.h>
#include <LiquidCrystal.h>

// * DEFINE EVERYTHING * //

double xVal; // Accel values
double yVal;
double zVal;

double constXVal; // The constrained xVal and yVal
double constYVal;

double xDeg; // Accel values
double yDeg;
double zDeg;

int xPWM; // Value to use for PWM on both motors when driving straight
int yPWM; // Value to use for PWM on either motor when turning
float xPWMRef; // X PWM value for reference when used in finding out Y PWM

int forwardRangeMax = 345; // X accel values for range of forward movement
int forwardRangeMin = 300;
int backwardRangeMax = 60;
int backwardRangeMin = 15;

int rightRangeMax = 345; // Y accel values for range of sideways movement
int rightRangeMin = 300;
int leftRangeMax = 60;
int leftRangeMin = 15;

int forwardRange = (forwardRangeMax - forwardRangeMin); // How many values between Min and Max
int backwardRange = (backwardRangeMax - backwardRangeMin);
int rightRange = (rightRangeMax - rightRangeMin);
int leftRange = (leftRangeMax - leftRangeMin);

int pwmFullMin = 115; // Lowest PWM duty cycle to reach on both motors when driving straight
int pwmFullMax = 205; // Highest PWM duty cycle to reach on both motors when driving straight
int pwmTurnMin = 95; // Lowest PWM duty cycle to reach on either motor when turning
int pwmTurnMax = 185; // Highest PWM duty cycle to reach on either motor when turning

int pwmFullRange = (pwmFullMax - pwmFullMin); // Range of usable PWM values
int pwmTurnRange = (pwmTurnMax - pwmTurnMin);

boolean driveForward; // Define some later used variables that let us know where the robot is moving
boolean driveBackward;
boolean driveRight;
boolean driveLeft; 


int motorA1 = 8; // * Define
int motorA2 = 9; // all of 
int motorB1 = 11; // the
int motorB2 = 12; // motor
int motorAPWM = 10; // driver
int motorBPWM = 13; // pins *

// * DEFINE XBEE DATA STUFF * //

EasyTransfer ET;

struct RECEIVE_DATA_STRUCTURE
{
  int xVal;
  int yVal;
  int zVal;
};

RECEIVE_DATA_STRUCTURE mydata;

// * DEFINE LCD PINS * //

LiquidCrystal lcd(7, 6, 5, 4, 3, 2);

// * SETUP * //

void setup()
{  
  // * BEGIN COMMUNICATION WITH LOCAL XBEE * //
  Serial1.begin(9600);
  // * BEGIN COMMUNICATION WITH REMOTE XBEE * //
  ET.begin(details(mydata), &Serial1);
  // * DEFINE LCD TYPE * //
  lcd.begin(16, 2);
  
  // * SET MOTOR PINS TO OUTPUTS * //
  pinMode(motorA1, OUTPUT);
  pinMode(motorA2, OUTPUT); 
  pinMode(motorB1, OUTPUT);
  pinMode(motorB2, OUTPUT);
  pinMode(motorAPWM, OUTPUT);
  pinMode(motorBPWM, OUTPUT);
}

// * MAIN PROGRAM * //

void loop()
{
  // * RUN IF SERIAL DATA FROM XBEE RECEIVED * //
  if(ET.receiveData())
  {
    lcdDisplay();
    if ((mydata.xVal < forwardRangeMax && mydata.xVal > backwardRangeMin) || (mydata.yVal < rightRangeMax && mydata.yVal > leftRangeMin)){ // If enough accelerometer movement detected...
    motorDriveDeterm();
    motorDrive();}
    else{
      motorBrake();}      
  }
  delay(50); // Needed by ET library
}

WHOLE CODE --- PART 2:

void motorDriveDeterm()
{  
    if (mydata.xVal > forwardRangeMin && mydata.xVal < forwardRangeMax){ // If forward
      driveForward = true;
      driveBackward = false;}
    
    if (mydata.xVal > backwardRangeMin && mydata.xVal < backwardRangeMax){ // If backward
      driveForward = false;
      driveBackward = true;}
   
     if (mydata.yVal <= leftRangeMin && mydata.yVal >= rightRangeMax){ // If going straight
      driveRight = false;
      driveLeft = false;}
    
    if (mydata.yVal > rightRangeMin && mydata.yVal < rightRangeMax){ // If right
     driveRight = true;
     driveLeft = false;}
    
    if (mydata.yVal > leftRangeMin && mydata.yVal < leftRangeMax){ // If left
      driveRight = false;
      driveLeft = true;}  
}

void motorDrive() // To actually drive motors
{
  if (driveForward == true && driveBackward == false) { // If forward
    motorForward();
    
    constXVal = constrain(mydata.xVal, forwardRangeMin, forwardRangeMax); // Constrain values that will be converted
    int xPWM = (pwmFullMin + (pwmFullRange * ((forwardRangeMax - constXVal)/forwardRange) ) ); // Convert amount of tilt to PWM values
    
    analogWrite(motorAPWM, xPWM);
    analogWrite(motorBPWM, xPWM);}    
   
  if (driveForward == false && driveBackward == true){ // If backward
    motorBackward();
   
    constXVal = constrain(mydata.xVal, backwardRangeMin, backwardRangeMax); // Constrain values that will be converted
    int xPWM = (pwmFullMin + (pwmFullRange * (constXVal/backwardRange) ) ); // Convert amount of tilt to PWM values
    
    analogWrite(motorAPWM, xPWM); // Set both motors to same speed
    analogWrite(motorBPWM, xPWM);}    
}

void motorTurn()
{
  if (driveRight == true && driveLeft == false){ // If turning to the right
    constYVal = constrain(mydata.yVal, rightRangeMin, rightRangeMax); // Constrain values that will be converted
    int yPWM = (xPWMRef - (pwmTurnRange * ((rightRangeMax - constYVal)/rightRange) ) ); // Convert amount of tilt to PWM values
    analogWrite(motorAPWM, yPWM); // Right motor will spin slower
    analogWrite(motorBPWM, xPWM);} // Left motor will spin regular speed
    
    
  if (driveRight == false && driveLeft == true){ // If turning to the left
    constYVal = constrain(mydata.yVal, leftRangeMin, leftRangeMax);
    int yPWM = (xPWMRef - (pwmTurnRange * (constYVal/leftRange) ) );
    analogWrite(motorAPWM, xPWM); // Right motor will spin regular speed
    analogWrite(motorBPWM, yPWM);} // Left motor will spin slower
}

void motorForward()
{
  digitalWrite(motorA1, HIGH); // Make motor A and B spin forward
  digitalWrite(motorA2, LOW);
  digitalWrite(motorB1, HIGH);
  digitalWrite(motorB2, LOW);
}

void motorBackward()
{ 
  digitalWrite(motorA1, LOW); // Make motor A and B spin backward
  digitalWrite(motorA2, HIGH);
  digitalWrite(motorB1, LOW);
  digitalWrite(motorB2, HIGH);
}

void motorBrake()
{ 
  digitalWrite(motorA1, HIGH); // Make motor A and B stop spinning
  digitalWrite(motorA2, HIGH);
  digitalWrite(motorB1, HIGH);
  digitalWrite(motorB2, HIGH);
  digitalWrite(motorAPWM, LOW);
  digitalWrite(motorBPWM, LOW);
}

void lcdDisplay()
{
    // * ROUTINE TO DISPLAY RECEIVED X, Y, Z VALUES ON LCD * //
    lcd.clear();
    // * DISPLAY X ACCEL DATA * //
    lcd.setCursor(0, 0);
    lcd.print("x=");
    lcd.print(mydata.xVal);
    lcd.write((char)223);
    // * DISPLAY Y ACCEL DATA * //
    lcd.setCursor(10, 0);
    lcd.print("y=");
    lcd.print(mydata.yVal);
    lcd.write((char)223);
    // * DISPLAY Z ACCEL DATA * //
    lcd.setCursor(5, 1);
    lcd.print("z=");
    lcd.print(mydata.zVal);   
    lcd.write((char)223);
}
    if ((mydata.xVal < forwardRangeMax && mydata.xVal > backwardRangeMin) || (mydata.yVal < rightRangeMax && mydata.yVal > leftRangeMin)){ // If enough accelerometer movement detected...

It is possible to add white space to this code to make it more readable:

    if ((mydata.xVal < forwardRangeMax && 
      mydata.xVal > backwardRangeMin) || 
     (mydata.yVal < rightRangeMax && 
      mydata.yVal > leftRangeMin))
    { // If enough accelerometer movement detected...

There are a variety of styles for C/C++ coding, that dictate where the { goes, with respect to the block that follows. NONE of them call for putting the } on the same line as any other code.

Now, my problem lies in implementing turning. I have the code down for moving forward and backward, but whenever I try to implement turning, the code doesn't do anything. As in, it doesn't 'recognize' that I'm turning or gets stuck or something. I don't know.

You need to debug your code somehow. Generally, that means using the serial port to write to the serial monitor. Reading the accelerometer and sending that data to the robot does not appear to be the problem. So, temporarily dump all that code.

Write some code that reads from the serial monitor, instead. Send the robot KNOWN instructions (i.e. known values of x, y, and z). Use Serial.print() to echo that data, to confirm that it is being received correctly. Use Serial.print() to show what motorDriveDeterm() decides should be done.

There are no else statements in motorDriveDeterm(), so it is likely that driveRight and driveForward are both true (or any number of other combinations). Is that going to be OK? Can you drive right and forward at the same time?

Hi.

Regarding my syntax, I just did that to simplify how the code looked to me. I know it can get confusing because you can't see right away where the curly brackets are, so I'll look at changing that.

Also, I don't really understand what you mean by sending know values... Are you suggesting I send fake accelerometer values from the computer over serial and have the robot use those values?
I know for a fact that all of the accelerometer data from the wireless controller is being properly transmitted to the robot, because I have an LCD connected to the robot that displays X, Y, and Z values. I have also added some debug code to send to the computer the state of 'driveForward', 'driveBackward', 'driveRight', 'driveLeft'. It appears to set the proper values when I tilt the controller. For example, if I tilt forward and to the right, only 'driveForward' and 'driveRight' are 1 and all the rest are 0. If I tilt back and to the left, only 'driveBackward' and 'driveLeft' are 1. Here's my modified sections of the code to debug these values over the computer

void motorDriveDeterm()
{  
    if (mydata.xVal > forwardRangeMin && mydata.xVal < forwardRangeMax){ // If forward
      driveForward = true;
      driveBackward = false;}
    
    if (mydata.xVal > backwardRangeMin && mydata.xVal < backwardRangeMax){ // If backward
      driveForward = false;
      driveBackward = true;}
   
     if (mydata.yVal <= leftRangeMin && mydata.yVal >= rightRangeMax){ // If going straight
      driveRight = false;
      driveLeft = false;}
    
    if (mydata.yVal > rightRangeMin && mydata.yVal < rightRangeMax){ // If right
     driveRight = true;
     driveLeft = false;}
    
    if (mydata.yVal > leftRangeMin && mydata.yVal < leftRangeMax){ // If left
      driveRight = false;
      driveLeft = true;}  
      
    Serial.print("driveForward =");
    Serial.println(driveForward);
    Serial.print("driveBackward =");
    Serial.println(driveBackward);
    Serial.print("driveRight = ");
    Serial.println(driveRight);
    Serial.print("driveLeft = ");
    Serial.println(driveLeft);
}

(I also added a Serial.begin to the setup function to talk to the computer).

However, when I debug it this way, 'driveRight' and 'driveLeft' are NEVER false at the same time, although they should be if one of those 'if' statements are true. I don't know why.

And, Yes, the robot can go forward and right at the same time. The robot has 7 modes of driving:

Forward-Only: Both motors spin at the same speed FORWARDS in direct relation to forward tilt

Forward-Right: LEFT motor spins at a speed in direct relation to forward tilt, RIGHT motor spins at a speed in inverse relation to right tilt but proportional to forward tilt/left motor speed

Forward-Left: RIGHT motor spins at a speed in direct relation to forward tilt, LEFT motor spins at a speed in inverse relation to left tilt but proportional to forward tilt/right motor speed

Backward-Only: Both motors spin at the same speed BACKWARDS in direct relation to backward tilt

Backward-Right: LEFT motor spins at a speed in direct relation to backward tilt, RIGHT motor spins at a speed in inverse relation to right tilt but proportional to backward tilt/left motor speed

Backward-Left: RIGHT motor spins at a speed in direct relation to backward tilt, LEFT motor spins at a speed in inverse relation to left tilt but proportional to backward tilt/right motor speed

So, basically, it is like this: When turning/tilting to one side, one motor spins slower than the other. The further you tilt to that side, the slower that motor spins in relation to the other motor. In effect, you turn more. But the farther you tilt forward, the faster both motors spin, but one is still spinning slower.
If you are not turning, the tilt/Y values is not taken into consideration. Both motors simply go faster the farther you tilt.

As for the way the code works: First, the microcontroller simply determines in which direction we are going, and sets the 'driveForward', 'driveBackward', 'driveRight', and 'driveLeft' booleans TRUE or FALSE. Then, we read these variables. If we are going forward, the motor pins are set to spin forward, same for backward. This is the ONLY place in the entire code where motor direction is set, hardware-wise. When setting the speed later on, we don't care about direction.
Next, we set both motors to spin at a speed based on X tilt. This speed is called 'xPWM'.
Next, if we are turning, one motor is set to a speed based on Y tilt. This speed is called 'yPWM'. The other motor is left alone, spinning at the same speed we determined in the previous step.

NOW, (wow, that was a lot, thanks for reading). See my 'motorTurn()' function?

void motorTurn()
{
  if (driveRight == true && driveLeft == false){ // If turning to the right
    constYVal = constrain(mydata.yVal, rightRangeMin, rightRangeMax); // Constrain values that will be converted
    int yPWM = (xPWMRef - (pwmTurnRange * ((rightRangeMax - constYVal)/rightRange) ) ); // Convert amount of tilt to PWM values
    analogWrite(motorAPWM, yPWM); // Right motor will spin slower
    analogWrite(motorBPWM, xPWM);} // Left motor will spin regular speed
    
    
  if (driveRight == false && driveLeft == true){ // If turning to the left
    constYVal = constrain(mydata.yVal, leftRangeMin, leftRangeMax);
    int yPWM = (xPWMRef - (pwmTurnRange * (constYVal/leftRange) ) );
    analogWrite(motorAPWM, xPWM); // Right motor will spin regular speed
    analogWrite(motorBPWM, yPWM);} // Left motor will spin slower
}

Right now, I'm not actually calling this function. Right now, only forward and backward tilt works perfectly.
Any implementation of this code results in no motor operation. The motors simply don't spin at all. Here's how I've been trying to implement this code:

void loop()
{
  // * RUN IF SERIAL DATA FROM XBEE RECEIVED * //
  if(ET.receiveData())
  {
    lcdDisplay();
    if ((mydata.xVal < forwardRangeMax && mydata.xVal > backwardRangeMin) || (mydata.yVal < rightRangeMax && mydata.yVal > leftRangeMin)){ // If enough accelerometer movement detected...
    motorDriveDeterm();
    motorDrive();
    
    motorTurn(); // NEW IMPLEMENTATION, DOESN'T WORK!!!
  }
    else{
      motorBrake();
    }      
  }
  delay(50); // Needed by ET library
}

I've also tried to add the contents of the 'motorTurn()' function directly to the 'motorDrive()' function, but still yields the same result. Any ideas? :frowning:

Anybody have any ideas or help?