Simple assignment operator not working! [SOLVED]

Projects Programming
1

I am having the weirdest problem with a simple assignment operator. I have a robot with 2 sonars and 2 rotary encoders for the wheels. the arduino monitors an RC receiver, does some calculations to get the angles and throttle, and calculates its heading based on the distance the wheels have traveled. As the loop() runs, it updates its heading by adding to or subtracting from the variable 'heading'. The angle that is going to be added to heading is 'temp4'. However something very strange happens when i take the //comment from the line '//heading = heading + temp4'. 'heading' seems to become a null variable or something, and the 'if' comparisons that check the heading against the RC angle stop working entirely. these 'if' checks are directly after the 'heading = heading + temp4'. the wheels are told to move forward at the speed of 'chthrottle'. but if the 'rca' is greater or less than 'heading' it tells the opposite wheel to reverse. Only, the one simple line 'heading = heading + temp4' does something, and the wheels only go forward. all the variables involved are of type float, and i believe are initialized properly. is the 'heading' variable off limits or something? I tried different variables and still same problem. I tried monitoring 'heading' with the serial.print, and heading always = 0.00. i tested a simple line like 'heading = heading + 1.0' and that worked. And I monitored 'temp4', it was reading correct numbers but as sson as i try to add 'heading' and 'temp4' I get issues. What am I missing??? Thanks in advance to anyone who can help!

#include <Servo.h> 
#include "math.h"

Servo wheelL;
Servo wheelR;

const int ch1Pin = 8;//RC Receiver Ch 1
const int ch2Pin = 9;//RC Receiver Ch 2
const int encpinL = 28;//Left motor encoder
const int encpinR = 29;//Right motor encoder
const int pwPinL = 7;//Left sonar
const int pwPinR = 6;//Right sonar
const int rxPinL = 30;//rx pin on Left sonar
const int rxPinR = 31;//rx pin on Right sonar

int thrinc, thrdec;
int ch1, ch1max, ch1min, ch1mid, ch1range;
int ch2, ch2max, ch2min, ch2mid, ch2range;
int inchesL, inchesR;
unsigned long pulseL, pulseR;
float mphL, mphR;
float wheelLpwm, wheelRpwm, wheelLpwmset, wheelRpwmset, wheelLmax, wheelRmax, wheeltemp;
int toggle1, toggle2, toggle3, toggle4, toggle5, toggle6, togglemain;//Toggles for activating different parts of the main program.
unsigned long enctickL, enctickR, timer, timer1, timer2;
float pi = 3.141592;
float temp1, temp2, temp3, temp4, heading, chthrottle, mpl, iplL, iplR;
float rca, rcx, rcy, posx, posy;

void setup() {

  wheelL.attach(2);
  wheelR.attach(3);
  wheelLpwmset = 91.0; wheelRpwmset = 91.0; wheelLmax = 1.0; wheelRmax = 1.0;
  
  thrinc = 2; ;thrdec = 2;//Throttle increase and decrease speeds
  pinMode(ch1Pin, INPUT);
  pinMode(ch2Pin, INPUT);
  pinMode(encpinL, INPUT);
  pinMode(encpinR, INPUT);
  pinMode(pwPinL, INPUT);
  pinMode(pwPinR, INPUT);
  pinMode(rxPinL, OUTPUT);
  pinMode(rxPinR, OUTPUT);
  Serial.begin(9600);  delay(100);
  
  //Set receiver max and min ranges
  ch1max = 2050; ch1mid = 1450; ch1min = 850; ch2max = 2080; ch2mid = 1480; ch2min = 880;
  ch1range = ch1max - ch1min; ch2range = ch2max - ch2min;
  heading = 0.0; temp4 = 0.0;
  toggle1 = 0; toggle2 = 0; toggle3 = 1; toggle4 = 1; toggle5 = 0; toggle6 = 0; togglemain = 0;
}

void loop() {
  
  //Turn on sonar
  if (toggle1 == 0) {//Sonar Sequence 1
    digitalWrite(rxPinL, HIGH);//Turn on Left sonar
    toggle1 = 1; toggle2 = 0;
  }
  if (toggle3 == 0) {//Sonar Sequence 3
    digitalWrite(rxPinR, HIGH);//Turn on Right sonar
    toggle3 = 1; toggle4 = 0;
  }
  //End turn on sonar
  
  //Begin motor encoder readings, keep close to motor.write command to stay accurate
  enctickL = pulseIn(encpinL, HIGH, 40000);
  enctickR = pulseIn(encpinR, HIGH, 40000);
  timer2 = timer1; timer1 = millis();
  mpl = (float)timer1 - (float)timer2;//temp3 = time in milliseconds between encoder readings
  temp1 = 1.0 / (float)enctickL * 1000.0;//temp1 = ticks per millisecond
  temp2 = 1.0 / (float)enctickR * 1000.0;
  temp1 = temp1 * 8125.0 / 84.0;//temp1 = inches per second
  temp2 = temp2 * 8125.0 / 84.0;
  mphL = temp1 * 5.0 / 88.0;//convert inches per second to MPH
  mphR = temp2 * 5.0 / 88.0;
  iplL = temp1 / 1000.0 * mpl;//iplL = inches per loop
  iplR = temp2 / 1000.0 * mpl;
  if (wheelLpwmset < 91.0) iplL = 0.0 - iplL;//make iplL negative if in reverse
  if (wheelRpwmset < 91.0) iplR = 0.0 - iplR;
  //End motor encoder readings
  
  //Begin Sonar Readings
  if (millis() - timer >= 40 && toggle2 == 0) {//Sonar Sequence 2
    pulseL = pulseIn(pwPinL, HIGH, 50000);
    inchesL = pulseL/147;//147uS per inch
    digitalWrite(rxPinL, LOW);//Turn off Left sonar
    toggle2 = 1; toggle3 = 0;
  }
  if (millis() - timer >= 40 && toggle4 == 0) {//Sonar Sequence 4
    pulseR = pulseIn(pwPinR, HIGH, 50000);
    inchesR = pulseR/147;
    digitalWrite(rxPinR, LOW);//Turn off Right sonar
    toggle4 = 1; toggle1 = 0; togglemain = 1;
  }
  //End Sonar Readings
  
  
  //Begin RC receiver code
  ch1 = pulseIn(ch1Pin, HIGH, 40000);
  ch2 = pulseIn(ch2Pin, HIGH, 40000);
    
  if (ch1 > 2200 || ch1 < 700) ch1 = ch1mid;//Set to mid if RC too far out of range
  if (ch2 > 2200 || ch2 < 700) ch2 = ch2mid;
  if (ch1 > ch1mid - 60 && ch1 < ch1mid + 60) ch1 = ch1mid;//Set deadzones for RC
  if (ch2 > ch2mid - 60 && ch2 < ch2mid + 60) ch2 = ch2mid;
  //End RC receiver code
  
  //Send commands to motors
  rcx = ch1 - ch1mid;
  rcx = rcx / ch1range * 2;
  rcy = ch2 - ch2mid;
  rcy = rcy / ch2range * 2;
  //rcx and rcy range from -1.0 to 1.0
  
  chthrottle = sq(rcx) + sq(rcy);
  chthrottle = sqrt(chthrottle) / sqrt(2);
  //chthrottle ranges from 0.0 to 1.0 depening on joystick position
  
  rca = atan2(rcy, rcx) - pi / 2.0;//convert receiver into angles for use in heading
  if (rca < 0.0 - pi) rca = rca + 2 * pi;
  
  //calculate heading based on distance wheels have traveled
  //Angle changed = (2(iplR-iplL)/39)
  temp4 = iplR - iplL;
  temp4 = temp4 / 19.5;
  //heading = heading + temp4;
  
  //apply wheel direction based on rca angle and heading
  wheelLpwm = chthrottle;
  wheelRpwm = chthrottle;
  if (rca >= heading + 0.01) wheelLpwm = 0.0 - chthrottle;
  if (rca <= heading - 0.01) wheelRpwm = 0.0 - chthrottle;
  
  //Begin motor adjustments according to sonars
  if (inchesR < 95) {
    wheelLmax = (float)inchesR - 5.0;
    wheelLmax = wheelLmax / 180.0;
    wheelLmax = constrain(wheelLmax, 0.0, 1.0);
    wheelLpwm = constrain(wheelLpwm, 0.0 - wheelLmax, wheelLmax);
  }
  if (inchesL < 95) {
    wheelRmax = (float)inchesL - 5.0;
    wheelRmax = wheelRmax / 180.0;
    wheelRmax = constrain(wheelRmax, 0.0, 1.0);
    wheelRpwm = constrain(wheelRpwm, 0.0 - wheelRmax, wheelRmax);
  }
  if (wheelLpwm > 0.0 && inchesL < 10) {
    wheelLpwm = 0.0; wheelLpwmset = 91;
  }
  if (wheelRpwm > 0.0 && inchesR < 10) {
    wheelRpwm = 0.0; wheelRpwmset = 91;
  }
  //End motor sonar adjustments
  
  wheelLpwm = wheelLpwm * 89 + 91;//convert -1 to 1 range to 2 to 180
  wheelRpwm = wheelRpwm * 89 + 91;
    
  
  //Throttle wheels to prevent sudden increase or decrease
  if (wheelLpwm > wheelLpwmset) wheelLpwmset = wheelLpwmset + 0.5;//Set throttle of wheels
  if (wheelLpwm < wheelLpwmset) wheelLpwmset = wheelLpwmset - 0.5;
  if (wheelRpwm > wheelRpwmset) wheelRpwmset = wheelRpwmset + 0.5;
  if (wheelRpwm < wheelRpwmset) wheelRpwmset = wheelRpwmset - 0.5;
  if (wheelLpwm > wheelLpwmset + thrinc) wheelLpwmset = wheelLpwmset + thrinc;//Set throttle of wheels
  if (wheelLpwm < wheelLpwmset - thrdec) wheelLpwmset = wheelLpwmset - thrdec;
  if (wheelRpwm > wheelRpwmset + thrinc) wheelRpwmset = wheelRpwmset + thrinc;
  if (wheelRpwm < wheelRpwmset - thrdec) wheelRpwmset = wheelRpwmset - thrdec;
  //End throttle
    
  wheelL.write(wheelLpwmset);
  wheelR.write(wheelRpwmset);
  //End motor commands
  
  
  //check if full loop is expired
  if (togglemain == 1) {
    Serial.println();
    //Serial.print("Milliseconds per full cycle:");
    //Serial.print(millis() - timer);
    //Serial.println();
    Serial.print("Milliseconds per loop:");
    Serial.print(mpl);
    Serial.println();
    Serial.print("Left Encoder:");
    Serial.print(enctickL); Serial.println();
    Serial.print("Right Encoder:");
    Serial.print(enctickR); Serial.println();
    
    Serial.print("L Wheel"); Serial.println();
    Serial.print(wheelLpwm); Serial.println();
    Serial.print("R Wheel"); Serial.println();
    Serial.print(wheelRpwm); Serial.println();
    Serial.print("Left wheel MPH:");Serial.print(mphL);Serial.println();
    Serial.print("Right wheel MPH:");Serial.print(mphR);Serial.println();
    Serial.print("Left wheel distance per loop:");Serial.print(iplL);Serial.println();
    Serial.print("Right wheel distance per loop:");Serial.print(iplR);Serial.println();
    Serial.print("chthrottle:");Serial.print(chthrottle);Serial.println();
    Serial.print("RC Angle:");Serial.print(rca);Serial.println();
    Serial.print("temp4:");Serial.print(temp4);Serial.println();
    Serial.print("Heading:");Serial.print(heading);Serial.println();
    
    timer = millis();
    togglemain = 0;
  }

}
2

I can only comment that it looks to me like your code is about twice as big as it needs to be (never give bugs dusty corners to hide in) and that your debug prints are a bit too verbose.
Maybe you could print some of your debug output.

Paragraph breaks I like too.

3

The angle that is going to be added to heading is 'temp4'

This is IMHO a clear sign that variable naming needs some improvement.

Also, my impression is that there's too much code duplication (i.e. copy-n-paste) and some refactoring would improve its "debuggability".

4

As would sticking to one statement per line.

5

Hi,
I see that the heading variable is initialized to 0.0 in the setup() function and then updated in the program loop only in the line:

heading = heading + temp4;

I would constrain its value to some range (a heading probably is between 0.0 and 360.0)

6

Which sort of Arduino is this running on?

Pete

7

heading was already initialised to 0.0 before setup even ran, and the toogle variables were all set to zero.
Knowing this saves a lot of typing.

8

Yes right now the code is kind of large and clunky, and verbose. For me it helps keep things organized mentally. I was pretty sure that initializing the 'heading' variable already sets it to 0.0, but I added the 'heading = 0.0' in my efforts to eliminate possibilities of why this was happening. My issue is that the 'heading' variable refuses to be anything but 0.0 even when the only time its affected is with the line "heading = heading + temp4' and I monitor 'temp4' and i get normal readings like 0.25 or -0.3. The 'temp' variables are just names for temporary calculation variables. As for code duplication, I'll optimize things once I get a working beta. The darn thing needs to work properly first! Any ideas why 'heading' and 'temp4' wont add?

9

As for code duplication, I'll optimize things once I get a working beta.

Reducing code duplication has nothing to do with "optimization", but will help you "get a working beta."
This IMHO, of course.

10

Tell us what the values are before you add them

11

well there is no code that attempts to set heading
so it really shouldn't change!

12

There is on line in there that looks like this:
//heading = heading + temp4;
I have it commented for now, but when its out of comment, heading should increase or decrease based on 'temp4'
'heading' starts at 0.0, which is North for now (I'm adding a compass for calibrating later).
The scenario is, if the left wheel moves forward 10 inches, and the right wheel moves forward 20 inches, by the calculations, 'temp4' will be the angle change, about 0.51. Its all measured in radians. so using the above line, 'heading = heading + temp4', 'heading' will then be 0.51. if the next angle change is 0.3, 'heading' will then change to 0.81. But my debugging prints show heading=0.0, and temp4=0.51. Furthermore, the wheels only move forward when I remove the comments, the if lines directly below seem to stop working. Can I not add two variables together like this?

How can I reduce code duplication when I have 2 sonars that each need their own variables? and 2 encoders, and 2 RC receiver channels? There are subtle differences in the 'duplicates'. I'm open to suggestion, can you give me an example?

I'm using an Arduino Mega 2560.

13

if (rca >= heading + 0.01) wheelLpwm = 0.0 - chthrottle;
if (rca <= heading - 0.01) wheelRpwm = 0.0 - chthrottle;

These two lines check 'heading' against the angle that the joystick is pointing. If i remove the 'heading' variable, the wheels turn in the direction they should. Even though my debugging shows that heading=0.0, for some reason these if statements don't switch the wheel into reverse with 'heading' in it. Why is this? All variables are float.

14

Xenophage:
How can I reduce code duplication when I have 2 sonars that each need their own variables? and 2 encoders, and 2 RC receiver channels? There are subtle differences in the 'duplicates'. I'm open to suggestion, can you give me an example?

For trivial things that need to be treated in the same way (pin numbers and so on) you can put the associated variables in arrays and just do 'thing' to each element in the array.

For anything non-trivial, such as managing a sensor which has state, or a motor which has related information about position, distance, speed etc, use classes to encapsulate the logic and data need for one thing, and then create as many instances of that class as necessary.

15

I'll ask the question again; what are the values before you add them?
You still haven't shown any of your debug output.

(You could also save a lot of typing by learning the difference between "print" and "println")

16

Oh a debug report! Here is a sample after pointing the joystick in the full northwest position. 'temp4' is getting a proper calculated number, but it refuses to be passed on to 'heading'. I have tried changing the names of the variables involved, and still get the same issue. This sample is taken with the comments taken out from in front of the 'heading = heading + temp4' line of course. Like I said, if I change the line to 'heading = heading + 1.0', it works, and 'heading' is updated. Is temp4 too small a number to add? is the math being done behind the scenes not able to pass 0.018... to 'heading'??? why is it that all the other simple variable additions work fine but this one doesn't?

I noticed that I could combine 'print' and 'println', I'll definitely read up more on it to remove the excess.

Milliseconds per loop:106.00
Left Encoder:3626
Right Encoder:4166
L Wheel
106.82
R Wheel
106.33
Left wheel MPH:1.52
Right wheel MPH:1.32
Left wheel distance per loop:2.83
Right wheel distance per loop:2.46
chthrottle:0.64
RC Angle:0.837
temp4:0.01879584
Heading:0.00000000

17

It might reveal something if you printed the variables near where you actually use them/see a problem. Perhaps temp4 is not what you think it is when you add it to heading.

18

Any ideas why 'heading' and 'temp4' wont add?

You won't get very far until you quit focusing on that darn two-floats addition and start improving your code with the suggestions received in this thread.
My two cents:

  • temp4 needs a better name. Period.
  • use functions instead of copy-n-pasting code. Period.
    Now my pocket is empty.
19

I renamed 'temp4' to 'angleinc'. I said before, I tried renaming all the variables involved with this, and still got the same problem.

I tried printing 'heading' and 'temp4', before the 'heading heading + temp4', and IMMEDIATELY after it. 'temp4' = 0.0187... or some small decimal number, and 'heading' always equaled 0.0! 'heading' only changes if I use an actual number in place of 'temp4'. These floating variables have to add together, or my robot can't update its heading and navigate.

I will go through my code and try to reduce the copy and paste, but to be honest none of the rest of the code should be affecting 'heading' or 'temp4'. I've done a "find" search and they aren't located anywhere else.

I am using the include math.h. Would that affect it???

20

Xenophage:
I renamed 'temp4' to 'angleinc'. I said before, I tried renaming all the variables involved with this, and still got the same problem.

I tried printing 'heading' and 'temp4', before the 'heading heading + temp4', and IMMEDIATELY after it. 'temp4' = 0.0187... or some small decimal number, and 'heading' always equaled 0.0!

In that case I suggest you post the actual code that incorporates these suggestions, and the trace output from that code showing the values immediately before and after the assignment.