// Calibration color order: BLUE --> GREEN --> RED
void calibrateSensors(int Iteration){
// Creating calculation variables
int delayTime = 40; // Delay time to allow phototransisitor reading to stabilize between color flashes
float calibrationSample = 20;
float blueVals[20];
float greenVals[20];
float redVals[20];
float blueSTD;
float greenSTD;
float redSTD;
float blueMin;
float blueMax;
float greenMin;
float greenMax;
float redMin;
float redMax;
// Resetting values
redSum = 0;
greenSum = 0;
blueSum = 0;
redAve = 0;
greenAve = 0;
blueAve = 0;
if(Iteration == 0){
Serial.println("Calibrating for blue.");
}
else if(Iteration == 1){
Serial.println("Calibrating for green");
}
else{
Serial.println("Calibrating for red");
}
// Gather data ******************************************************************************************
Serial.println("");
Serial.println("Blue, Green, Red");
for (int i = 0; i <= calibrationSample; i++) {
Serial.println(i);
//*** set LED color to blue ***//
digitalWrite(bluePin, LOW);
digitalWrite(greenPin, HIGH);
digitalWrite(redPin, HIGH);
delay(delayTime); // wait for photo tranistor to come to steady state
blueVals[i] = analogRead(colorPin); // read phototransistor and store value
blueSum = blueSum + blueVals[i]; // Update the sum
Serial.print(blueVals[i]); Serial.print(", "); // print stored value.
//*** set LED color to green ***//
digitalWrite(bluePin, HIGH);
digitalWrite(greenPin, LOW);
digitalWrite(redPin, HIGH);
delay(delayTime); // wait for photo tranistor to come to steady state
greenVals[i] = analogRead(colorPin); // read phototransistor and store value
greenSum = greenSum + greenVals[i]; // Update the sum
Serial.print(greenVals[i]); Serial.print(", "); // print stored value.
//*** set LED color to Red ***//
digitalWrite(bluePin, HIGH);
digitalWrite(greenPin, HIGH);
digitalWrite(redPin, LOW);
delay(delayTime); // wait for photo tranistor to come to steady state
redVals[i] = analogRead(colorPin); // read phototransistor and store value
redSum = redSum + redVals[i]; // Update the sum
Serial.println(redVals[i]); // print stored value.
}
// turn off LED and wait for another cycle
digitalWrite(bluePin, HIGH);
digitalWrite(greenPin, HIGH);
digitalWrite(redPin, HIGH);
// Interpret the data *******************************************************************************************
// Calculate the averages
blueAve = blueSum / calibrationSample;
greenAve = greenSum / calibrationSample;
redAve = redSum / calibrationSample;
// Calculate the standard deviations
for(int i = 0; i <= calibrationSample; i++){
blueSTD = blueSTD + sq(blueVals[i] - blueAve);
greenSTD = greenSTD + sq(greenVals[i] - greenAve);
redSTD = redSTD + sq(redVals[i] - redAve);
}
blueSTD = sqrt(blueSTD / calibrationSample);
greenSTD = sqrt(greenSTD / calibrationSample);
redSTD = sqrt(redSTD / calibrationSample);
// Calculate the color ranges
blueMin = blueAve - 3*blueSTD;
blueMax = blueAve + 3*blueSTD;
greenMin = greenAve - 3*greenSTD;
greenMax = greenAve + 3*greenSTD;
redMin = redAve - 3*redSTD;
redMax = redAve + 3*redSTD;
Serial.println("Getting to assignment");
// Set the calibration data
switch(Iteration){
case 0:
blueBounds[0][0] = blueMin; // Blue reading minium
blueBounds[0][1] = blueMax; // Blue reading maximum
blueBounds[1][0] = greenMin; // Green reading minimum
blueBounds[1][1] = greenMax; // Green reading maximum
blueBounds[2][0] = redMin; // Red reading minimum
blueBounds[2][1] = redMax; // Red reading maximum
break;
case 1:
greenBounds[0][0] = blueMin; // Blue reading minium
greenBounds[0][1] = blueMax; // Blue reading maximum
greenBounds[1][0] = greenMin; // Green reading minimum
greenBounds[1][1] = greenMax; // Green reading maximum
greenBounds[2][0] = redMin; // Red reading minimum
greenBounds[2][1] = redMax; // Red reading maximum
break;
case 2:
redBounds[0][0] = blueMin; // Blue reading minium
redBounds[0][1] = blueMax; // Blue reading maximum
redBounds[1][0] = greenMin; // Green reading minimum
redBounds[1][1] = greenMax; // Green reading maximum
redBounds[2][0] = redMin; // Red reading minimum
redBounds[2][1] = redMax; // Red reading maximum
break;
}
}
The serial print that prints out the value of i at the start of the first for loop is what is causing the behavior.
Not having the serial print causes the function to get stuck in the first for() loop, looping infinitely and not stopping at the value of calibrationSample as expected. If I add the serial print, the first for() loop behaves as expected, leaving the for loop after 20 iterations.
I was trying to provide the minimum reproducible example, here's the main script:
#include <SoftwareSerial.h>
#include "DualTB9051FTGMotorShieldMod3230.h"
#include "Encoder.h"
#include "PWMServo.h"
#include "ezButton.h"
// Declaring Functions just in case
void findZero();
void processOrders();
void resetRobot();
void scanForBlock(int);
void calibrateSensors(int);
void collectBlock();
void depositOrder(int);
void goToTable1();
void goToTable2();
void goToTable3();
void extendGrabberSetDistance(float, float);
void loweringRobot();
// State Variable ******************************************************************************************************
int stateVal = 9;
// Order variable
char orders[13] = { 0 };
// Create servo object *************************************************************************************************
PWMServo releaseServo;
const int servoMotorPin = A13;
// Block sensing setup *************************************************************************************************
// Initializing block sensing pins
const int bluePin = 53;
const int greenPin = 51;
const int redPin = 49;
int colorPin = A9;
int hallPin = A8;
// Initializing variables for block sensing
int valCheck; // Storage variable for the photodiode reading when gathering data
int hallRange = 450; // Set value for desired range for hall effect sensor
int delayTime = 40; // Delay time to allow phototransisitor reading to stabilize between color flashes
int ticks = 5; // Number of readings per sample
int redSum; // Sum of red readings per sample
int greenSum; // Sum of green readings per sample
int blueSum; // Sum of blue readings per sample
int redAve; // average of red readings per sample
int greenAve; // average of green readings per sample
int blueAve; // average of blue readings per sample
// Setting the sensor color ranges
float blueBounds[3][2] = { { 110, 195 }, // Blue LED
{ 34, 124 }, // Green LED
{ 0, 103 } }; // Red LED
float redBounds[3][2] = { { 33, 158 }, // Blue LED
{ 38, 126 }, // Green LED
{ 126, 174 } }; // Red LED
float greenBounds[3][2] = { { 64, 143 }, // Blue LED
{ 56, 169 }, // Green LED
{ 13, 99 } }; // Red LED
// Variables for keeping track of detected, collected, and deposited blocks
char detectedBlock; // Storage variable for the currently read block
int depositToTable = 3; // Which table is going to be deposited at
int tablesCollected = 0; // Variable for tracking how many table orders have been collected
int blocksCollected = 0; // Variable for tracking how many blocks have been collected
bool table1Complete = false; // Boolean to keep track of when table 1 order is completed
bool table2Complete = false; // Boolean to keep track of when table 2 order is completed
bool table3Complete = false; // Boolean to keep track of when table 3 order is completed
// Positional and Zero Finding *****************************************************************************************
int proximityPin = A10; // Pin for sensor that detects when bot has finished lowering
int switch1 = 35; // Switch 1 = Zeroing driver wheel
int switch2 = 37; // Switch 2 = Zeroing inner arm
int switch3 = 11; // Switch 3 = Zeroing grabber arm
int debounceTime = 50; // Debounce time for switches
ezButton limitSwitch1(switch1); // Limit switch for setting the rotation zero point
ezButton limitSwitch2(switch2); // Limit switch for setting the inner arm zero point
ezButton limitSwitch3(switch3); // Limit switch for setting the grabber arm zero point
// Setting variables to store the switch states
bool switchState1 = LOW; // Variable for keeping track of grabber arm limit switch
bool switchState2 = LOW; // Variable for keeping track of inner arm limit switch
bool switchState3 = LOW; // Variable for keeping track of arm position limit switch
// State Positions *****************************************************************************************************
int collectionAngle = 0; // Angle where the robot will collect blocks
int depositAngle = 90; // Angle where the robot will deposit blocks
int grabberCollection = 0; // Position where the grabber will collect blocks
int grabberRetraction = -6; // Position where the grabber will move to collect a block
int table3Inner = 20; // Inner arm position for table 3 deposit
int table2Inner = 10; // Inner arm position for table 2 deposit
int table1Inner = 5; // Inner arm position for table 1 deposit
int avoidBlocks = 60; // Position the arm will rotate to between moving to the next table to avoid knocking over deposited blocks
int depositCount = 0; // Number of orders that have been deposited
float prevDepositPosition; // Where the grabber arm went to deposit it's previous order
bool grabComplete = false; // Variable to track when the block has been pulled into arm
// Order Variables
int fourBlockTable; // Variable to track which table is the four block order
char table1Orders[5] = { 0 }; // Character array for table 1 order
char table2Orders[5] = { 0 }; // Character array for table 2 order
char table3Orders[5] = { 0 }; // Character array for table 3 order
// Motor Setup & Control ***********************************************************************************************
// Desired Velocities
float timeDesCollect = 0.5; // Time to complete each step in the collection operation
float timeDesGoTo3 = 5; // Time to move from collection to table 3 position
float timeDesGoTo2A1 = 2; // Time to go from tables 3 to 2 and 2 to 1
float collectDist = 6; // Distance to perform each operation of collection
float collectTo3Dist = 24; // Extension distance from collection position to table 3
float collectTo3Rot = 90.0 * (PI / 180); // Rotation distance from collection position to table 3
float rotateOutOfWay = 30.0 * (PI / 180.0); // Rotation distance from collection position to move arm out of way
float goTo1A2 = 6; // Retraction distance from table 3 to 2 and 2 to 1
float collectBlockVel = collectDist / timeDesCollect; // Velocity for block collection operation
float goTo3Vel = collectTo3Dist / timeDesGoTo3; // Velocity for extending from collection to table 3
float goTo3Omega = collectTo3Rot / timeDesGoTo3; // Rotational velocity for moving from collection to table 3
float goTo2A1Vel = goTo1A2 / timeDesGoTo2A1; // Retraction velocity for moving from table 3 to 2 and 2 to 1
float outOfWayOmega = rotateOutOfWay / timeDesGoTo2A1; // Rotational velocity for moving out of the way of blocks
float timeDesToDeposit = 2; // Variable for calculating the deposit time
float deposit4Dist = 8; // Distance to deposit 4 blocks
float deposit3Dist = 6; // Distance to deposit 3 blocks
float depositVel; // Variable for deposit velocity of 3 blocks
// Declaring encoders
Encoder M1Encoder(2, 3);
Encoder M2Encoder(18, 19);
Encoder M3Encoder(20, 21);
// Declaring remapping for motor 1
unsigned char M1EN = 22;
unsigned char M1DIR = 7;
unsigned char M1PWM = 9;
unsigned char M1DIAG = 6;
unsigned char M1OCM = A0;
// Declaring remapping for motor 2
unsigned char M2EN = 4;
unsigned char M2DIR = 8;
unsigned char M2PWM = 10;
unsigned char M2DIAG = 12;
unsigned char M2OCM = A1;
// Declaring remapping for motor 3
unsigned char M3EN = 23;
unsigned char M3DIR = 27;
unsigned char M3PWM = 45;
unsigned char M3DIAG = 31;
unsigned char M3OCM = A2;
// Just in case setup for motor 4
unsigned char M4EN = 25;
unsigned char M4DIR = 29;
unsigned char M4PWM = 46;
unsigned char M4DIAG = 33;
unsigned char M4OCM = A4;
// Command to perform remaps
DualTB9051FTGMotorShieldMod3230 md(M1EN, M1DIR, M1PWM, M1DIAG, M1OCM, M2EN, M2DIR, M2PWM,
M2DIAG, M2OCM, M3EN, M3DIR, M3PWM, M3DIAG, M3OCM, M4EN,
M4DIR, M4PWM, M4DIAG, M4OCM);
// Hardware Constants
float motorGearRatio = 131; // 131 RevsMotor/RevsShaft
float countsPerMotorRev = 64; // 64 counts/RevsMotor
float grabberGearRadius = .85; // Radius of grabber gear in inches
float innerGearRadius = .54; // Radius of inner gear in inches
float leverArm = 33.5; // Distance of drive wheel from base in inches
float wheelRadius = 1.37795276; // Wheel radius in inches
float kp1 = 150; // Tuning variable for grabber arm
float kp2 = 150; // Tuning variable for inner arm
float kp3 = 80; // Tuning variable for arm rotator
// Time tracking variables
unsigned long startTime = 0; // Time the operation actually starts
unsigned long t1 = 0; // Motor 1 current time variabe
unsigned long t2 = 0; // Motor 2 current time variabe
unsigned long t3 = 0; // Motor 3 current time variabe
unsigned long tOld1 = 0; // Motor 1 previous time variable
unsigned long tOld2 = 0; // Motor 2 previous time variable
unsigned long tOld3 = 0; // Motor 3 previous time variable
unsigned long deltaT = 0; // Difference between t & tOld
// Position tracking variables
float completeTime; // Time that the operation will be completed in
long counts1 = 0; // Current encoder reading for motor 1
long counts2 = 0; // Current encoder reading for motor 2
long counts3 = 0; // Current encoder reading for motor 3
float shaft1 = 0; // Current motor 1 shaft position in radians
float shaft2 = 0; // Current motor 2 shaft position in radians
float shaft3 = 0; // Current motor 3 shaft position in radians
float pos1 = 0; // Actual current grabber arm position in inches
float pos2 = 0; // Actual current inner arm position in inches
float angle = 0; // Actual current arm position in degrees
float M1 = 0; // Motor 1 power
float M2 = 0; // Motor 2 power
float M3 = 0; // Motor 3 power
float omegaDesired; // Desired rotational velocity in rad/sec
float vel1Desired; // Desired grabber arm extension velocity in inches/microsecond
float vel2Desired; // Desired inner arm extension velocity in inches/microsecond
float posD1; // Iteration desired extension variable for grabber arm
float posD2; // Iteration desired extension variable for inner arm
float angleD; // Iteration desired angle variable
void setup() {
// Initializing communication
Serial.begin(19200); // Open serial communications with computer and wait for port to open:
Serial2.begin(9600); // Open serial communications with the other Arduino board
delay(20); // Wait for serial to initialize
//Initializing motors
md.init();
md.enableDrivers();
// Configure color sensing pins
pinMode(redPin, OUTPUT);
pinMode(greenPin, OUTPUT);
pinMode(bluePin, OUTPUT);
// Set LED to off
digitalWrite(redPin, HIGH);
digitalWrite(greenPin, HIGH);
digitalWrite(bluePin, HIGH);
// Configure the servo
releaseServo.attach(servoMotorPin);
releaseServo.write(0);
// Configure limit switch debounce times
limitSwitch1.setDebounceTime(debounceTime);
limitSwitch2.setDebounceTime(debounceTime);
limitSwitch3.setDebounceTime(debounceTime);
// update the old times
tOld1 = micros();
tOld2 = micros();
tOld3 = micros();
// Clear the serial buffer for upload
while(Serial2.available() != 0){
Serial2.read();
}
}
void loop() {
// Check for orders and if it's good data, if bad data, flush the serial buffer, otherwise, go to order processing state
if (Serial2.available() > 12) {
if (Serial2.read() != 255) {
Serial.println("Bad data received, wiping the serial buffer.");
while (Serial2.available() != 0) {
Serial2.read();
}
}
else {
Serial.print("formatting orders, the receieved table orders are: ");
for (int i = 0; i <= 11; i++) {
orders[i] = Serial2.read();
Serial.print(orders[i]);
}
Serial.println();
delay(20);
while (Serial2.available() != 0) {
Serial2.read();
}
stateVal = 0;
}
}
switch (stateVal) {
// --> Lowering the robot from start position ***************************************************************************************
case -2:
loweringRobot();
break;
// --> Emergency stop, shutting down and resetting the robot ************************************************************************
case -1:
resetRobot();
stateVal = 0;
break;
// --> Awaiting and processing orders ***********************************************************************************************
case 0:
processOrders();
break;
// --> Calibrate the sensors ********************************************************************************************************
case 1:
// Perform calibration for 1 color at a time, allowing a four second period in between data collection for the user to turn the block to the next color
// Color order: Blue --> Green --> Red
for (int i = 0; i <= 2; i++) {
Serial.println("Calibration protocol initiated, beginning calibration. You have 4 seconds to place the block and 4 seconds in between scans.");
Serial.println("Block order is blue --> green --> red");
delay(4000);
calibrateSensors(i);
}
stateVal = 0; // Return to awaiting and processing orders
break;
// --> Find and set the zero position ***********************************************************************************************
case 2:
findZero();
break;
// --> Start scanning for blocks ***************************************************************************************************
case 3:
if (tablesCollected == 0) {
scanForBlock(1);
if (table1Orders[0] == 0 && table1Orders[1] == 0 && table1Orders[2] == 0 && table1Orders[3] == 0) {
tablesCollected = tablesCollected + 1;
}
} else if (tablesCollected == 1) {
scanForBlock(2);
if (table2Orders[0] == 0 && table2Orders[1] == 0 && table2Orders[2] == 0 && table2Orders[3] == 0) {
tablesCollected = tablesCollected + 1;
}
} else if (tablesCollected == 2) {
scanForBlock(3);
if (table3Orders[0] == 0 && table3Orders[1] == 0 && table3Orders[2] == 0 && table3Orders[3] == 0) {
tablesCollected = tablesCollected + 1;
stateVal = 5;
}
}
break;
// --> Block collecting *******************************************************************************************
case 4:
collectBlock();
break;
// --> Move arm to table 3 ****************************************************************************************
case 5:
goToTable3();
stateVal = 8;
break;
// --> Move arm to table 2 ****************************************************************************************
case 6:
goToTable2();
stateVal = 8;
break;
// --> Move arm to table 1 ****************************************************************************************
case 7:
goToTable1();
stateVal = 8;
break;
// --> Deposit order ***********************************************************************************************
case 8:
depositOrder(depositToTable);
depositToTable = depositToTable - 1;
break;
}
}
What are you trying to point out here? I don't see anything wrong with this.
Ah! yea that's a problem. I fixed that, and I found what you meant by the callout. It's doing 21 samples, not 20. Fixing both of those makes it work properly.
Ah! Thank you! That lines up with the problems I just identified. This isn't the first time I've run into the presence of serial prints modifying how things behave and I've been trying to figure out what it means.
What other things can cause this behavior? In the past, a serial print was changing the value of a variable.
I posted on it, you can go into my history to see the scenario.
A quick description is that if I had a serial print before an if statement, it was causing the variable to be read as 0.0, but if I didn't have the serial print there, the variable was being treated as 0.000014 or something of the like and so it wasn't interacting with the if statement the way I expected.
Looks like someone had already directed you to the source of the problem before I posted my comment.
The general reason the serial print changes the behavior of the code is that the text used in the serial print is stored in ram, and when you add/remove the print the data stored in ram is arranged slightly differently, changing what an out-of-bounds write is overwriting.
Another affect of an additional serial print is the delay caused by having to wait for space in the serial transmit buffer if the buffer becomes full.
I'd appreciate if you just tell me the issues you see.
Scroll towards the bottom. That thread is actually two threads. The issue regarding the serial prints was originally a second separate thread that got merged with the link I gave you.
And yet the location of where I placed a serial print affected what that serial print printed out to me even though there were no lines of code that changed the value of what I was printing.
The issue of comparing floats was the problem, however, it still doesn't explain how having the serial print before or after the if statement affected what it displayed
Or I'm just new to this, taking a college class that assumes I know more than I do and am trying to learn and do a lot on extremely tight deadlines. I find their and your attitude to be rather rude.