Landing : Athabascau University
  • Blogs
  • Troubleshooting Force Sensors

Troubleshooting Force Sensors

Troubleshooting Force Sensors

My force sensors aren't working properly. It's been a while since I set them up and the system isn't simple, so I need to refresh myself as I dig into it. This blog post follows along as I attempt to fix the issue.

What's the problem?

image

The sensors are supposed to spit out values between 0 and 255 (8-bits), corresponding to high and low force values, but, without any load on them, one outputs 0 and the other 255. At least one of them is way off, since they're supposed to be about equal. Since they are setup in exactly the same way, I have to assume that there is a problem with both of them.

They are mounted onto the end-effector gripper, reporting the gripping force, with an elastic providing plenty of static force for testing.

How do you fix it?

KISS: "Keep It Simple, Stupid" covers my troubleshooting philosophy. This means checking the easiest, most obvious possibilities first.

1. Wiring Errors

Did a wire pop out, or is it wired incorrectly? To start, I drew a simplified circuit diagram then measured resitance between the various nodes. Something was wrong: the two power pins were routed to the two contacts on just one of the sensors, and the two ADC pins were on the other. (A direct connection should be less than 1 Ohm.) Swapping one power and ADC connector on my wire connection board fixed this up.

image

Did it work, now? A quick test said "no"! They still reported wildly different values. Other issues were afoot.

2. Programming Errors

I make a lot of programming errors. To make up for it I am also diligent about testing and including thorough debugging capabilities. These sensors get their own class called ForceSensor, the relevant functions being raw() and read(). The former reports the value given by a single analogRead(pin); the latter averages many readings and reports it back as an 8-bit value (uint8_t, or byte). With a light The debug lines reported the following, where the number outside the parentheses is that returned by read():

Force sensors configured: [left] 28 (raw: 128), [right] 255 (raw: 549)

The '[right]' sensor is in error, obvious to me because read() is supposed to filter out those values outside of 10 to 245. The raw() values are about right, comparing them to my measurements with a meter. Again, since they are setup in exactly the same way, I have to assume that there is a problem with both of them.

The first error I found when reviewing my code was the logic to filter out those values above the maximum assumed 8-bit values when it was getting 10-bit values. I changed the maximum from 245 to 982 -- both numbers correspond to about 200mV below the maximum (255 or 1023). The report changed to this:

Force sensors configured: [left] 255 (raw: 905), [right] 255 (raw: 953)

Well, that's better -- at least they're reporting about the same thing, now. read()is still wonky, reporting the maximum of 255. What's going on there? I'll dig a bit into that. This function turns on power to the sensors if it's off, calls the private function filteredMeasure(), then turns it off if it was like that beforehand. filteredMeasure() calls analogRead(pin) and sums the values if they are within the allowed range. At the moment, it does this 50 times and uses the range 10 to 982, which was supposed to correspond to about 200mV above ground to 200mV below source voltage. The 10 is an error, corresponding to 50mV above ground, but I don't think that's the issue, here. Regardless, I changed the range to 50mV above and below, respectively (10 to 1013).

Here are the debug macros I used in ForceSensor.cpp:

#define ForceSensor_DEBUG_MODE
#ifdef ForceSensor_DEBUG_MODE
# define PRE Serial.print(F("ForceSensor: "))
# define POST delay(5)
# define BREAK Serial.println()
# define DEBUG(x) (x); POST
# define DEBUGLN(x) PRE; (x); BREAK; POST
#else
# define PRE
# define POST
# define BREAK
# define DEBUG(x)
# define DEBUGLN(x)
#endif // ForceSensor_DEBUG_MODE

Here's the debug block in ForceSensor::filteredMeasure(), looking specifically at the output formula return (uint8_t)((sum + count / 2) / count);:

 ...
BREAK; PRE;
DEBUG(Serial.print(F("filteredMeasure(): count=")));
DEBUG(Serial.print(count));
DEBUG(Serial.print(F(", sum=")));
DEBUG(Serial.print(sum));
DEBUG(Serial.print(F(", timeout? ")));
DEBUG(Serial.print((millis() < timeout) ? (F("no")) : (F("yes"))));
BREAK; POST;
 // convert from 10-bit analogRead to 8-bit value (1023 to 254)
count *= 4; // 1023/254 = 4.03, or about 4

DEBUGLN(Serial.print(F("(uint8_t)((sum+count*4/2)/count*4)=")));
PRE;
DEBUG(Serial.print(F("(uint8_t)((sum+")));
DEBUG(Serial.print(count));
DEBUG(Serial.print(F("/2)/")));
DEBUG(Serial.print(count));
DEBUG(Serial.print(F(")=")));
BREAK; POST; PRE;
DEBUG(Serial.print(F("(uint8_t)((")));
DEBUG(Serial.print(sum));
DEBUG(Serial.print(F("+")));
DEBUG(Serial.print(count / 2));
DEBUG(Serial.print(F(")/")));
DEBUG(Serial.print(count));
DEBUG(Serial.print(F(")=")));
BREAK; POST; PRE;
DEBUG(Serial.print(F("(uint8_t)(")));
DEBUG(Serial.print(sum + count / 2));
DEBUG(Serial.print(F(")/")));
DEBUG(Serial.print(count));
DEBUG(Serial.print(F(")=")));
BREAK; POST; PRE;
DEBUG(Serial.print(F("(uint8_t)(")));
DEBUG(Serial.print((sum + count / 2) / count));
DEBUG(Serial.print(F(")=")));
BREAK; POST;
DEBUGLN(Serial.print((uint8_t)((sum + count / 2) / count)));
PRE;
DEBUG(Serial.print(F("returning ")));
DEBUG(Serial.print((count == 0) ? (0) : ((uint8_t)((sum + count / 2) / count))));
BREAK; POST;
return (uint8_t)((sum + count / 2) / count); // rounds to nearest int
}
...

Serial output:

Opening port
Port open
VMDPV_1|1_�MDPV
AutoMove.cpp compiled Dec 7 2017 at 09:58:22
AutoMove : Servos powered on.
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
SD card malfunction.
Force sensors configured: [left]
ForceSensor: filteredMeasure(): count=0, sum=0, timeout? yes
ForceSensor: (uint8_t)((sum+count*4/2)/count*4)=
ForceSensor: (uint8_t)((sum+0/2)/0)=
ForceSensor: (uint8_t)((0+0)/0)=
ForceSensor: (uint8_t)(0)/0)=
ForceSensor: (uint8_t)(65535)=
ForceSensor: 255
0 (raw: 906), [right]
ForceSensor: filteredMeasure(): count=0, sum=0, timeout? yes
ForceSensor: (uint8_t)((sum+count*4/2)/count*4)=
ForceSensor: (uint8_t)((sum+0/2)/0)=
ForceSensor: (uint8_t)((0+0)/0)=
ForceSensor: (uint8_t)(0)/0)=
ForceSensor: (uint8_t)(65535)=
ForceSensor: 255
0 (raw: 955)
[#] [name] : [servo angle] ([physical angle])
[1] boom1 : 86 (135)
[2] boom2 : 84 (-41)
[3] turret : 2 (1)
[4] claw : 93 (73)
Current position: (3,161,1)
Enter height:
Port closed

Well, would you look at that -- there's a divide-by-zero error, fixed with a bit of logic. It's also saying that the filter rejected all of the values recorded before the sum loop timed out. Maybe it needs more time? I'll increase the timeout value from 100ms to 5000ms, just to see what happens and I can keep up with it with my sub-par human reaction times.

It didn't make a difference, ruling out too-short timeout values. There's something else going on. I'll change the timeout back to 100ms. At this point it's time to debug the summing loop itself. I'll change the number of points to sum from 50 to 10 to limit the number of debug lines, too.

 

 ...
do {
dat = analogRead(adcPin);
 
 BREAK; PRE;
DEBUG(Serial.print(F("filteredMeasure() do-while: dat=")));
DEBUG(Serial.print(dat));
DEBUG(Serial.print(F(", (dat > minReading && dat < maxReading)=(")));
DEBUG(Serial.print((dat > minReading && dat < maxReading) ? (F("TRUE, sum=")) : F("FALSE, sum=")));
 
 if (dat > minReading && dat < maxReading)
{
sum += dat;
count++;
}
 
 DEBUG(Serial.print(sum));
DEBUG(Serial.print(F(", count=")));
DEBUG(Serial.print(count));
POST; BREAK;
 
} while (count < FILTERED_READ_POINTS && millis() < timeout);
...

Serial output:

Opening port
Port open
VMDPV_1|1_�MDPV
AutoMove.cpp compiled Dec 7 2017 at 10:16:05
AutoMove : Servos powered on.
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
SD card malfunction.
Force sensors configured: [left]
ForceSensor: filteredMeasure() do-while: dat=908, (dat > minReading && dat < maxReading)=FALSE, sum=0, count=0
ForceSensor: filteredMeasure(): count=0, sum=0, timeout? yes
ForceSensor: (uint8_t)((sum+count*4/2)/count*4)=
ForceSensor: (uint8_t)((sum+0/2)/0)=
ForceSensor: (uint8_t)((0+0)/0)=
ForceSensor: (uint8_t)(0)/0)=
ForceSensor: (uint8_t)(65535)=
ForceSensor: 255
0 (raw: 908), [right]
ForceSensor: filteredMeasure() do-while: dat=955, (dat > minReading && dat < maxReading)=FALSE, sum=0, count=0
ForceSensor: filteredMeasure(): count=0, sum=0, timeout? yes
ForceSensor: (uint8_t)((sum+count*4/2)/count*4)=
ForceSensor: (uint8_t)((sum+0/2)/0)=
ForceSensor: (uint8_t)((0+0)/0)=
ForceSensor: (uint8_t)(0)/0)=
ForceSensor: (uint8_t)(65535)=
ForceSensor: 255
0 (raw: 955)
[#] [name] : [servo angle] ([physical angle])
[1] boom1 : 86 (135)
[2] boom2 : 84 (-41)
[3] turret : 2 (1)
[4] claw : 93 (73)
Current position: (3,161,1)
Enter height:

Hrmm, the loop seems to have timed out after only a single iteration, and the filter logic doesn't seem to have worked. I'll take a closer look at the filter logic with some more debug lines:

...
do {
dat = analogRead(adcPin);
BREAK; PRE;
DEBUG(Serial.print(F("filteredMeasure() do-while: dat=")));
DEBUG(Serial.print(dat));
DEBUG(Serial.print(F(", (dat > minReading && dat < maxReading)=(")));
DEBUG(Serial.print((dat > minReading) ? (F("TRUE && ")) : (F("FALSE && "))));
DEBUG(Serial.print((dat < maxReading) ? (F("TRUE)=")) : (F("FALSE)="))));
DEBUG(Serial.print((dat > minReading && dat < maxReading) ? (F("TRUE, sum=")) : F("FALSE, sum=")));
if (dat > minReading && dat < maxReading)
{
sum += dat;
count++;
}
DEBUG(Serial.print(sum));
DEBUG(Serial.print(F(", count=")));
DEBUG(Serial.print(count));
POST; BREAK;
} while (count < FILTERED_READ_POINTS && millis() < timeout);
...

Serial output:

Opening port
Port open
VMDPV_1|1_�MDPV
AutoMove.cpp compiled Dec 7 2017 at 10:41:05
AutoMove : Servos powered on.
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
SD card malfunction.
Force sensors configured: [left]
ForceSensor: filteredMeasure() do-while: dat=915, (dat > minReading && dat < maxReading)=(TRUE && FALSE)=FALSE, sum=0, count=0
ForceSensor: filteredMeasure(): count=0, sum=0, timeout? yes
ForceSensor: (uint8_t)((sum+count*4/2)/count*4)=
ForceSensor: (uint8_t)((sum+0/2)/0)=
ForceSensor: (uint8_t)((0+0)/0)=
ForceSensor: (uint8_t)(0)/0)=
ForceSensor: (uint8_t)(65535)=
ForceSensor: 255
ForceSensor: returning 0
0 (raw: 915), [right]
ForceSensor: filteredMeasure() do-while: dat=956, (dat > minReading && dat < maxReading)=(TRUE && FALSE)=FALSE, sum=0, count=0
ForceSensor: filteredMeasure(): count=0, sum=0, timeout? yes
ForceSensor: (uint8_t)((sum+count*4/2)/count*4)=
ForceSensor: (uint8_t)((sum+0/2)/0)=
ForceSensor: (uint8_t)((0+0)/0)=
ForceSensor: (uint8_t)(0)/0)=
ForceSensor: (uint8_t)(65535)=
ForceSensor: 255
ForceSensor: returning 0
0 (raw: 956)
[#] [name] : [servo angle] ([physical angle])
[1] boom1 : 86 (135)
[2] boom2 : 84 (-41)
[3] turret : 2 (1)
[4] claw : 93 (73)
Current position: (3,161,1)
Enter height:
Port closed

There it is: "dat < maxReading" registered as false. It should have been true. I'll add more debug lines to spit out minReading and maxReading.

...
DEBUG(Serial.print(F(", count=")));
DEBUG(Serial.print(count));
POST; BREAK; PRE;
DEBUG(Serial.print(F("minReading: ")));
DEBUG(Serial.print(minReading));
DEBUG(Serial.print(F(", maxReading: ")));
DEBUG(Serial.print(maxReading));
POST; BREAK;
...

Serial output:

Opening port
Port open
VMDPV_1|1_�MDPV
AutoMove.cpp compiled Dec 7 2017 at 10:45:49
AutoMove : Servos powered on.
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
SD card malfunction.
Force sensors configured: [left]
ForceSensor: filteredMeasure() do-while: dat=914, (dat > minReading && dat < maxReading)=(TRUE && FALSE)=FALSE, sum=0, count=0
ForceSensor: minReading: 10, maxReading: 245
ForceSensor: filteredMeasure(): count=0, sum=0, timeout? yes
ForceSensor: (uint8_t)((sum+count*4/2)/count*4)=
ForceSensor: (uint8_t)((sum+0/2)/0)=
ForceSensor: (uint8_t)((0+0)/0)=
ForceSensor: (uint8_t)(0)/0)=
ForceSensor: (uint8_t)(65535)=
ForceSensor: 255
ForceSensor: returning 0
0 (raw: 914), [right]
ForceSensor: filteredMeasure() do-while: dat=956, (dat > minReading && dat < maxReading)=(TRUE && FALSE)=FALSE, sum=0, count=0
ForceSensor: minReading: 10, maxReading: 245
ForceSensor: filteredMeasure(): count=0, sum=0, timeout? yes
ForceSensor: (uint8_t)((sum+count*4/2)/count*4)=
ForceSensor: (uint8_t)((sum+0/2)/0)=
ForceSensor: (uint8_t)((0+0)/0)=
ForceSensor: (uint8_t)(0)/0)=
ForceSensor: (uint8_t)(65535)=
ForceSensor: 255
ForceSensor: returning 0
0 (raw: 956)
[#] [name] : [servo angle] ([physical angle])
[1] boom1 : 86 (135)
[2] boom2 : 84 (-41)
[3] turret : 2 (1)
[4] claw : 93 (73)
Current position: (3,161,1)
Enter height:

Aha! maxReading is still at 245, though I thought I changed it to 1013. Looking at my initialization code in AutoMove.cpp, it is indeed at 1013. It must have been redefined or otherwise changed somewhere else... Found it: the datatype for maxReading is uint8_t, which isn't large enough to store 1013. I'll change it to uint16_t. Let's see what this does.

Serial output:

Opening port
Port open
VMDPV_1|1_�MDPV
AutoMove.cpp compiled Dec 7 2017 at 10:45:49
AutoMove : Servos powered on.
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
SD card malfunction.
Force sensors configured: [left]
ForceSensor: filteredMeasure() do-while: dat=914, (dat > minReading && dat < maxReading)=(TRUE && TRUE)=TRUE, sum=914, count=1
ForceSensor: minReading: 10, maxReading: 1013
ForceSensor: filteredMeasure(): count=1, sum=914, timeout? yes
ForceSensor: (uint8_t)((sum+count*4/2)/count*4)=
ForceSensor: (uint8_t)((sum+4/2)/4)=
ForceSensor: (uint8_t)((914+2)/4)=
ForceSensor: (uint8_t)(916)/4)=
ForceSensor: (uint8_t)(914)=
ForceSensor: 229
ForceSensor: returning 229
229 (raw: 914), [right]
ForceSensor: filteredMeasure() do-while: dat=956, (dat > minReading && dat < maxReading)=(TRUE && TRUE)=TRUE, sum=956, count=1
ForceSensor: minReading: 10, maxReading: 1013
ForceSensor: filteredMeasure(): count=1, sum=956, timeout? yes
ForceSensor: (uint8_t)((sum+count*4/2)/count*4)=
ForceSensor: (uint8_t)((sum+4/2)/4)=
ForceSensor: (uint8_t)((956+2)/4)=
ForceSensor: (uint8_t)(958)/4)=
ForceSensor: (uint8_t)(239)=
ForceSensor: 239
ForceSensor: returning 239
239 (raw: 956)
[#] [name] : [servo angle] ([physical angle])
[1] boom1 : 86 (135)
[2] boom2 : 84 (-41)
[3] turret : 2 (1)
[4] claw : 93 (73)
Current position: (3,161,1)
Enter height:

Well, the returned result is about right, but the do-while loop still only does a single iteration. It's supposed to do 10 at the moment. Time to check the loop exit logic with more debug lines... I'll bump the timeout back up to 5000ms, just in case my debugging slows it down too much.

...
do {
dat = analogRead(adcPin);
BREAK; PRE;
DEBUG(Serial.print(F("filteredMeasure() do-while: dat=")));
DEBUG(Serial.print(dat));
DEBUG(Serial.print(F(", (dat > minReading && dat < maxReading)=(")));
DEBUG(Serial.print((dat > minReading) ? (F("TRUE && ")) : (F("FALSE && "))));
DEBUG(Serial.print((dat < maxReading) ? (F("TRUE)=")) : (F("FALSE)="))));
DEBUG(Serial.print((dat > minReading && dat < maxReading) ? (F("TRUE, sum=")) : F("FALSE, sum=")));
if (dat > minReading && dat < maxReading)
{
sum += dat;
count++;
}
DEBUG(Serial.print(sum));
DEBUG(Serial.print(F(", count=")));
DEBUG(Serial.print(count));
POST; BREAK; PRE;
DEBUG(Serial.print(F("minReading: ")));
DEBUG(Serial.print(minReading));
DEBUG(Serial.print(F(", maxReading: ")));
DEBUG(Serial.print(maxReading));
POST; BREAK;

PRE;
DEBUG(Serial.print(F("(count < FILTERED_READ_POINTS && millis() < timeout)=(")));
DEBUG(Serial.print(count));
DEBUG(Serial.print(F(" < ")));
DEBUG(Serial.print(FILTERED_READ_POINTS));
DEBUG(Serial.print(F(" && ")));
DEBUG(Serial.print(millis()));
DEBUG(Serial.print(F(" < ")));
DEBUG(Serial.print(timeout));
DEBUG(Serial.print(F(")=(")));
DEBUG(Serial.print((count < FILTERED_READ_POINTS) ? (F("TRUE && ")) : (F("FALSE && "))));
DEBUG(Serial.print((millis() < timeout) ? (F("TRUE)=")) : (F("FALSE)="))));
DEBUG(Serial.print((count < FILTERED_READ_POINTS && millis() < timeout) ? (F("TRUE")) : (F("FALSE"))));
POST; BREAK;
} while (count < FILTERED_READ_POINTS && millis() < timeout);
...

Serial output:

Opening port
Port open
VMDPV_1|1_�MDPV
AutoMove.cpp compiled Dec 7 2017 at 10:59:47
AutoMove : Servos powered on.
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
SD card malfunction.
Force sensors configured: [left]
ForceSensor: filteredMeasure() do-while: dat=926, (dat > minReading && dat < maxReading)=(TRUE && TRUE)=TRUE, sum=926, count=1
ForceSensor: minReading: 10, maxReading: 1013
ForceSensor: (count < FILTERED_READ_POINTS && millis() < timeout)=(1 < 10 && 5615 < 10336)=(TRUE && TRUE)=TRUE
ForceSensor: filteredMeasure() do-while: dat=926, (dat > minReading && dat < maxReading)=(TRUE && TRUE)=TRUE, sum=1852, count=2
ForceSensor: minReading: 10, maxReading: 1013
ForceSensor: (count < FILTERED_READ_POINTS && millis() < timeout)=(2 < 10 && 5916 < 10336)=(TRUE && TRUE)=TRUE
ForceSensor: filteredMeasure() do-while: dat=926, (dat > minReading && dat < maxReading)=(TRUE && TRUE)=TRUE, sum=2778, count=3
ForceSensor: minReading: 10, maxReading: 1013
ForceSensor: (count < FILTERED_READ_POINTS && millis() < timeout)=(3 < 10 && 6218 < 10336)=(TRUE && TRUE)=TRUE
ForceSensor: filteredMeasure() do-while: dat=926, (dat > minReading && dat < maxReading)=(TRUE && TRUE)=TRUE, sum=3704, count=4
ForceSensor: minReading: 10, maxReading: 1013
ForceSensor: (count < FILTERED_READ_POINTS && millis() < timeout)=(4 < 10 && 6519 < 10336)=(TRUE && TRUE)=TRUE
ForceSensor: filteredMeasure() do-while: dat=926, (dat > minReading && dat < maxReading)=(TRUE && TRUE)=TRUE, sum=4630, count=5
ForceSensor: minReading: 10, maxReading: 1013
ForceSensor: (count < FILTERED_READ_POINTS && millis() < timeout)=(5 < 10 && 6821 < 10336)=(TRUE && TRUE)=TRUE
ForceSensor: filteredMeasure() do-while: dat=926, (dat > minReading && dat < maxReading)=(TRUE && TRUE)=TRUE, sum=5556, count=6
ForceSensor: minReading: 10, maxReading: 1013
ForceSensor: (count < FILTERED_READ_POINTS && millis() < timeout)=(6 < 10 && 7122 < 10336)=(TRUE && TRUE)=TRUE
ForceSensor: filteredMeasure() do-while: dat=926, (dat > minReading && dat < maxReading)=(TRUE && TRUE)=TRUE, sum=6482, count=7
ForceSensor: minReading: 10, maxReading: 1013
ForceSensor: (count < FILTERED_READ_POINTS && millis() < timeout)=(7 < 10 && 7425 < 10336)=(TRUE && TRUE)=TRUE
ForceSensor: filteredMeasure() do-while: dat=926, (dat > minReading && dat < maxReading)=(TRUE && TRUE)=TRUE, sum=7408, count=8
ForceSensor: minReading: 10, maxReading: 1013
ForceSensor: (count < FILTERED_READ_POINTS && millis() < timeout)=(8 < 10 && 7726 < 10336)=(TRUE && TRUE)=TRUE
ForceSensor: filteredMeasure() do-while: dat=926, (dat > minReading && dat < maxReading)=(TRUE && TRUE)=TRUE, sum=8334, count=9
ForceSensor: minReading: 10, maxReading: 1013
ForceSensor: (count < FILTERED_READ_POINTS && millis() < timeout)=(9 < 10 && 8028 < 10336)=(TRUE && TRUE)=TRUE
ForceSensor: filteredMeasure() do-while: dat=926, (dat > minReading && dat < maxReading)=(TRUE && TRUE)=TRUE, sum=9260, count=10
ForceSensor: minReading: 10, maxReading: 1013
ForceSensor: (count < FILTERED_READ_POINTS && millis() < timeout)=(10 < 10 && 8330 < 10336)=(FALSE && TRUE)=FALSE
ForceSensor: filteredMeasure(): count=10, sum=9260, timeout? no
ForceSensor: (uint8_t)((sum+count*4/2)/count*4)=
ForceSensor: (uint8_t)((sum+40/2)/40)=
ForceSensor: (uint8_t)((9260+20)/40)=
ForceSensor: (uint8_t)(9280)/40)=
ForceSensor: (uint8_t)(232)=
ForceSensor: 232
ForceSensor: returning 232
232 (raw: 926), [right]
ForceSensor: filteredMeasure() do-while: dat=959, (dat > minReading && dat < maxReading)=(TRUE && TRUE)=TRUE, sum=959, count=1
ForceSensor: minReading: 10, maxReading: 1013
ForceSensor: (count < FILTERED_READ_POINTS && millis() < timeout)=(1 < 10 && 8978 < 13699)=(TRUE && TRUE)=TRUE
ForceSensor: filteredMeasure() do-while: dat=959, (dat > minReading && dat < maxReading)=(TRUE && TRUE)=TRUE, sum=1918, count=2
ForceSensor: minReading: 10, maxReading: 1013
ForceSensor: (count < FILTERED_READ_POINTS && millis() < timeout)=(2 < 10 && 9280 < 13699)=(TRUE && TRUE)=TRUE
ForceSensor: filteredMeasure() do-while: dat=959, (dat > minReading && dat < maxReading)=(TRUE && TRUE)=TRUE, sum=2877, count=3
ForceSensor: minReading: 10, maxReading: 1013
ForceSensor: (count < FILTERED_READ_POINTS && millis() < timeout)=(3 < 10 && 9581 < 13699)=(TRUE && TRUE)=TRUE
ForceSensor: filteredMeasure() do-while: dat=959, (dat > minReading && dat < maxReading)=(TRUE && TRUE)=TRUE, sum=3836, count=4
ForceSensor: minReading: 10, maxReading: 1013
ForceSensor: (count < FILTERED_READ_POINTS && millis() < timeout)=(4 < 10 && 9883 < 13699)=(TRUE && TRUE)=TRUE
ForceSensor: filteredMeasure() do-while: dat=959, (dat > minReading && dat < maxReading)=(TRUE && TRUE)=TRUE, sum=4795, count=5
ForceSensor: minReading: 10, maxReading: 1013
ForceSensor: (count < FILTERED_READ_POINTS && millis() < timeout)=(5 < 10 && 10184 < 13699)=(TRUE && TRUE)=TRUE
ForceSensor: filteredMeasure() do-while: dat=959, (dat > minReading && dat < maxReading)=(TRUE && TRUE)=TRUE, sum=5754, count=6
ForceSensor: minReading: 10, maxReading: 1013
ForceSensor: (count < FILTERED_READ_POINTS && millis() < timeout)=(6 < 10 && 10487 < 13699)=(TRUE && TRUE)=TRUE
ForceSensor: filteredMeasure() do-while: dat=959, (dat > minReading && dat < maxReading)=(TRUE && TRUE)=TRUE, sum=6713, count=7
ForceSensor: minReading: 10, maxReading: 1013
ForceSensor: (count < FILTERED_READ_POINTS && millis() < timeout)=(7 < 10 && 10789 < 13699)=(TRUE && TRUE)=TRUE
ForceSensor: filteredMeasure() do-while: dat=959, (dat > minReading && dat < maxReading)=(TRUE && TRUE)=TRUE, sum=7672, count=8
ForceSensor: minReading: 10, maxReading: 1013
ForceSensor: (count < FILTERED_READ_POINTS && millis() < timeout)=(8 < 10 && 11092 < 13699)=(TRUE && TRUE)=TRUE
ForceSensor: filteredMeasure() do-while: dat=959, (dat > minReading && dat < maxReading)=(TRUE && TRUE)=TRUE, sum=8631, count=9
ForceSensor: minReading: 10, maxReading: 1013
ForceSensor: (count < FILTERED_READ_POINTS && millis() < timeout)=(9 < 10 && 11395 < 13699)=(TRUE && TRUE)=TRUE
ForceSensor: filteredMeasure() do-while: dat=958, (dat > minReading && dat < maxReading)=(TRUE && TRUE)=TRUE, sum=9589, count=10
ForceSensor: minReading: 10, maxReading: 1013
ForceSensor: (count < FILTERED_READ_POINTS && millis() < timeout)=(10 < 10 && 11699 < 13699)=(FALSE && TRUE)=FALSE
ForceSensor: filteredMeasure(): count=10, sum=9589, timeout? no
ForceSensor: (uint8_t)((sum+count*4/2)/count*4)=
ForceSensor: (uint8_t)((sum+40/2)/40)=
ForceSensor: (uint8_t)((9589+20)/40)=
ForceSensor: (uint8_t)(9609)/40)=
ForceSensor: (uint8_t)(240)=
ForceSensor: 240
ForceSensor: returning 240
240 (raw: 959)
[#] [name] : [servo angle] ([physical angle])
[1] boom1 : 86 (135)
[2] boom2 : 84 (-41)
[3] turret : 2 (1)
[4] claw : 93 (73)
Current position: (3,161,1)
Enter height:
Port closed

Well, what do you know, putting in that debug code tricked the loop into working properly. A bit of math shows that each loop took around 300ms, longer than the previous timeout value of 100ms. I'll try taking the debug lines out of the loop but keep the long timeout value (5000ms), and time the loop.

...
BREAK; PRE;
DEBUG(Serial.print(F("millis() before = ")));
DEBUG(Serial.print(millis()));
POST; BREAK;
do {
dat = analogRead(adcPin);
//BREAK; PRE;
//DEBUG(Serial.print(F("filteredMeasure() do-while: dat=")));
//DEBUG(Serial.print(dat));
//DEBUG(Serial.print(F(", (dat > minReading && dat < maxReading)=(")));
//DEBUG(Serial.print((dat > minReading) ? (F("TRUE && ")) : (F("FALSE && "))));
//DEBUG(Serial.print((dat < maxReading) ? (F("TRUE)=")) : (F("FALSE)="))));
//DEBUG(Serial.print((dat > minReading && dat < maxReading) ? (F("TRUE, sum=")) : F("FALSE, sum=")));
if (dat > minReading && dat < maxReading)
{
sum += dat;
count++;
}
//DEBUG(Serial.print(sum));
//DEBUG(Serial.print(F(", count=")));
//DEBUG(Serial.print(count));
//POST; BREAK; PRE;
//DEBUG(Serial.print(F("minReading: ")));
//DEBUG(Serial.print(minReading));
//DEBUG(Serial.print(F(", maxReading: ")));
//DEBUG(Serial.print(maxReading));
//POST; BREAK;
//
//PRE;
//DEBUG(Serial.print(F("(count < FILTERED_READ_POINTS && millis() < timeout)=(")));
//DEBUG(Serial.print(count));
//DEBUG(Serial.print(F(" < ")));
//DEBUG(Serial.print(FILTERED_READ_POINTS));
//DEBUG(Serial.print(F(" && ")));
//DEBUG(Serial.print(millis()));
//DEBUG(Serial.print(F(" < ")));
//DEBUG(Serial.print(timeout));
//DEBUG(Serial.print(F(")=(")));
//DEBUG(Serial.print((count < FILTERED_READ_POINTS) ? (F("TRUE && ")) : (F("FALSE && "))));
//DEBUG(Serial.print((millis() < timeout) ? (F("TRUE)=")) : (F("FALSE)="))));
//DEBUG(Serial.print((count < FILTERED_READ_POINTS && millis() < timeout) ? (F("TRUE")) : (F("FALSE"))));
//POST; BREAK;
} while (count < FILTERED_READ_POINTS && millis() < timeout);
BREAK; PRE;
DEBUG(Serial.print(F("millis() after = ")));
DEBUG(Serial.print(millis()));
POST; BREAK;
...

Serial output:

Opening port
Port open
VMDPV_1|1_�MDPV
AutoMove.cpp compiled Dec 7 2017 at 11:24:29
AutoMove : Servos powered on.
AutoMoveSD : startScript() failed at F_CPU/2
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
SD card malfunction.
Force sensors configured: [left]
ForceSensor: millis() before = 5408
ForceSensor: millis() after = 5449
ForceSensor: filteredMeasure(): count=10, sum=9230, timeout? no
ForceSensor: (uint8_t)((sum+count*4/2)/count*4)=
ForceSensor: (uint8_t)((sum+40/2)/40)=
ForceSensor: (uint8_t)((9230+20)/40)=
ForceSensor: (uint8_t)(9250)/40)=
ForceSensor: (uint8_t)(231)=
ForceSensor: 231
ForceSensor: returning 231
231 (raw: 923), [right]
ForceSensor: millis() before = 5834
ForceSensor: millis() after = 5874
ForceSensor: filteredMeasure(): count=10, sum=9590, timeout? no
ForceSensor: (uint8_t)((sum+count*4/2)/count*4)=
ForceSensor: (uint8_t)((sum+40/2)/40)=
ForceSensor: (uint8_t)((9590+20)/40)=
ForceSensor: (uint8_t)(9610)/40)=
ForceSensor: (uint8_t)(240)=
ForceSensor: 240
ForceSensor: returning 240
240 (raw: 959)
[#] [name] : [servo angle] ([physical angle])
[1] boom1 : 86 (135)
[2] boom2 : 84 (-41)
[3] turret : 2 (1)
[4] claw : 93 (73)
Current position: (3,161,1)
Enter height:
Enter height:
Enter height:

That one only took 40ms. Extrapolating to 50 points, the value I had it at before all of this mess, the loop will take about 200ms. I'll set the timeout to 250ms and bump the points back up to 50.

...
#define FILTERED_READ_POINTS 50
#define FILTERED_READ_TIMEOUT_MS 100
...

Serial output:

Port open
VMDPV_1|1_�MDPV
AutoMove.cpp compiled Dec 7 2017 at 11:35:00
AutoMove : Servos powered on.
AutoMoveSD : startScript() failed at F_CPU/2
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
SD card malfunction.
Force sensors configured: [left]
ForceSensor: millis() before = 5419
ForceSensor: millis() after = 5459
ForceSensor: filteredMeasure(): count=50, sum=46101, timeout? no
ForceSensor: (uint8_t)((sum+count*4/2)/count*4)=
ForceSensor: (uint8_t)((sum+200/2)/200)=
ForceSensor: (uint8_t)((46101+100)/200)=
ForceSensor: (uint8_t)(46201)/200)=
ForceSensor: (uint8_t)(231)=
ForceSensor: 231
ForceSensor: returning 231
231 (raw: 922), [right]
ForceSensor: millis() before = 5853
ForceSensor: millis() after = 5893
ForceSensor: filteredMeasure(): count=50, sum=47950, timeout? no
ForceSensor: (uint8_t)((sum+count*4/2)/count*4)=
ForceSensor: (uint8_t)((sum+200/2)/200)=
ForceSensor: (uint8_t)((47950+100)/200)=
ForceSensor: (uint8_t)(48050)/200)=
ForceSensor: (uint8_t)(240)=
ForceSensor: 240
ForceSensor: returning 240
240 (raw: 959)
[#] [name] : [servo angle] ([physical angle])
[1] boom1 : 86 (135)
[2] boom2 : 84 (-41)
[3] turret : 2 (1)
[4] claw : 93 (73)
Current position: (3,161,1)
Enter height:
Enter height:
Enter height:
Enter height:
Port closed

Well, it only took 40ms again, so I can safely set the timeout back to 100ms, as it was originally. It looks like this bug is fixed. I'll remove the debug code by commenting out the appropriate macro line, and try it one last time.

...
#define FILTERED_READ_POINTS 50
#define FILTERED_READ_TIMEOUT_MS 100
//#define ForceSensor_DEBUG_MODE // remember to increase timeout value
#ifdef ForceSensor_DEBUG_MODE
# define PRE Serial.print(F("ForceSensor: "))
# define POST delay(5)
# define BREAK Serial.println()
# define DEBUG(x) (x); POST
# define DEBUGLN(x) PRE; (x); BREAK; POST
#else
# define PRE
# define POST
# define BREAK
# define DEBUG(x)
# define DEBUGLN(x)
#endif // ForceSensor_DEBUG_MODE
...

Serial output:

Opening port
Port open
VMDPV_1|1_�MDPV
AutoMove.cpp compiled Dec 7 2017 at 11:42:52
AutoMove : Servos powered on.
AutoMoveSD : startScript() failed at F_CPU/2
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
SD card malfunction.
Force sensors configured: [left] 230 (raw: 922), [right] 240 (raw: 959)
[#] [name] : [servo angle] ([physical angle])
[1] boom1 : 86 (135)
[2] boom2 : 84 (-41)
[3] turret : 2 (1)
[4] claw : 93 (73)
Current position: (3,161,1)
Enter height:
Enter height:
Enter height:

Looks good! I had created a static function in ForceSensor to print out sensor values as above. Might as well try it out in the main code.

ForceSensor.cpp:

...
void ForceSensor::report(ForceSensor * sensorArr, uint8_t count)
{
for (uint8_t i = 0; i < count; i++)
{
if (i > 0)
Serial.print(F(", "));

Serial.print(sensorArr[i].read());
Serial.print(F(" (raw: "));
Serial.print(sensorArr[i].raw());
Serial.print(F(")"));
}
}
...

AutoMove.cpp:

...
Serial.print(F("Force sensors configured: "));
ForceSensor * sensorList[] = { &sensorL, &sensorR };
ForceSensor::report(sensorList, 2);
}
...

Serial output:

Opening port
Port open
VMDPV_1|1_�MDPV
AutoMove.cpp compiled Dec 7 2017 at 11:50:40
AutoMove : Servos powered on.
AutoMoveSD : startScript() failed at F_CPU/2
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
SD card malfunction.
Force sensors configured: [left] 230 (raw: 919), [right] 240 (raw: 958)
Force sensors configured: 230 (raw: 920), 0 (raw: 174)
[#] [name] : [servo angle] ([physical angle])
[1] boom1 : 86 (135)
[2] boom2 : 84 (-41)
[3] turret : 2 (1)
[4] claw : 93 (73)
Current position: (3,161,1)
Enter height:
Enter height:
Port closed

Well, the first sensor worked, but the second didn't. Must be a syntax error. I get confused when mixing references, pointers, and arrays. The function is currently generalized for any number of sensors, but it doesn't need to be made for any more than 2, so I'll simplify it with that in mind.

ForceSensor.cpp:

...
void ForceSensor::report(ForceSensor & sLeft, ForceSensor & sRight, bool lineBreak)
{
Serial.print(F("[left] "));
Serial.print(sLeft.read());
Serial.print(F(" (raw: "));
Serial.print(sLeft.raw());
Serial.print(F("), "));
Serial.print(F("[right] "));
Serial.print(sRight.read());
Serial.print(F(" (raw: "));
Serial.print(sRight.raw());
Serial.print(F(")"));
if (lineBreak)
Serial.println();
}
...

AutoMove.cpp:

...
Serial.print(F("Force sensors configured: "));
ForceSensor::report(sensorL, sensorR, true);
}
...

Serial output:

Opening port
Port open
VMDPV_1|1_�MDPV
AutoMove.cpp compiled Dec 7 2017 at 11:59:40
AutoMove : Servos powered on.
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
SD card malfunction.
Force sensors configured: [left] 231 (raw: 925), [right] 240 (raw: 959)
Force sensors configured: [left] 231 (raw: 926), [right] 240 (raw: 959)
[#] [name] : [servo angle] ([physical angle])
[1] boom1 : 86 (135)
[2] boom2 : 84 (-41)
[3] turret : 2 (1)
[4] claw : 93 (73)
Current position: (3,161,1)
Enter height:

Perfect. I'll remove the other method, and turn the sensor power logic back on. (I mentioned that we turned it off during debugging, but didn't show that code block, so I won't show it here, either.)

Serial output:

Opening port
Port open
VMDPV_1|1_�MDPV
AutoMove.cpp compiled Dec 7 2017 at 12:03:21
AutoMove : Servos powered on.
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
SD card malfunction.
Force sensors configured: [left] 231 (raw: 0), [right] 240 (raw: 0)
[#] [name] : [servo angle] ([physical angle])
[1] boom1 : 86 (135)
[2] boom2 : 84 (-41)
[3] turret : 2 (1)
[4] claw : 93 (73)
Current position: (3,161,1)
Enter height:
Enter height:
Port closed

Looks like we're done! The raw() values are correctly reported as zero, as the sensors are not automatically turned on for that function. (It is primarily for debugging.) Turning on the sensors as in the following code should show their values.

...
Serial.print(F("Force sensors configured (normally off): "));
ForceSensor::report(sensorL, sensorR, true);
// debug demo
sensorL.on();
sensorR.on();
Serial.print(F("Force sensors configured (normally on) : "));
ForceSensor::report(sensorL, sensorR, true);
}
...

Serial output:

Opening port
Port open
VMDPV_1|1_�MDPV
AutoMove.cpp compiled Dec 7 2017 at 12:06:37
AutoMove : Servos powered on.
AutoMoveSD : startScript() failed at F_CPU/2
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
AutoMoveSD : getUnique...() : sd.chdir() to root failed.
AutoMoveSD : sd.chdir() to root failed.
SD card malfunction.
Force sensors configured (normally off): [left] 231 (raw: 0), [right] 240 (raw: 0)
Force sensors configured (normally on) : [left] 231 (raw: 925), [right] 240 (raw: 959)
[#] [name] : [servo angle] ([physical angle])
[1] boom1 : 86 (135)
[2] boom2 : 84 (-41)
[3] turret : 2 (1)
[4] claw : 93 (73)
Current position: (3,161,1)
Enter height:
Port closed

This bug is officially squashed. The program is overdue for a git commit. (There should have been intermediate commits, perhaps even its own bugfix branch, during the above efforts. I'll do better in the future.) You can see the results in the development branch here: [GitHub.com/.../ForceSensor.cpp]

Comments

  • Susanne Cardwell December 7, 2017 - 4:21pm

    Thanks Tyler. I noticed you defined debug and then used that command.  Why did you define debug?  

    Also, could you have constrained the sensor values into 0 to 255 range?  Or is my thinking wrong? 

    Thank you. 

  • Tyler Lucas December 7, 2017 - 5:00pm

    The way the preprocessor debug blocks are setup, they can all be completely removed from compilation by commenting out one line. It makes it really easy to turn debugging on or off in different files. Though ForceSensor.cpp has only simple DEBUG() macros, I've gone a bit further with them in other files. AutoMoveSD.cpp, for example, uses three 'levels' of debugging, each of which can be activated or deactivated individually, or all at once. It lets me control how many debug lines the program is spitting out. Might make it easier to remove all of them, if need be, too. Would use some parser to remove any line with 'DEBUG', 'PRE', 'POST', or 'BREAK', then use a diff tool (git, etc) to make sure I'm not deleting a line by accident. Anyway, it seems ugly but its better than nothing, and way better than bare 'Serial.print(blah)' lines that I later have to hunt down and comment out.

    Constraining the sensor values wouldn't have worked since any value over 255 would have been set to 255, but mapping them might have -- Arduino core has a map(...) function

    uint8_t eightBitValue = map(tenBitValue, 0, 1023, 0, 255);

    The way I did it ensured half-up rounding with integers ((numerator + denominator/2)/denominator). I'm not certain that map does this.

  • Susanne Cardwell December 7, 2017 - 8:12pm

    Thanks Tyler. How did you know the "debug" command worked with the sensor?  Was it in the documentation?

    I don't know where to find documentation for sensors and libraries.  Where do you get yours?  Please kindly advise.

    Thank you. 

  • Tyler Lucas December 7, 2017 - 9:02pm

    Nothing really works with the sensor. It's just a resistor. Arduino's analogRead() does the trick. I usually find docs by Googling them.