-
Notifications
You must be signed in to change notification settings - Fork 1
Code Walkthrough
Note To Editors: instead of adding massive comments in the code, put your overly detailed explanations here. If it is a more general reference, make a separate page and link to it (like the PID reference). Please try to keep this explanation up to date as changes are made in URSA's code.
Welcome to an explanation of the code running on the URSA robot!
If this is your first introduction to Arduino (welcome!), or if you find a command in this code you don't know about, I recommend checking this great Arduino reference page. If the Arduino site doesn't answer your question, try googling.
The code for the robot is divided into 5 files, to contain related code and avoid a single very long file. There is (in the order the tabs are explained on this page) the ursa tab for all the main code, the stepper tab with code for running the stepper motors, the mpu tab for MPU6050 reading and orientation code, the wifi tab for the wifi code, and the saverecall tab with code for saving and recalling settings to memory.
There are comments throughout the code (words after //) used to explain some of what's happening, and while some of this reference is just those same comments, some of this reference should be more in depth.
It may be helpful to have the code open in the Arduino IDE next to this walkthrough so you can see it with the proper colors and without the constant interruption too.
This code walkthough will start with the ursa tab because that's where the main structure of the code is.
The program starts by including libraries - code that someone else wrote that we can use to do various functions.
- The
PID_v1library (can be downloaded from here or installed using Arduino's library manager) nicely wraps up PID control loops which we use for balancing. Please see the pid reference page for a more detailed explanation of the pid loops. PID library reference - The
Wirelibrary is used for communicating with the I2C protocol used for the mpu6050 orientation sensor. Arduino reference page for the wire library -
WiFi,WiFiAP, andWiFiUdpare libraries for using the esp32's wifi module to create an access point and send Udp messages over wifi, so the robot can be controlled remotely. example of making a wifi network from an esp32 - The
EEPROMlibrary is used to store settings to memory that isn't erased when the robot is powered off. Unlike normal arduinos, esp32s don't actually have EEPROM memory; this library stores data to a section of flash memory.
In the Arduino IDE, look under Sketch > Include Library to see some of the many arduino libraries available.
Next, we define some numbers. See this page for a list of different types of variables.
#define does a find and replace before the code is compiled. It is good for a number that will never be changed (at least not until you re-upload code)-it doesn't take any RAM memory. #define doesn't need a semicolon
Other "constants" here are declared as variables since it is possible that someone would want to change them while the robot is running. The numbers these constants are set equal to here are just examples - the point of declaring these variables here is to make them easy to adjust.
#define ROBOT_ID 0 unique robot ID, sent to DS, and used to name wifi network
#define MODEL_NO 0 specifies the configuration of the robot (what features does it have that the DS should know about)
#define WiFiLossDisableIntervalMillis 500 if no data packet has been received for this number of milliseconds, the robot disables to prevent running away. 1000 miliseconds in a second.
#define DACUnitsPerVolt 123.4 Use to calibrate voltage read through voltage divider. Set so that by dividing value analogRead from voltage divider by this constant gets the voltage of the batteries. analogRead returns from 0 to 4095 corresponding to 0-3.3 volts The maximum voltage put into the input pin can't be above 3.3v
float MAX_ACCEL = 180; Limits the maximum change in the motor speed value per loop. StepperSteps per Second per loop Stepper motors will stall if they try to accelerate too fast.
float COMPLEMENTARY_FILTER_CONSTANT = .9997; higher = more gyro based, lower=more accelerometer based here's one explanation of this kind of filter also see the explanation in the mpu tab where this constant is used
int MAX_SPEED = 1500; max speed (in steps/sec) that the motors can run at
float MAX_TIP = 15; max angle in degrees that the robot can target when leaning to change speed. The output limit for the speed PID loop. Increasing this means the robot could change speed faster, but it shouldn't be so high that the motors can't accelerate fast enough to stay balanced.
float DISABLE_TIP = 50; max angle in degrees the robot will attempt to recover from -- if passed, robot will disable so it won't keep trying to drive if it's lying on the floor and unable to get up.
float DRIVE_SPEED_SCALER = .85; what proportion of MAX_SPEED the robot's target driving speed can be-some extra speed must be kept in reserve to be able to remain balanced
float TURN_SPEED_SCALER = .05; what proportion of MAX_SPEED can be given differently to each wheel in order to turn - controls maximum turn rate
float PITCH_OFFSET_CHANGE = .999994; The variable pitchOffset (see below) is adjusted by slowly moving it towards the current pitch so it ends up equaling the average pitch value, and the average pitch of a self balancing robot should be the balance point since it should accelerate forwards about as much as backwards. The closer this number is to 1 the slower the value changes.
float pitchOffset = 0.000; subtracted from the output in readMPU6050 so that a value of 0 for pitch corresponds to balanced. Because the MPU6050 may not be mounted in the robot perfectly or because the robot's weight might not be perfectly centered, zero may not otherwise respond to perfectly balanced.
Next, the pin numbers various components are connected to are defined.
STEP pins are used to trigger a step (when rides from LOW to HIGH) whereas DIR pins are used to change the direction at which the motor is driven (connected to a4988 driver). The ENS_PIN is wired to both motor driver chips' ENable pins, to turn on and off motors. The esp32 board has a built in led on pin 2, used as a status/RSL light. The VOLTAGE_PIN is the pin that is connected to the voltage divider to measure the voltage of the battery. The VOLTAGE_PIN must be on ADC1, as ADC2 is incompatible with wifi use.
#define LEFT_STEP_PIN GPIO_NUM_32
#define LEFT_DIR_PIN GPIO_NUM_33
#define RIGHT_STEP_PIN GPIO_NUM_34
#define RIGHT_DIR_PIN GPIO_NUM_35
#define ENS_PIN GPIO_NUM_23
#define LED_BUILTIN GPIO_NUM_2
#define VOLTAGE_PIN GPIO_NUM_36 //ADC1 CH0
The next constants relate to the movement detection code that waits for the robot to be held still before calibrating the gyroscope.
#define movementThreshold 25 sets how much the robot can move, larger is less strict, smaller requires less movement.
#define movementMeasurements 15 sets how many measurements are taken and summed before checking if the sum is small enough, larger will be more accurate because more samples are taken, smaller runs faster
#define maxWifiRecvBufSize 50 defines how large of an array should be declared to receive data into-set large enough to hold as many bytes as will be sent to the robot (including however many auxiliary values might get sent) but don't take any more memory than necessary.
#define maxWifiSendBufSize 50 defines how large of an array should be declared to send data from-set large enough to hold as many bytes as will be sent from the robot (including however many auxiliary values might get sent) but don't take any more memory than necessary.
byte voltage=0 declares a variable to hold the measured battery voltage. 0 corresponds to 0 volts, 255 corresponds to 13v.
Next we declare a "mutex," (the name comes from "mutual exclusion"). The code that receives wifi data runs in one task on one processor, and the main control task using input from that data runs in a different task on the other processor. Bad things happen if a variable is being accessed by multiple tasks at once. The mutex is a way for the tasks signal to each other whether they are using a variable.
SemaphoreHandle_t mutexReceive;
boolean robotEnabled = false; declares a variable of whether the robot should enable outputs
boolean wasRobotEnabled = false; to know if robotEnabled has changed
boolean enable = false; is the DS telling the robot to enable? (different from robotEnabled so the robot can disable when tipped even if the DS is telling it to enable)
boolean tipped = false; Is the robot currently tipped past MAX_TIP?
double targetPitch = 0.000; what angle the balancing control loop should aim for the robot to be at, the output of the speed control loop
double motorSpeed = 0.000; how much movement in the forwards/backwards direction the motors should move-only one set of control loops is used for balancing, not one for each motor
volatile int leftMotorWriteSpeed = 0; stepper ticks per second that the left motor is currently doing "volatile" because used in an interrupt
volatile int rightMotorWriteSpeed = 0;
double motorAccel = 0; how many stepper ticks per second per loop cycle the motors should be made to accelerate at, used as output of angle balancing loop
double speedVal = 0; how many stepper ticks per second the robot should try to drive at-the input to the speed control loop.
int turnSpeedVal = 0; differential stepper ticks (positive=turn right, negative=turn left)
doubles kP_angle, kI_angle, kD_angle are fed to the angle PID loop as the Proportional Integral and Derivative constants for the PID loop that controls the angle that the robot tries to tilt to.
For more details about PID loops, see this page.
doubles kP_speed, kI_speed, kD_speed are fed to the speed PID loop as the Proportional Integral and Derivative constants for the PID loop that controls the speed that the robot tries to drive at.
declare variables int16_t accelerationX, accelerationY, accelerationZ, rotationX, rotationY, rotationZ to hold the raw values for acceleration and rotation read from the MPU6050 orientation sensor. Currently only used internally in orientation code.
declare variables int32_t rotationOffsetX, rotationOffsetY, rotationOffsetZ The gyro can get slightly out of calibration so that 0 does not correspond to no rotation. These values are set to the measured offset from zero on start up (see zeroMPU6050) and used to cancel out the gyro drift.
unsigned long lastCalcedMPU6050 is used to store the micros() value of the last orientation read, it is used to integrate gyro data to get rotation.
doubles rotationDPS_X, rotationDPS_Y, rotationDPS_Z are set to the rotation in Degrees Per Second around the X, Y, and Z axes, with x left right, y forwards and backwards and z up and down. Currently only used internally in orientation code.
the double pitch is calculated in the readMPU6050 function. The forwards backwards pitch of the robot is important for balancing-this is an input to the control loop. negative=tipping forwards, positive=backwards, angle in degrees
hw_timer_t *leftStepTimer = NULL;
hw_timer_t *rightStepTimer = NULL;
These lines declare timers that will be used to time pulses for the stepper drivers.
byte numBytesToSend is used in the wifi code to know how many bytes the message to send is
char robotSSID[12] this array of characters will be filled in setup() with the name for the robot's wifi access point.
const char *robotPass = "sert2521"; this is the wifi password for the robot's network. It's not that secure, but compared to an open wifi network might be less likely to be connected to by someone other than a SERT member with a driverstation.
Volatile because shared between tasks.
volatile byte recvdData[maxWifiRecvBufSize] = {0}; array to hold data recieved from DS.
volatile boolean receivedNewData = false; set true when data gotten, set false when parsed. This is used to signal code in the main loop when there is new data to parse from the wifi code.
volatile byte dataToSend[maxWifiSendBufSize] = {0}; array to hold data to send to DS.
char packetBuffer[maxWifiRecvBufSize] the UDP library needs a char array to read into
byte numAuxRecv is a variable received from the DS telling how many extra bytes are about to be sent for extra features.
byte auxRecvArray[12] = {0} declares an array (and fills it with zeros) for storing bytes sent for extra features from the driverstation. If more than 12 bytes will be sent, increase the number in [] or there will be some nasty errors.
byte numSendAux is a variable for how many extra bytes should be sent back to the DS (sensor data)
byte auxSendArray[12] = {0} declares an array for storing extra bytes to send to the DS. If more than 12 bytes will be sent, increase the number in [] or there will be some nasty errors.
volatile uint32_t lastMessageTimeMillis is used to record when a wifi message was last received. This is used for making the robot disable if it has gotten no commands for a time interval. millis() which this variable is set to when a message is received returns an unsigned long of how many milliseconds the processor has been running since it turned on.
byte saverecallState is sent by the DS to signal what to do about setting and recalling values saved to the robot. Nothing is done if the value is 0. If the value is 1, the values are recalled from memory, and sent to the DS (the DS should make this request on startup to learn what settings the robot is running with). If the value is 2, the robot saves the values to its flash memory. The save (2) command should only be sent by the DS rarely, since flash memory has a limited number of write cycles and (though it's in the hundreds of thousands) writing every time around the loop could quickly add up.
WiFiUDP Udp; used for sending messages over wifi
The following lines set up the PID loops. Please see the PID reference for details about the PID control loops.
The arguments taken are: PID(&Input, &Output, &Setpoint, Kp, Ki, Kd, Direction)
PID PIDA(&pitch, &motorAccel, &targetPitch, kP_angle, kI_angle, kD_angle, DIRECT); sets up the PID for controlling robot Angle
PID PIDS(&motorSpeedVal, &targetPitch, &speedVal, kP_speed, kI_angle, kD_angle, DIRECT); sets up the PID for controlling robot Speed
put code in the braces after this that you want run once on startup. Great for setting up things.
pinMode(LED_BUILTIN, OUTPUT);
This line sets the pin connected to the status led to the output mode. check Arduino reference
digitalWrite(LED_BUILTIN, HIGH);
This line sets the status led pin to the high state (3.3V) turning it on. check Arduino reference
` pinMode(ENS_PIN, OUTPUT); This line sets the PIN that controls the enable pins on the motor controllers to the output mode.
digitalWrite(ENS_PIN, HIGH);
This line sets the enable pin HIGH disabling the a4988 motor controllers. See the "enable input" section on page ten of the datasheet.
pinMode(VOLTAGE_PIN, INPUT);
This line sets the pin that will be used for reading the voltage of the batteries to input mode.
pinMode(LEFT_STEP_PIN, OUTPUT);
pinMode(RIGHT_STEP_PIN, OUTPUT);
pinMode(LEFT_DIR_PIN, OUTPUT);
pinMode(RIGHT_DIR_PIN, OUTPUT);`
These lines set the pins for controlling the stepper drivers to the output mode.
digitalWrite(LEFT_STEP_PIN, LOW);`
digitalWrite(RIGHT_STEP_PIN, LOW);`
digitalWrite(LEFT_DIR_PIN, LOW);`
digitalWrite(RIGHT_DIR_PIN, LOW);`
These lines set the pins for controlling the stepper drivers to the low state, driving them to zero volts. The stepper drivers don't like it if the pins are not always driven to a value.
Serial.begin(115200);
Start the serial communication port (at a specified bits per second) used for communicating with the computer over USB. This helps with debugging because you can print the state of variables. Arduino Serial reference specifically print() and println() are useful. Set the serial monitor's "baud rate" (in a drop down menu at the bottom of the monitor) to the same value or you will see nothing or gibberish.
mutexReceive = xSemaphoreCreateMutex();
This line sets up the mutex created earlier for avoiding conflicts around reading variables between tasks.
sprintf(robotSSID, "SERT_URSA_%02d", ROBOT_ID);
Create unique network SSID consisting of "SERT_URSA_<ROBOT_ID>" and put it in the char array robotSSID to be given to the wifi access point starting function.
To generate the robot's WiFi network name, the code takes in a template (SERT_URSA_%02d) and an unique ID number for the robot. The sprintf function takes in 2 required parameters, with additional optional parameters that can be added to substitute into the template, giving it a signature of sprintf(output, template, ...values). Substitutions in the template string are denoted by %s, followed by a d, s, or some combination of other options which can be found here.
In this case, the %02d denotes a substitution of type integer (given by the d), left padded by 0s to make it 2 characters long. Note that if ID is ever > 99, you'll get an exception. For example, our function could be used like so:
int ID = 5;
char output[12];
sprintf(output, "SERT_URSA_%02d", ID);
// output => SERT_USRA_05
ID = 93;
sprintf(output, "SERT_URSA_%02d", ID);
// output => SERT_USRA_93
try it here
EEPROM.begin(64);
Start the EEPROM library, with 64 bytes of storage. If many more settings are added, increase the bytes here as necessary.
PIDA.SetMode(MANUAL);
turn the angle PID loop off. see the pid reference page
PIDS.SetMode(MANUAL);
turn the speed PID loop off
PIDA.SetSampleTime(10);
Tell the angle PID loop how often to run (in milliseconds) We have to call PID.Compute() at least this often. The interval (currently 10 millisecond) needs to be longer than how long long it takes to run loop or the "promise" made to the PID loop about how often to run PID.Compute isn't kept.
PIDS.SetSampleTime(1);
Tell the angle PID loop how often to run (in milliseconds) We have to call PID.Compute() at least this often.
PIDA.SetOutputLimits(-MAX_ACCEL, MAX_ACCEL);
The output of the Angle PID loop is what acceleration to change the motor speed at. Acceleration changes tip, velocity by itself doesn't make the robot tip.
PIDS.SetOutputLimits(-MAX_TIP, MAX_TIP);
Tell the Speed PID loop how far it can ask the Angle PID loop to tip the robot while trying to change speed.
recallSettings();
Recall variables that were stored to EEPROM so that settings aren't lost after a power cycle.
setupMPU6050();
This function starts the connection to the MPU6050 gyro/accelerometer board using the I2C Wire library, and tells the MPU6050 some settings to use.
zeroMPU6050();
This function averages some gyro readings so later the readings can be calibrated to zero. This function blocks until the robot is held still, so the robot needs to be set flat on the ground before startup will finish. See the mpu tab.
setupWifi();
This function, defined in the wifi tab (see wifi section below for details), starts the wifi connection with the right settings and also starts a second task for the wifi code to run in.
setupStepperTimers();
This function, defined in the stepper tab, starts two timers that will be used for timing motor steps, and attaches them to the functions that cause a step. See the the stepper section below for details.
digitalWrite(LED_BUILTIN, LOW);
}
This brace closes void setup(). This is the end of the setup function.
The code in the function void loop gets run in a loop on core 1, put the main control code here, and try to keep it as fast as possible.
readMPU6050();
Call the function that reads the orientation board and sets the pitch variable. See the mpu tab for where this is defined.
voltage = map(analogRead(VOLTAGE_PIN) * 1000.00 / DACUnitsPerVolt, 0, 13000.0, 0, 255);
This line uses the analogRead(pin) function to get the voltage on an analog input capable pin-it is returned as a number from 0 to 4095 corresponding to 0 to 3.3 volts. The returned number is divided by the constant DACUnitsPerVolt
The following code is the connection from the main loop to the wifi task.
if (receivedNewData) {
run the code in brackets if there is a new message from wifi
if (xSemaphoreTake(mutexReceive, 1) == pdTRUE) {
run the code in brackets if the variables that the wifi function writes into are free. In the (probably unlikely) case that the wifi task is in the middle of changing values, the code in brackets won't be run so there won't be a conflict.
parseDataReceived();
call a function (defined later in the ursa tab) that takes the array of bytes that was recieved over WiFi and splits it up and changes the variables the DriverStation was trying to change
numBytesToSend = createDataToSend(); call a function (defined later in the ursa tab) that takes the variables the robot wants to send and splits them up and puts them in an array of bytes so they're ready to get sent by the wifi code. This function also returns a number numBytesToSend that equals how many places in the array of values to send have been filled with data and should be sent. numBytesToSend gets used by the wifi code.
receivedNewData = false; reset the new data flag boolean to false so this code won't run again until the wifi code sets the boolean true again.
xSemaphoreGive(mutexReceive); give up the mutex, the code's done using the variables that might be in conflict
} close bracket of mutex checking if
} close bracket of new data flag checking if - the section of code connecting to the wifi task is done.
robotEnabled = enable; set robotEnabled (whether the robot should actually enable) equal to enable (variable from DS of whether it wants the robot to enable).
The following section of code checks to see if the robot has fallen past its disable threshold and if so sets robotEnabled false and sets the variable tipped to whether the robot has tipped too far. Otherwise tipped is set false because the robot is upright.
if (abs(pitch) > DISABLE_TIP) {
tipped = true;
robotEnabled = false;
} else {
tipped = false;
}
The following code disables the robot if more microseconds (1/1000ths of a second) than the constant WiFiLossDisableIntervalMillis have passed (as measured by the function millis() which returns the number of milliseconds since the microcontroller started) since a message was last received (comparing to the millis() timestamp recorded in lastMessageTimeMillis).
if (millis() - lastMessageTimeMillis > WiFiLossDisableIntervalMillis) {
robotEnabled = false;
}
if (robotEnabled) {
The following code inside this bracket gets run when the robot is enabled. It's a large section of code.
digitalWrite(LED_BUILTIN, (millis() % 500 < 250));
This line blinks the led to show that the robot is enabled - RSL style. See the Arduino reference page for information on the digitalWrite function.
Here's how the light is blinked: millis() is always counting up at a steady rate (counting milliseconds since program start), using the % (modulo) operator, which returns the remainder of a division operation, we get a number counting from 0-499 every half second, then we use the less than operator and 500/2==250 so we end up with a boolean that changes between true and false 4 times a second. 1/4 second on, 1/4 second off, repeat.
The following code gets run only the first loop after the robot was disabled.
if (!wasRobotEnabled) { !==not, so if wasRobotEnabled==false. The boolean wasRobotEnabled is used to record whether the robot was enabled last loop-it is set to the current enable state at the end of the loop.
digitalWrite(ENS_PIN, LOW); enables stepper motors a4988 datasheet says LOW enables, HIGH disables
PIDA.SetMode(AUTOMATIC); turn on the Angle PID loop
PIDS.SetMode(AUTOMATIC); turn on the Speed PID loop
See the PID reference page for information on the SetMode command from the PID library.
} close the bracket of the if statement above, the section of code that runs once per enabled is finished.
The following lines of code set the PID tuning constants of each PID loop. The tuning values need to keep being set so that they are updated if new values are sent from the driverstation. The library supports live updating of the PID constants, which makes it faster to tune the robot.
PIDA.SetTunings(kP_angle, kI_angle, kD_angle);
PIDS.SetTunings(kP_speed, kI_speed, kD_speed);
The following lines call the Compute() function for the two PID loops. Please see the PID reference page for more information on the Compute() command from the PID library and how it is used here.
The compute function runs the PID calculation of how the output value should be changed to try to get the input value closer to the setpoint.
PIDA.Compute();
The angle controlling PID changes the motorAccel variable that the PID loop was set up with earlier. The output needs to be acceleration because that's how to change the tip angle of a self balancing robot.
PIDS.Compute();
The speed controlling PID changes the targetPitch variable as its output, and that is the input to the angle controlling loop.
motorSpeed += constrain(motorAccel, -MAX_ACCEL, MAX_ACCEL);
This line adds motorAccel to the current motorSpeed after limiting it to within MAX_ACCEL It's the acceleration to speed conversion.
motorSpeed = constrain(motorSpeed, -MAX_SPEED, MAX_SPEED);
This line limits the range of motorSpeed to within MAX_SPEED as a motor can't accelerate to unlimited speed.
leftMotorWriteSpeed = constrain(motorSpeed + turnSpeedVal, -MAX_SPEED, MAX_SPEED);
combine turnSpeedVal and the motor speed required for forwards/backwards movement and set the speed for the left motor so the robot can move and turn and again limit the speed to within the motor's limits
rightMotorWriteSpeed = constrain(motorSpeed - turnSpeedVal, -MAX_SPEED, MAX_SPEED); just like for the left motor's speed, but as a positive turnSpeedVal means a turn to the right requiring the right wheel to slow down, turnSpeedVal is subtracted from motorSpeed instead of added like for the left motor.
The following lines set the pins telling the stepper drivers which way to turn the motor.
if (leftMotorWriteSpeed >= 0) {
digitalWrite(LEFT_DIR_PIN, HIGH);
} else {
digitalWrite(LEFT_DIR_PIN, LOW);
}
if (rightMotorWriteSpeed >= 0) {
digitalWrite(RIGHT_DIR_PIN, HIGH);
} else {
digitalWrite(RIGHT_DIR_PIN, LOW);
}
The following lines adjust the delay of the timers controlling the step pulses so they pulse however many times per second the motorWriteSpeed variables call for.
if (abs(leftMotorWriteSpeed) >= 1) { if the left motor speed is not zero, run the next line that will make the motor move.
timerAlarmWrite(leftStepTimer, 1000000 / abs(leftMotorWriteSpeed), true); This function sets how many timer ticks should go by between the alarm triggering its attached function. The timer ticks at 1MHz (milliseconds, 1000000 times per second) so taking 1000000 and dividing it by how many times per second the motor should step gives a time in milliseconds for the timer.
} else { if the motor shouldn't move
timerAlarmWrite(leftStepTimer, 1e17, true); don't step (technically this doesn't tell it to stop stepping, it tells it to step once every three thousand years)
}
same for right motor
if (abs(rightMotorWriteSpeed) >= 1) {
timerAlarmWrite(rightStepTimer, 1000000 / abs(rightMotorWriteSpeed), true); // 1Mhz / # = rate
} else {
timerAlarmWrite(rightStepTimer, 1e17, true); // don't step
}
The next lines make the timers run, but luckily don't reset the counting.
timerAlarmEnable(leftStepTimer);
timerAlarmEnable(rightStepTimer);
} else {
This is the end of the code that runs conditionally when the robot is enabled. The following code runs when the robot is disabled and turns off the motors
digitalWrite(LED_BUILTIN, HIGH);
RSL solid on to signal powered on and disabled
PIDA.SetMode(MANUAL);
Turn off angle PID loop because we don't want to control the motors while disabled. See the PID reference for information on the PID library and this function.
PIDS.SetMode(MANUAL);
Turn off speed PID loop.
timerAlarmWrite(leftStepTimer, 1e17, true);
Make the timer (almost) never trigger a step
timerAlarmWrite(rightStepTimer, 1e17, true);
Make the timer (almost) never trigger a step
timerAlarmEnable(leftStepTimer); Makes the timer run at the set rate. As the motors get disabled it doesn't matter what the timers are set to and it matters even less if they actually are enabled to step.
timerAlarmEnable(rightStepTimer);
leftMotorWriteSpeed = 0;
Reset the leftMotorWriteSpeed to zero so the motors start from stopped when the robot is restarted.
rightMotorWriteSpeed = 0;
Reset right, just like left. The angle PID loop controls acceleration, never speed, and the robot's motors will be stopped when it first gets enabled, so the speed value should be reset.
targetPitch = 0;
Reset the targetPitch variable to 0.
motorAccel = 0;
Reset the motorAccel variable to 0.
motorSpeed = 0;
Set the motorSpeed variable (input from DS) to 0.
Some of those variable resets were probably unnecessary, but harmless.
digitalWrite(ENS_PIN, HIGH);
disable the a4988 stepper drivers so power to the motors is turned off
} end of code run conditionally if disabled
wasRobotEnabled = robotEnabled; record whether the robot was enabled for the next loop
} end of void loop() function!
This function gets called by the wifi code, and is where functions for adding variables to a buffer of bytes to send go.
byte counter = 0;
This counter variable is used to keep track of what element of the array to be sent over wifi should be the starting point for adding the next variable.
To have a variable sent, use the function corresponding to the variable type and pass the variable and the counter in. addBoolToBuffer sends a boolean, addByteToBuffer sends a byte, addIntToBuffer sends an int, and addFloatToBuffer sends a float. See the wifi tab for the declarations of these functions. See the variable types page for information on different variable types.
addBoolToBuffer(robotEnabled, counter);
addBoolToBuffer(tipped, counter);
addByteToBuffer(ROBOT_ID, counter);
addByteToBuffer(MODEL_NO, counter);
addFloatToBuffer(pitch, counter);
addByteToBuffer(voltage, counter);
addIntToBuffer(leftMotorWriteSpeed, counter);
addIntToBuffer(rightMotorWriteSpeed, counter);
addFloatToBuffer(targetPitch, counter);
addFloatToBuffer(pitchOffset, counter);
addByteToBuffer(numSendAux, counter); how many bytes of extra data will be sent from auxSendArray
for (int i = 0; i < numSendAux; i++) { send numSendAux number of bytes from auxSendArray See the Arduino reference page for the for loop for more information on that command.
addByteToBuffer(auxSendArray[i], counter); send extra data in auxSendArray
} end of for loop
if (saverecallState == 1) { a 1 for saverecallState is a signal from the DS that it wants to be told the values of the settings the robot has stored to EEPROM.
recallSettings(); call a function, defined in the saverecall tab that reads the settings out of EEPROM
Send all the setting variables:
addBoolToBuffer(true, counter);
addFloatToBuffer(kP_angle, counter);
addFloatToBuffer(kI_angle, counter);
addFloatToBuffer(kD_angle, counter);
addFloatToBuffer(kP_speed, counter);
addFloatToBuffer(kI_speed, counter);
addFloatToBuffer(kD_speed, counter);
} else { If saverecallState does not equal 1...
addBoolToBuffer(false, counter);
}
return counter; This function returns the number of bytes used back to the code that called it.
} End of function for collecting bytes to send
void parseDataReceived() { This function is where functions that read variables out of the wifi buffer should go. It gets called from the wifi tab.
The function names are pretty self explanatory, use the one for the variable type you want to read and give it the counter so it knows where to read from (It increments the counter however many bytes it reads (however many bytes the variable is)).
byte counter = 0; make a counter variable equal 0 to count byte array locations.
enable = readBoolFromBuffer(counter); read from the DS whether it is telling the robot to enable or disable.
speedVal = map(readByteFromBuffer(counter), 200, 0, -MAX_SPEED * DRIVE_SPEED_SCALER, MAX_SPEED * DRIVE_SPEED_SCALER);
The forwards backwards speed control is sent from the DS as a byte with 200=full forwards 100=stopped and 0=full backwards. Maybe this can be fixed later, but currently a negative motor drive speed value is forwards (forwards is direction lean to go positive). The maximum motor speed is scaled down by the drive speed scaler so there's a bit of reserve speed to accelerate into to remain balanced even when the robot is told by the driver station to drive its fastest.
turnSpeedVal = map(readByteFromBuffer(counter), 0, 200, -MAX_SPEED * TURN_SPEED_SCALER, MAX_SPEED * TURN_SPEED_SCALER);
Read the turn control value from the DS (0=turn left 100=straight 200=turn right) and scale it to motor speed.
numAuxRecv = readByteFromBuffer(counter); read how many bytes of control data to expect to recieve next for extra things
The following code reads numAuxRecv number of bytes and puts them in the auxRecvArray to be used.
for (int i = 0; i < numAuxRecv; i++) {
auxRecvArray[i] = readByteFromBuffer(counter);
}
if (readBoolFromBuffer(counter)) { If the driverstation says it will send new PID setting values...read the values and update the settings.
kP_angle = readFloatFromBuffer(counter);
kI_angle = readFloatFromBuffer(counter);
kD_angle = readFloatFromBuffer(counter);
kP_speed = readFloatFromBuffer(counter);
kI_speed = readFloatFromBuffer(counter);
kD_speed = readFloatFromBuffer(counter);
}
saverecallState = readByteFromBuffer(counter); Read the byte used by the DS to signal what to do about saving or recalling settings from EEPROM.
if (saverecallState == 2) { 2=save settings to EEPROM
saveSettings(); This function, defined in the saverecall tab, writes all setting values to EEPROM.
}
} the end of the parseDataReceived function, and the end of the ursa tab!!
void IRAM_ATTR onLeftStepTimer() { This function is the Interrupt Service Routine attached to the left stepper timer that the timer calls when it's time to make the left motor step. IRAM_ATTR makes the code for this function get stored in RAM instead of Flash memory like most of the program instructions - This means the function is very quickly accessible to the cpu so the interrupt routine can run quickly.
The following lines of code send a pulse from the step pin to the a4988 motor driver to signal it to step. First the pin is pulled high, then there needs to be a short pause for the pulse to register with the driver and then the pin is pulled back to low. A full microsecond is more than what's required for the driver, but it is difficult to make a shorter delay. A delay in an interrupt routine is bad practice, but it is effective and simple.
digitalWrite(LEFT_STEP_PIN, HIGH);
delayMicroseconds(1);
digitalWrite(LEFT_STEP_PIN, LOW);
} end of left step ISR
void IRAM_ATTR onRightStepTimer() { right step ISR
digitalWrite(RIGHT_STEP_PIN, HIGH);
delayMicroseconds(1);
digitalWrite(RIGHT_STEP_PIN, LOW);
}
void setupStepperTimers() { This function gets called on startup in void setup() and contains the code for setting up and starting the stepper pulse timers.
leftStepTimer = timerBegin(2, 80, true); Initialize the left timer
timerBegin takes three parameters: what number hardware timer to use (0-3), what "prescaler" to devide the 80Mhz clock rate by to get the timer rate used later when setting how many ticks to wait before triggering the timer, and true, we want the timer to repeat.
rightStepTimer = timerBegin(3, 80, true); Initialize the right timer with timer 3 // 80Mhz / 80 = 1Mhz, 1microsecond
The following lines attach the timers to the functions to be called when the timers trigger.
timerAttachInterrupt(leftStepTimer, &onLeftStepTimer, true);
timerAttachInterrupt(rightStepTimer, &onRightStepTimer, true);
Setup the timers to trigger, but for now with a delay that is practically never, and enable them.
timerAlarmWrite(leftStepTimer, 1e17, true); // 1Mhz / # = rate // practically never
timerAlarmWrite(rightStepTimer, 1e17, true); // 1Mhz / # = rate
timerAlarmEnable(leftStepTimer);
timerAlarmEnable(rightStepTimer);
} the end of the setupStepperTimers function and the end of the stepper tab!
The mpu tab contains the code for setting up, calibrating, and reading the accelerometer and gyroscope in the mpu6050 orientation sensor and calculating a pitch angle.
void setupMPU6050() { This function starts I2C communication to the mpu6050 sensor and sends some commands to set up the sensor.
Wire.begin(); begin communication channel
Wire.setClock(400000L); send data at a faster clock speed
A command is set by beginning a transmission, writing a byte (written here in hexadecimal) to signal what register should be changed and then sending a new register value.
reference for the mpu6050's registers
The sensor's I2C address is 0x68, this is used in beginTransmission
wake up the sensor:
Wire.beginTransmission(0x68); //begin communication with sensor with address 0x68
Wire.write(0x6B); //which register
Wire.write(0); //setting value
Wire.endTransmission(true);
set the gyroscope range to 1000 deg/sec 0x10=1000 0x18=2000
Wire.beginTransmission(0x68);
Wire.write(0x1B); //register
Wire.write(0x10); //value
Wire.endTransmission(true);
Set the sensor's clock divider
Wire.beginTransmission(0x68);
Wire.write(0x19);
Wire.write(0x02);
Wire.endTransmission(true);
Change sensor's buffering setting
Wire.beginTransmission(0x68);
Wire.write(0x1A);
Wire.write(0x00);
Wire.endTransmission(true);
} end of mpu setup function
void readMPU6050() {
This function gets called every loop, and reads the mpu6050 sensor's accelerometer and gyroscope, and calculates a pitch value.
Wire.beginTransmission(0x68); start communication to the mpu6050
Wire.write(0x3B); send address location for first byte of orientation data
Wire.endTransmission(false); next data will be asked for from the previously sent address
Wire.requestFrom(0x68, 14, true); ask the MPU6050 for accelerometer and gyroscope data bytes. With accel and gyro each having 3 axes, that is 6 values, each value is a two byte integer, and there's two more bytes for a temperature reading that is stuck in the middle- request 14 bytes.
accelerationX = Wire.read() << 8 | Wire.read(); read two bytes and put them together into a sixteen bit integer value
accelerationY = Wire.read() << 8 | Wire.read(); read next two bytes and put together y accel value
accelerationZ = Wire.read() << 8 | Wire.read();
Wire.read(); Wire.read(); read and throw away temperature bytes, it's annoying they put it in the middle here
rotation (gyroscope) values now:
rotationX = Wire.read() << 8 | Wire.read();
rotationY = Wire.read() << 8 | Wire.read();
rotationZ = Wire.read() << 8 | Wire.read();
In the following lines, a rotationOffset value that was found for each axis during calibration is subtracted from the raw read value. This makes 0 acually correspond to no rotation so there's less gyro drift. The rotation value is then multiplied by a scaler to convert the raw number to Degrees Per Second.
rotationDPS_X = (rotationX - rotationOffsetX) * 1000.00 / 32766;
rotationDPS_Y = (rotationY - rotationOffsetY) * 1000.00 / 32766;
rotationDPS_Z = (rotationZ - rotationOffsetZ) * 1000.00 / 32766;
if (micros() < lastCalcedMPU6050) { micros() can "overflow" if the number of microseconds since the program started is more than the variable storing the value can handle. On a standard arduino it takes 1.5hrs. lastCalcedMPU6050 records the micros value of the last time the sensor was read. If micros() < lastCalcedMPU6050 micros must have overflowed, since it usually always increases.
lastCalcedMPU6050 = micros() - 10000; If micros has overflowed, lastCalcedMPU6050 is set to 10000 microseconds (1/100th of a second) before the current reading of micros which is harmless compared to ending up with a huge negative number when calculating the interval between an overflowed micros and an about to overflow lastCalcedMPU6050.
}
The following formula for calculating pitch combines gyro and accelerometer tilt data in a way that takes advantage of short term accuracy of the gyro and long term accuracy of the accelerometer, using a "complementary filter." In the short term, the pitch angle changes at the rate measured from the gyro, but in the long term the angle measured by the accelometer (by measuring the direction of gravity) has enough effect to cancel drift that would happen with just the gyro and keep the pitch angle calibrated.
pitch = COMPLEMENTARY_FILTER_CONSTANT * ((pitch) + rotationDPS_X * (micros() - lastCalcedMPU6050) / 1000000.000)`
+ (1 - COMPLEMENTARY_FILTER_CONSTANT) * (degrees(atan2(accelerationY, accelerationZ)) - pitchOffset);
The first part of the formula calculates how many degrees would have been turned in the amount of time since the last reading at the measured rate of rotation, and adds that to the current pitch variable; integrate the rotation rate over time to get rotation. The second part of the formula uses inverse tangent to calculate pitch from the direction of acceleration of gravity; pitchOffset is subtracted here to account for the mpu6050 not necessarily being perfectly aligned with what angle the robot balances at-the gyroscope only measures changes in angle, as it's not absolute, it doesn't need the offset. The first and second parts of the formula are added together after being scaled by weights that add to one. The gyroscope part has the largest effect on pitch over a single reading, but over time the accelerometer part adds up and makes a difference.
The next section of code adjusts pitchOffset so that when pitch == 0 the robot is balanced.
if (robotEnabled) { pitchOffset adjustments should only be done when the robot is enabled, since when it's disabled it isn't balancing.
pitchOffset = (pitch + pitchOffset) * (1 - PITCH_OFFSET_CHANGE) + pitchOffset * (PITCH_OFFSET_CHANGE); slowly move (adding two values weighted with scalers that add to one) pitchOffset towards the current pitch value, the overall average pitch value should be the balance point.
}
This current method has issues and makes the robot not always move at the right speed since the robot is tipping when it should be balanced.
lastCalcedMPU6050 = micros(); Record time (in microseconds) of last calculation so we know next time how much time has passed (how much time to integrate rotation rate for).
} end of readMPU6050 function
void zeroMPU6050() { find how much offset each gyro axis has to zero out drift. should be run on startup (when robot is still)
The first part of zeroing the sensor is making sure the robot is still so a gyro drift offset can be calculated accurately. The amount of jitter in the gyro readings is calculated, and while there is more movement than allowed by a theshold the looping code won't exit and will just wait for the robot to be held still.
do { a do...while() command is like if() but even if the condition in while() is false, the code in do is run once.
The following code uses the exact same method as earlier to read acceleration and rotation data from the MPU6050, except the rotation values are put into new variables that will be used to compare old and new rotation values to test for change. The following code gets a starting value for lastrotation so the following loop has something to compare to on its first iteration.
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(0x68, 14, true);
accelerationX = Wire.read() << 8 | Wire.read();
accelerationY = Wire.read() << 8 | Wire.read();
accelerationZ = Wire.read() << 8 | Wire.read();
Wire.read(); Wire.read(); // throw away temperature
int16_t lastrotationX = Wire.read() << 8 | Wire.read();
int16_t lastrotationY = Wire.read() << 8 | Wire.read();
int16_t lastrotationZ = Wire.read() << 8 | Wire.read();
rotationOffsetX = 0;
rotationOffsetY = 0;
rotationOffsetZ = 0;
for (int i = 0; i < movementMeasurements; i++) { the following code sums the change in rotation speed movementMeasurements number of times before checking to see if it's over the threshold.
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(0x68, 14, true);
accelerationX = Wire.read() << 8 | Wire.read(); // acceleration reads aren't used at the moment
accelerationY = Wire.read() << 8 | Wire.read();
accelerationZ = Wire.read() << 8 | Wire.read();
Wire.read(); Wire.read(); // throw away temperature
rotationX = Wire.read() << 8 | Wire.read();
rotationY = Wire.read() << 8 | Wire.read();
rotationZ = Wire.read() << 8 | Wire.read();
The difference between the just read values and the last read values is summed and added into the rotationOffset variables.
rotationOffsetX += abs(rotationX - lastrotationX);
rotationOffsetY += abs(rotationY - lastrotationY);
rotationOffsetZ += abs(rotationZ - lastrotationZ);
and then the current values are saved in lastrotation for next loop
lastrotationX = rotationX;
lastrotationY = rotationY;
lastrotationZ = rotationZ;
digitalWrite(LED_BUILTIN, i % 2); flash the led while the loop keeps going so the user knows it is still starting up. i % 2 alternates between 0 and 1 each interation of the for loop
delay(15); A small delay separates the readings in time a bit so there's time for the sensor to update.
} end of for loop
} while (abs(rotationOffsetX) > movementThreshold * movementMeasurements || abs(rotationOffsetY) > movementThreshold * movementMeasurements || abs(rotationOffsetZ) > movementThreshold * movementMeasurements);
while any of the three rotationOffset sums are higher than movementThreshold * movementMeasurements (more movement than movementThreshold on average per measurement), the code in do is repeated.
Now the measurement of the actual rotationOffset values can be done since the robot is known to be still. The variables need to be reset to 0 before the average of many readings is taken.
rotationOffsetX = 0;
rotationOffsetY = 0;
rotationOffsetZ = 0;
digitalWrite(LED_BUILTIN, LOW); Set the light off for the final zeroing stage to indicate that the robot has been held still enough.
for (int i = 0; i < 50; i++) { Run the following code 50 times so we can get many measurements to average into an offset value.
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(0x68, 14, true);
accelerationX = Wire.read() << 8 | Wire.read();
accelerationY = Wire.read() << 8 | Wire.read();
accelerationZ = Wire.read() << 8 | Wire.read();
Wire.read(); Wire.read(); // throw away temperature
rotationX = Wire.read() << 8 | Wire.read();
rotationY = Wire.read() << 8 | Wire.read();
rotationZ = Wire.read() << 8 | Wire.read();
The first part of taking an average is summing all the reads together.
rotationOffsetX += rotationX;
rotationOffsetY += rotationY;
rotationOffsetZ += rotationZ;
delay(10 + i / 5); Add some time between reads, changing the delay each time a bit to be less likely to be thrown by a periodic oscillation. Does it work better? maybe. Is it harmless, does the calibration work overall and does it look fancy? yes.
} end of the for loop summing 50 measurements
To get the average, the sum has to be divided by the number of reads that were taken.
rotationOffsetX /= 50;
rotationOffsetY /= 50;
rotationOffsetZ /= 50;
} the end of the zeroMPU6050 function and the end of the mpu tab.
The wifi tab contains the code that lets the robot communicate with the driver station. It has code to set a wifi hotspot network, send and recieve messages from a connected client, and functions to make it easy to use multi-byte variables with the byte array wifi buffers.
void WiFiTaskFunction(void * pvParameters) { The WiFiTaskFunction is a function that is run in a separate task from the main code. The main loop is run on esp32 core #1. The wifi task gets run on core #0 to not interfere with the main loop which needs to run very fast. DuAl COrE pRosSEsOrs WOOT! Core #0 also runs some other sytem tasks relating to wifi, but doesn't seem to get overloaded.
while (true) { this while (true) makes and infinite loop in which to run code that checks for new wifi data and reads it out then replies. Without the infinite loop, this function would only run once and then end, like void setup. We're basically making a second void loop that runs on the other processor.
int packetSize = Udp.parsePacket(); Udp.parsePacket() is a function from the Udp wifi library that looks for whether there is a recieved packet that can be prepared for reading and returns the size of the packet of data.
if (packetSize) { if there is data to read, run the following code.
if (xSemaphoreTake(mutexReceive, 1) == pdTRUE) { You saw the other side of this code in the ursa tab. Because wifi data has to be sent between tasks, there is a risk of both accessing the same resource at one time and causing a conflict. This line prevents that. Before putting the recieved data in an array a mutex is used to make sure the variable isn't in use.
receivedNewData = true; There's code in void loop that when this boolean is set true reads the array of wifi data and changes DS controlled variables.
lastMessageTimeMillis = millis(); record the time the message was recieved so the robot can disable if it has been too long since a signal.
Udp.read(packetBuffer, maxWifiRecvBufSize); This function reads the data packet recieved over wifi and puts it into the packetBuffer array.
for (int i = 0; i < maxWifiRecvBufSize; i++) { go through the packetBuffer array which is made of chars (8 bits, signed so -128 to 127, what the wifi library uses) and convert to bytes 0-255.
recvdData[i] = (byte)((int)(256 + packetBuffer[i]) % 256); nasty looking but this conversion works
}
Udp.beginPacket(); when a signal is recieved, send a response. the beginPacket command from the UDP library is necessary before giving data to send.
for (byte i = 0; i < numBytesToSend; i++) { loop through the array for however many bytes need to be sent
Udp.write(dataToSend[i]); using Udp.write adds data to what will get sent over wifi
}
Udp.endPacket(); this command sends the data over wifi
xSemaphoreGive(mutexReceive); release control over wifi variables
}
}
}
} end of WiFiTaskFunction
void setupWifi() { This function gets called by void setup and sets up the wifi access point (hotspot network), starts the communication library, and starts the wifi task.
WiFi.softAPConfig(IPAddress(10, 25, 21, 1), IPAddress(10, 25, 21, 1), IPAddress(255, 255, 255, 0)); Configure the esp32 to have an IP address of 10.25.21.1
WiFi.softAP(robotSSID, robotPass, 4, 0, 1); start wifi network, with the name and password set up early in void setup, on channel 4, not hiding, and only allowing one client/DS
Udp.begin(2521); start UDP server on port 2521
xTaskCreatePinnedToCore( create task to run WiFi task that will run on core #0
WiFiTaskFunction, Function to implement the task (see above for definition of this function)
"WiFiTask", Name of the task
15000, Stack size in words
NULL, Task input parameter
0, Priority of the task. I know 0 is a very low priority. When I raised the priority I got watchdog errors, when I took the suggestion of adding a vTaskDelay into the `WiFiTaskFunction it worked better, but I would be driving the robot and every 10-40 seconds the robot would fall over because the code crashed. I changed back to a priority of 0 and everything works well now so please just leave it alone.
NULL, Task handle.
0 Core on which task should run
);
} end of wifi setup function
Next are the functions for reading and writing various kinds of variables into an array of bytes. These functions are used in createDataToSend() and parseDataRecieved() to easily send and recieve variables.
You may want to review variable types before continuing.
The following function reads the value stored in the array recvdData at the position pos converts to a boolean and returns it from the function.
boolean readBoolFromBuffer(byte &pos) { declare the function readBoolFromBuffer as returning a boolean variable and taking the input of a byte. The input byte is used to keep track of what position in the array the reading is up to. Pointers are used here so that the function gets the memory location of the input variable, not just the value so that the variable can be changed by the function.
byte msg = recvdData[pos]; read the byte at the location pos and store it in a new variable
pos++; You read a byte so increment the pos variable so when the position counter is passed into the next read function it reads the next value not just the first array value over and over.
return (msg == 1); if the read value is 1, return a boolean of true, otherwise (if it was set to 0) return false. A function returning a variable, kind of makes itself equal to that variable. Like in math f(x) could equal a number.
} end of read boolean function
The following function reads the value stored in the array recvdData at the position pos and returns the byte from the function.
byte readByteFromBuffer(byte &pos) { declare a function readByteFromBuffer as returning a byte variable and taking the input of a byte. Again a pointer is used for the counter variable.
byte msg = recvdData[pos]; read a byte out of the byte array at position pos
pos++; You read a byte so increment thepos` variable so when the position counter is passed into the next read function it reads the next value not just the first array value over and over.
return msg; have this function return the byte it read from the array
}
The following function reads 4 values stored in the array recvdData starting at the position pos and assembles and returns an int type value from the function.
int readIntFromBuffer(byte &pos) { declare a function readIntFromBuffer as returning an int variable. Again using a pointer for counter value.
This function has some added complexity. An int type variable on an esp32 is made of 4 bytes. A union lets us translate between two variable types (equal size, but one's four bytes in an array, and one's a four byte int). Here's a great reference for unions.
union {
byte b[4];
int v;
} d;
d is the union, d.b acceses the byte array, d.v acceses the int
for (int i = 0; i < 4; i++) { loop through the 4 bytes, read 4 bytes from the array and fill the 4 byte union array
d.b[i] = recvdData[pos]; read the byte at the current position in pos and put it into the union's byte array.
pos++; step the counter forward to get the next position in the array next time around.
} close for
return d.v; return the int form of union d. Values of an array have been converted into a variable.
}
The following function reads 4 values stored in the array recvdData starting at the position pos and assembles and returns a float type value from the function.
float readFloatFromBuffer(byte &pos) { declare a function readFloatFromBuffer as returning a float variable.
union { translate between two variable types (equal size, but one's 4 bytes in an array, and one's a 4 byte float)
byte b[4];
float v;
} d;
d is the union, d.b acceses the byte array, d.v acceses the float
Step through 4 times reading bytes and filling the union's array
for (int i = 0; i < 4; i++) {
d.b[i] = recvdData[pos];
pos++;
}
return d.v; return the float form of union d
}
void addBoolToBuffer(boolean msg, byte &pos) { add a boolean to the dataToSend array, it takes in a boolean value to add, and a pointer to a variable to count position in the array
dataToSend[pos] = msg; setting an element of a byte array equal to a boolean apperently works fine. true==1
pos++; increment the counter variable
}
void addByteToBuffer(byte msg, byte &pos) { add a passed in byte to the dataToSend array at the position of pos
dataToSend[pos] = msg;
pos++;
}
void addIntToBuffer(int msg, byte &pos) { add an int to the dataToSend array (four bytes)
union { using a union to translate between 4 bytes and an int
`byte b[4];`
`int v;`
} d; d is the union, d.b acceses the byte array, d.v acceses the int
d.v = msg; // put the value into the union as an int
for (int i = 0; i < 4; i++) {
dataToSend[pos] = d.b[i];
pos++;
}
read 4 bytes out of the union and put them in the dataToSend array, increasing the counter once each byte
}
void addFloatToBuffer(float msg, byte &pos) { add a float to the dataToSend array (four bytes)
union { this lets us translate between two variables (equal size, but one's 4 bytes in an array, and one's a 4
byte float
`byte b[4];`
`float v;`
} d; d is the union, d.b acceses the byte array, d.v accesses the float
d.v = msg;
for (int i = 0; i < 4; i++) {
dataToSend[pos] = d.b[i];
pos++;
}
read the 4 bytes out of the union and put them in dataToSend
} This is the end of the wifi tab
The saverecall tab contains the code for saving and reading settings to EEPROM.
void recallSettings() { This function should be called when settings should be read out of EEPROM.
byte counter = 0; set counter to zero, like in the wifi tab, there is a counter to keep track of where to read from the array next, the read functions increase the counter.
Use EEPROMread functions (see later in this tab) to read settings and update variables
kP_angle = EEPROMreadFloat(counter);
kI_angle = EEPROMreadFloat(counter);
kD_angle = EEPROMreadFloat(counter);
kP_speed = EEPROMreadFloat(counter);
kI_speed = EEPROMreadFloat(counter);
kD_speed = EEPROMreadFloat(counter);
pitchOffset = EEPROMreadFloat(counter);
}
void saveSettings() { This funciton writes settings into memory that lasts when the robot is turned off.
byte counter = 0; set counter variable to zero, each function increases the counter by how many bytes it reads so each read moves through the array
Use EEPROMwrite functions (defined later in this tab) to easily add save variables to EEPROM(Flash) memory.
EEPROMwriteFloat(kP_angle, counter);
EEPROMwriteFloat(kI_angle, counter);
EEPROMwriteFloat(kD_angle, counter);
EEPROMwriteFloat(kP_speed, counter);
EEPROMwriteFloat(kI_speed, counter);
EEPROMwriteFloat(kD_speed, counter);
EEPROMwriteFloat(pitchOffset, counter);
EEPROM.commit(); this EEPROM library function writes to flash memory so the settings stay even when the robot is turned off. Writing to flash memory should only happen when new settings have to be saved.
}
The following functions read and write variables to memory. They are very similar to the wifi read and write functions since both the wifi buffer arrays and EEPROM memory are arrays of bytes. See the wifi tab for explanations of how the variables are split into bytes and reconstructed from bytes.
EEPROM.read(pos) returns the byte stored at position pos
boolean EEPROMreadBoolean(byte &pos) {
byte msg = byte(EEPROM.read(pos));
pos++;
return (msg == 1);
}
Just like with wifi, a pointer is used so the function can increase the counter variable that gets passed into the next read function so the position moves through the array.
byte EEPROMreadByte(byte &pos) {
byte msg = byte(EEPROM.read(pos));
pos++;
return msg;
}
int EEPROMreadInt(byte &pos) {
union {
byte b[4];
int v;
} d;
for (int i = 0; i < 4; i++) {
d.b[i] = byte(EEPROM.read(pos));
pos++;
}
return d.v;
}
float EEPROMreadFloat(byte &pos) {
union {
byte b[4];
float v;
} d;
for (int i = 0; i < 4; i++) {
d.b[i] = byte(EEPROM.read(pos));
pos++;
}
return d.v;
}
EEPROM.write(pos,msg); puts byte msg in memory at positon pos
void EEPROMwriteBool(boolean msg, byte &pos) {
EEPROM.write(pos, msg);
pos++;
}
void EEPROMwriteByte(byte msg, byte &pos) {
EEPROM.write(pos, msg);
pos++;
}
void EEPROMwriteInt(int msg, byte &pos) {
union {
byte b[4];
int v;
} d;
d.v = msg;
for (int i = 0; i < 4; i++) {
EEPROM.write(pos, d.b[i]);
pos++;
}
}
void EEPROMwriteFloat(float msg, byte &pos) {
union {
byte b[4];
float v;
} d;
d.v = msg;
for (int i = 0; i < 4; i++) {
EEPROM.write(pos, d.b[i]);
pos++;
}
}
This is the end of the saverecall tab.