Previous lab. Return home. Next lab.
Just a forewarning, there are a LOT of code snippets. I hope that doesn't turn out to be a problem.
Compiling takes quite a long time, and connecting and disconnecting the USB-C port to and from my robot isn't the easiest task. With this, and the suggestion from the lab handout website, I decided to quickly implement a way to do this with a command named CHANGE_PID_VALUE. A snippet of that is below.
// Assign the value to the proper variable
if( strcmp( parameter, "p") == 0 ) {
// Set p to value
pid_values.p = new_pid_value;
Serial.print( "p set to " );
Serial.println( new_pid_value );
} else if( strcmp( parameter, "i" ) == 0 ) {
// Set i to value
pid_values.i = new_pid_value;
Serial.print( "i set to " );
Serial.println( new_pid_value );
} else if( strcmp( parameter, "d" ) == 0 ) {
// Set d to value
pid_values.d = new_pid_value;
Serial.print( "d set to " );
Serial.println( new_pid_value );
} else {
Serial.print( "Received: " );
Serial.print( parameter );
Serial.println( ", but expected: p, i, or d. Failed at assignment.");
}
The Python command to set the values looks like the following.
ble.send_command( CMD.CHANGE_PID_VALUE, "p|10.0" )
ble.send_command( CMD.CHANGE_PID_VALUE, "i|1.0" )
ble.send_command( CMD.CHANGE_PID_VALUE, "d|0.5" )
Later on, I implemented a lot of commands like this, like maximum motor speed, PID duration, whether or not I want to autosend data after each PID (this is because it takes so long to send data sometimes, and I just can't test quickly enough if I have to wait every time. I figure, if there is good data, I will send it myself), change the setpoint, stop the motors and stop PID, start PID, and send PID data.
To send and receive data over Bluetooth, I basically took exactly what I did in previous labs and adapted it to the PID controller data. This ended up looking like the following. This is run every loop, but is almost always skipped, because the send_pid_data flag is almost never on, only once per run. So, I first check if the send_pid_data flag is on. If it is, then we first start by looping through the distance array, sending back all of the gathered entries. The distance array just holds only the distance readings and the timestamp attached to each one. This could likely be wrapped into the PID array, but they are written to at different frequencies, and it is not very much a problem I have to think about right now. I have ample space, and it is written to much less frequently than the PID array, plus each entry is smaller, so it is unlikely to be an issue in the immediate future.
I then loop through, sending the various things relating to the PID data, so the current distance to the setpoint, the u(t) value being sent to the motors, the time difference between the two previous readings (from the sensors or extrapolated), the distance difference between the previous to readings (from the sensors or extrapolated), the absolute time in milliseconds, and the time elapsed from the start of the PID session.
Then, we set the send_pid_data flag to false so we don't enter back into this if block and thusly into a data-sending loop each time we loop. Lastly, we set the array indicies for both the distance and PID arrays to 0, so that we effectively will overwrite the data next time we collect, and effectively we now have deleted the data for us to store more.
// If we want to send back PID data
if( send_pid_data ) {
// Send back the distance
for( int i = 0; i < front_distance_entries_gathered; i++ ) {
tx_estring_value.clear();
tx_estring_value.append( "Front Distance: " );
tx_estring_value.append( front_distance_array[i].distance );
tx_estring_value.append( " Time: " );
tx_estring_value.append( front_distance_array[i].time );
tx_characteristic_string.writeValue( tx_estring_value.c_str() );
Serial.println( tx_estring_value.c_str() );
}
for( int i = 0; i < pid_entries_gathered; i++ ) {
tx_estring_value.clear();
tx_estring_value.append( "setpoint_distance: " );
tx_estring_value.append( pid_array[i].setpoint_distance );
tx_estring_value.append( " pid_u: " );
tx_estring_value.append( pid_array[i].pid_u );
tx_estring_value.append( " time_difference: " );
tx_estring_value.append( pid_array[i].time_difference );
tx_estring_value.append( " distance_difference: " );
tx_estring_value.append( pid_array[i].distance_difference );
tx_estring_value.append( " time: " );
tx_estring_value.append( pid_array[i].time );
tx_estring_value.append( " pid_time: " );
tx_estring_value.append( pid_array[i].pid_time );
tx_characteristic_string.writeValue( tx_estring_value.c_str() );
Serial.println( tx_estring_value.c_str() );
}
// We don't want to get stuck in a horrible Bluetooth loop
send_pid_data = false;
// When we are done sending the data, we need to essentially wipe the array clean by just resetting the index, or the front_distance_entries_gathered = 0
front_distance_entries_gathered = 0;
pid_entries_gathered = 0;
}
Additionally, as I had previously mentioned, I implemented a setting via a command to allow me to choose to autosend data or not. This looks like the following.
/*
* SET_PID_AUTOSEND
*/
case SET_PID_AUTOSEND:
// Parse to find the value to be set
int new_pid_autosend_value;
success = robot_cmd.get_next_value( new_pid_autosend_value );
if( !success ) {
Serial.println( "Expected an int. Failed at parsing." );
return;
}
...
// Assign the new autosend value
if( new_pid_autosend_value == 0 ) {
autosend_pid = false;
} else {
autosend_pid = true;
}
...
break;
It is by default on, but I will almost always send the command to turn it off and just explicitly send command to send the data to myself. The command to explicitly send the data is pretty straighforward and looks like the following.
/*
* SEND_PID_DATA
*/
case SEND_PID_DATA:
// Simply set the boolean to allow data to be sent
send_pid_data = true;
break;
I saw somebody in lab point the car at their white box. I thought this was absolutely wonderful, because I thought it abhorrent the thought that my car crash full speed into a wall and I have to resolder the distaster that is my car's internals. The box acts as a buffer that can be pushed around, buying me time to grab my car or send a STOP_MOTORS command to the car.
One slightly difficult thing encountered was that my car wasn't driving straight enough to always be pointing at the box, resulting in the car's sensor pointing past the box and the car ramming into the box. To solve this, in Phillips 238, I just found another cardboard box that was similarly light and put it next to my car box. I didn't have this luxury at home, so I just stuck with the white box.
The following, though still a lot of code, is a very slimmed down version of what I implemented. Essentially adding together the different terms to be able to make a value to send to the motors, however, with all the calculations involved in creating the different terms, and saving all the different information for later reference, as well as the logic for making decisions regarding these things.
// If the executing_pid boolean is true, this means we want to attempt to execute pid
if( executing_pid ) {
// We want to stop under the condition that the time range is completed or if the data array is full
if( ((int) millis() - pid_start_time > pid_duration) || (front_distance_entries_gathered >= DISTANCE_ARRAY_SIZE) || (pid_entries_gathered >= PID_ARRAY_SIZE) ) {
... // Things to stop: stopping motors and setting variables
}
// But if neither of those things are true, then we want to continue to gather data and run the PID controller
else {
...
// We first have to check to make sure that we aren't trying to check distance data that doesn't yet exist.
// The robot won't start driving until the front distance sensor gathers at least one data point
if( front_distance_entries_gathered > 0 ) {
int distance_to_set_point = estimated_distance - pid_set_point;
float pid_time_difference_scaled = ( pid_current_reading_time - pid_previous_reading_time ) * 0.001;
pid_integration_accumulator += distance_to_set_point * pid_time_difference_scaled;
float distance_derivative = 0;
// Only if we now have two data entries are we able to accurately estimate the derivative term
if( front_distance_entries_gathered > 1 ) {
pid_array[pid_entries_gathered].distance_difference = estimated_distance - previous_estimated_distance;
distance_derivative = pid_array[pid_entries_gathered].distance_difference;
distance_derivative /= pid_time_difference_scaled;
}
else {
pid_array[pid_entries_gathered].distance_difference = 0;
}
float pid_u = pid_values.p * distance_to_set_point + pid_values.i * pid_integration_accumulator + pid_values.d * distance_derivative;
... // This just saved the data into the arrays
drive_straight( pid_u, 0, max_motor_speed );
}
}
}
All of this can be seen in action in the video below, where it stops at around 290~300 millimeters when trying to maintain 304 millimeters.
The ToF sensor is set to its long range, so about 4m. It is checked every loop, meaning the limiting speed factor is the sensor itself, because the ToF sensor is a fairly slow sensor, on the order of 100ms per reading. However, I will find a more accurate and precise value for that. The other way I can know that my code runs faster is by looking at some of my data from testing my PID controller. I get multiple PID controller value updates per ToF sensor reading. The PID controller runs every loop, meaning that my loop runs faster than the ToF sensors read data.
Now that the limiting speed factor is determined, we can move onto actually testing the speed of the time of flight sensor. I took 88 samples of the ToF sensor. The first reading came in at the 52980 millisecond mark, and the last reading came in at the 61550 millisecond mark, meaning a time duration of 8570 milliseconds for the 88 samples. This means it takes about 97.386 milliseconds per sample, or a sample frequency of about 10.268 Hz.
The lab asked me to make sure my PID controller ran each loop. However, as described in the previous section, my PID controller already runs every loop, independently of the ToF sensor. This is because I implemented the execution of the PID controller using a few booleans. The first is start_pid. What this does is set the executing_pid flag true, the start_pid flag false, grab the initial time, and reset the PID data arrays in the case that I chose not to send the data from the previous run. Then the executing_pid flag is checked every loop. If it is true, then the PID controller will update itself. So, in other words, while PID is running, the PID controller is updating the output to the motors. And then, within the PID controller that runs every loop, it checks the sensor output. What this means is that the ToF sensor is checked every loop cycle (the fastest possible), only when we are running PID because that is the only time we care about the ToF data at this point. This effectively means that they both run at their own fastest-possible paces, divorcing them while still tying them together in so far as it is useful.
In hindsight, I might have considered making some sort of more explicit state machine for this and the rest of my robot. This would clean up the code a lot, instead of just having these horrible boolean flags scattered throughout my code. Perhaps if I have the time at some point this semester, I will find it in myself to restructure my code. However, at this point in time, I think my current code suffices and honestly functions well.
To check the PID loop speed, I will do the same thing I did for the ToF sensor readings. I took a total of 1013 samples from the PID controller. The first data point is at 52984 milliseconds, while the last is at 61620 milliseconds. This gives a range of 8636 milliseconds. This means it takes about 8.525 milliseconds per sample, or about 117.300 Hz. This means the PID loop is running about 11.423x faster than the ToF sensor is gathering data. This means there will be about 10 duplicated samples for every new sample when the PID loop is running. And I'm sure I could speed up the PID loop. The Serial.print() and Serial.println() statements certainly do no service to the PID loop speed. Plus whatever other bloat is leftover in that loop. That's a lot of opportunity wasted for the PID controller to make more decisions.
To get linear extrapolation working, I basically followed what was told me to do in the lab. I implemented it by creating a function, which takes three inputs, and returns the extrapolated distance. It is run every time the distance sensor does not return anything, in the form of an if-else statement, shown below.
The following are defined as global variables.
// Variables for the estimated distance, whether or not we estimate from sensor or estimate from previous readings
int estimated_distance;
int previous_estimated_distance;
The following is in the main loop of the code.
// Check if the data from the sensor is ready to be collected
if( distance_sensor_front.checkForDataReady() ) {
...
// Get the result of the measurement from the sensor
front_distance_array[front_distance_entries_gathered].distance = distance_sensor_front.getDistance();
...
// Set the estimated distance
previous_estimated_distance = estimated_distance;
estimated_distance = front_distance_array[front_distance_entries_gathered].distance;
...
}
// If the data from the sensor is not ready to be collected
else {
// Then we want to extrapolate the data. But we first need to see if there are enough entries. We need two.
if( front_distance_entries_gathered > 1 ) {
previous_estimated_distance = estimated_distance;
estimated_distance = extrapolate_distance_reading( front_distance_array[front_distance_entries_gathered - 2], front_distance_array[front_distance_entries_gathered - 1], (int) millis() );
}
}
The previous estimated distance is used in the calculation of the differential component of u(t). The proportional and integral terms only need the current estimated distance. The extrapolation function is defined as follows.
int extrapolate_distance_reading( distance_reading_t ere_previous_reading, distance_reading_t previous_reading, int current_time ) {
// Calculate the slope given the previous samples
float distance_slope = ( (float) previous_reading.distance - (float) ere_previous_reading.distance ) / ( (float) previous_reading.time - (float) ere_previous_reading.time );
// Calculate the likely distance traveled since previous reading
return ( current_time - previous_reading.time ) * distance_slope;
}
To implement wind-up protection for my integrator, I took the pid_u value, and clamped it at max motor speed. This means that we cannot output to the motors faster than they will go. Max motor speed is theoretically 100, but I have allowed it to be set lower, as I previously mentioned, for the sake of safer testing. If we find we exceed this number, and the car is travelling beyond the setpoint, then we should clamp the integrator to stop growing. If we exceed this number but are going in the correct direction, we won't clamp it. This is shown in the code below.
// Check if the signs for the setpoint distance and the original u value are the same
if( (distance_to_set_point <= 0 && pid_u <= 0) || (distance_to_set_point >= 0 && pid_u >= 0) ) {
pid_signs_equal = true;
}
// Clamp the pid_u value for integral clamping
if( pid_u > max_motor_speed ) {
pid_clamped = true;
pid_u = max_motor_speed;
}
else if( pid_u < -max_motor_speed ) {
pid_clamped = true;
pid_u = -max_motor_speed;
}
// If the integrator term is overpowered and the signs are different (car moving further away from setpoint)
if( !pid_signs_equal && pid_clamped ) {
// Then we should clamp
zero_next_pid_integrator = true;
}
I found I kept writing about things I would have done and would have liked to do, and I honestly think it was kind of distracting at most moments and veered off on a tangent. But I figured since this website is for documentation, and I'm sure y'all would like to hear about these kinds of things, I would just throw this on here. Hopefully I can find it in me to implement even a few of these things in time.
At the time of writing this, I can't yet set a wheel deadband as a function of percentage from 0 to 100, but I might add that. Currently, it is preprogrammed at 5% (with each individual PWM's deadband still accounted for).
Also, honestly, having all of these settings to configure different things makes it more convenient for me. As I chose to have the cover for my robot (plus the LEDs on the top that are soldered to the Artemis, making it inseparable, even), it is admittedly very difficult to keep plugging and unplugging it, so having as much customizable via Bluetooth as I can has made it much easier. I honestly wish I could program it via Bluetooth, but I know that is way outside of the scope of this class.
Maybe a generic change variable function to be able to really customize things would be nice.
For a software reset. I would like to be able to reset the Arduino with a command. It turns out that isn't so straightforward.
https://arduino.stackexchange.com/questions/1477/reset-an-arduino-uno-in-code
https://cdn.sparkfun.com/assets/c/8/3/d/2/Apollo3-Blue-SoC-Datasheet.pdf?_gl=1*15ztg6w*_ga*NTg2MzQ3NTg4LjE3MDYxMjM4MTE.*_ga_T369JS7J9N*MTcwOTY5NDQzOS4xOS4xLjE3MDk2OTYwNjcuNDQuMC4w
https://electronics.stackexchange.com/questions/661349/what-are-w1c-and-r1c-register-access-types
https://dannas.name/2023/04/27/write-one-to-clear