Week of 4/28/25

I didn't do much work on my code this week, but I thought I would make a post explaining my code in detail now that I'm getting closer to finishing my prototype.

Like all code, I start by defining libraries and variables.

These are all the variables I use in my code

The Speed, x, and y variables are all placeholders I used for testing, and aren't important to the code's function.

Accell:

The library for the Accelerometer(which stores angular data) stores the position of the x axis as a separate function; lis.x. In order to able to use this data in my code, I have to create a variable that stores the value of lis.x as an integer(any number that isn't a decimal) This makes it easier to do math using the data from the accelerometer.

DivFactor:

The Accell variable only stores the raw data from the sensor, and using it in my code would make the code extremely sensitive to disturbances and become mostly unusable when tested. DivFactor takes this raw value and divides it by ten.

deg:

where another variable, deg, will convert the divided value to a value between 0 and 360, meaning I'm able to read the accelerometer's output as a full circle (0 to 360 degrees).

error/prev_error:

If the target point of the monorail is to sit at a perfectly flat 180 degrees, then the error is the difference between the target point and the actual position of the rig. If the monorail is leaning at a 45 degree angle, then the code will subtract 180(target point) minus 45(actual point), making the error 135, meaning the rig is 135 degrees away from the target point. When the code completes one cycle, it will set the prev_error variable to the current error, before starting the next cycle and reading a new error. You can subtract the current error from the previous error to determine the speed the rig is rotating at. Think of it as this; if one cycle was completed every second, the code would store the position of the rig as it is right now, and where it was as of a second prior. By subtracting the two, you can see you many degrees the rig has rotated in a one second period. The bigger the difference, the faster the rig is rotating. This is important, as the gyros have to overcompensate to account for the speed of rotation in order to stay level. This is partly how the Derivative component of the PD loop works.

PD Variables:

The kP and kD variables are both responsible for the effectiveness of the Proportional(P) and Derivative(D) components of the PD loop. These constants are added to the sensor readings to determine how much the component will affect the final output. The final outputs of these variables is stored as PD_P and PD_D respectively. The output is determined by combining the component's constant with data from either the sensor readings or error readings.

SpeedInt: Very simple variable. Adds the derivative output to the final output of the PD loop. If the rig is rotating faster, this number will be greater.

time:

Counts the milliseconds that have passed since the code started running. Used to determine the Derivative output.

pos:

Arguably the most important variable. Contains the final cumulative output of all the PD loop's components, and controls the position of the Gyroscopes. period:

The amount of time between each cycle(currently 50).

Setup

The setup of the code is relatively simple, and mainly just makes sure that the accelerometer is working properly and responding(Note: This code is included in the example code provided by the accelerometer's library, I did not write it myself).

When the code first starts, the monitor will print this message;

This just confirms to me that the accelerometer(LID3DH is the model name) is properly installed and working. I also defined my servo motor(line 52), the 'time' variable(line 53) and I set the servo to 90 degrees so it sits straight when the code starts.

Loop

The loop contains the repeated cycle that makes up the PD controller. The code starts by setting the values of a couple of the above mentioned variables(Accell, time) and then sets up the cycle to run every 50 milliseconds.

The cycle goes as follows:

Proportional output:

PD_P is defined as the product of the kP constant multiplied by the current error. This means that it will cause the servo to increase its angle more as the error increases, and to decrease as the error decreases, with the servo settling at 90 degrees when the error is close to zero. This value will increase by a fixed amount of degrees for every 1 degree of error, meaning the ratio is proportional.

Derivative output:

PD_D is defined as the constant(kD) multiplied by the speed(error - previous error) and divided by time(period, or 50 milliseconds). This means that if the rig is rotating faster, the final Derivative output will be higher as well. This value is then added to the proportional output, meaning that the PD loop will react to both the rig's position and its speed of rotation, allowing it to adjust for changes in each and counteract them to keep the rig balanced.

Both the Derivative and Proportional outputs are added and stored in another variable, 'SpeedInt.' Then, this raw output is mapped to a value between 0 and 180 and stored in yet another variable, 'pos.' After this, the value is given to the servo motor, which sets its position equal to the value of 'pos.'

The final step of the cycle is to take the error reading from the beginning of the cycle and store it as the previous error.

This way, when the cycle starts again the code will take a new error reading, while the error it had read in the last cycle will still be stored in a different variable.

It's a little counterintuitive that the previous error is read at the end of each cycle, but think of it this way: when a new error is taken, the current error becomes the previous error, and the reason for taking it at the end is because the value of the variable 'error' is reset at the beginning of each cycle, and so the previous error is taken before that, so it will still be stored when the new cycle starts.

The code has completed one cycle. After 50 milliseconds, a new cycle starts and the servo is moved to a new position.