ROS Arduino Wrapper

The ROS_arduino_wrapper file holds code that is essential to the bike’s balance and navigation. Given that it runs on the Arduino, the ROS wrapper collects and interprets data from sensors that are connected to the Arduino and uses them to inform the navigation algorithm and balance controller.

Sensors

The ROS_arduino_wrapper is used to collect and parse the data received from the sensors on the bike. These sensors include GPS, Hall sensors, IMU, and an Encoder. It is necessary to parse data from these sensors so the Arduino can then send readable data to the other parts of the bicycle (the sensors send their data in many different formats, so we must deal with this first before we can use this data).

ROS Topics

Two of the topics found in the ROS wrapper are gps and bike_state. gps stores data from the GPS and contains the bike’s latitude, longitude, speed (m/s), and the age of the location data. bike_state contains data from the IMU, encoder and hall sensors. It stores the velocity and position of the front motor, velocity of the rear motor, battery voltage and the bike’s orientation. Other software components, such as the bike’s navigation algorithm, are able to access this data through the ROS topic.

Wrapper File

Organization

Because the ROS_arduino_wrapper has so many responsibilities on the bike, it is split up into a main file as well as a number of helper files. Pictured below is the generalized structure of the code. It is split into modules, each module representing a different role or subject in the code.

File:ROS Arduino Wrapper Diagram

Diagram of the organizational structure of the ROS_arduino_wrapper code

Each of the helper files (the yellow ones in the diagram) are composed of .h (header) and .cpp files. The header files have comments documenting what each piece of code does, as well as declares which variables are to be used externally and internally. The actual implementation of the functions declared by the header files are in the associated .cpp files, and the variables themselves are defined in the .cpp files or the main ROS_arduino_wrapper file. The vast majority of functions and variables used in the code are defined in the helper files and then either called or referenced in the main wrapper file. With the exception of the Encoder files, all of the files exclusively feed into the ROS_arduino_wrapper. This is because the data from the encoder is used in some of the calculations for the front wheel speed and steer angle. Everything referencing PID controller is in the FrontWheel file.

Overview of Functionality

• Import all of the relevant files into the arduino wrapper
• Initialize all of the timing variables
• Initialize all of the ROS publisher objects and listeners
• Define constants (this is the #define keyword). This gives constants a variable name in order to make the code more readable. These are typically used for defining arduino pins and other things in hardware (because they don’t tend to change while the program runs).
• Define a few relevant functions
  • sendUBX: the method for sending hex messages to the gps
  • navOrRC: the function that sets the steer and speed based on either RC or nav instructions, depending on which mode the user set it to using the RC remote (by default this is nav, not RC)
• Arduino setup loop (what we do in the beginning every time we run the code)
  • Define the size/lengths of the data we’re passing back and forth between the sensors and the ROS.
  • Define the arrays we’re passing back and forth
  • Start the timers
  • Attach hardware interrupts to the RC channels (this tells us when something on the RC remote has changed, i.e. when we moved one of the switches/joysticks on the remote)
  • Start the serial connections and start them at the appropriate baud rates (gps is at 9600, rest is 115200). Baud rates are important because you need them to match up in order to get/print readable data.
  • Tell gps what data we want from it
  • Initialize the IMU with the data we want from it
  • Set up the encoder pins
  • Activate peripheral functions for quad pins on the Arduino
  • Set up motor outputs
  • Set up the watchdog, landing gear, RC, LED, rear motor pins
  • Set the rear motor to a given speed (analogWrite(PWM_rear, pwm)), pwm is the variable that directly controls voltage to the rear motor
  • Run the front wheel calibration loop to find whether the center is (define a zero position with respect to the Z channel of the encoder)
  • Initialize the rear motor
  • Set nav mode as the default mode for the bike (as opposed to RC)
• Arduino loop (runs constantly once set up and powered up)
  • Tell the LEDs to blink (we use this to see if the loop is slowing down for whatever reason)
  • Update the speed of the bike and the steer using instructions from either nav or RC (depending on the mode set by the switch on the remote -- RC channel 5 in the code) (navOrRC();)
  • Send the appropriate rear wheel speed to the rear wheel (<codet>analogWrite(PWM_rear, foreward_speed);)
  • Update the encoder position (float encoder_position = updateEncoderPosition();). The relevant function is defined in the encoder helper file.
  • Get roll angle, rate, and yaw from the IMU (roll_t imu_data = updateIMUData();) The relevant function is defined in the IMU helper file.
  • Set the desired velocity based on data from balance controller, IMU, and encoder.
  • Put the bike state data into an array
  • Get data from the gps buffer
  • Put the data from the gps buffer in an array
  • Publish address of bike, gps, and PID controller state objects for ROS
  • Update the timer variables to count how long it took to do all of the above (prev_millis)
  • Update total time elapsed counter to see what hz we’re running at (total_millis)

Overview of Helper Files

Encoder

Variables Declare which registers to get the relative position from and the index value Set the pins we’re using as quad pins Set which pins are being used by encoder Declare all the variables used to hold position, velocity, error, and pwm Functions updateEncoderPosition: Updates global variables representing encoder position (relativePos, indexValue, and current_pos). current_pos is defined using the difference between the relative position and the x-axis offset of the wheel multiplied by some constants and converted to radians.

Front Wheel

Variables Define PID constants Declare the array holding the PID controller data Assign the front motor variables to pins (PWM_front and DIR) Initialize steer direction, desired steer, desired lean variables Define the balance controller constants Functions PID_Controller: Runs a PID controller to keep the front wheel at desired_pos and returns the current velocity (this controls the motor speed). eulerIntegrate: Takes in commanded velocity from balance controller, and converts it to position frontWheelControl: Takes in desired velocity and applies a PID controller to calculate and use the desired velocity in order to minimize the error between current position and desired position. (Calls PID_Controller and eulerIntegrate) balanceController: returns desired angular velocity of front wheel based on roll angle, roll rate (both from IMU) and encoder angle as well as desired lean and steer

IMU

Variables Define the structure that holds IMU info Defines array that holds euler angles Defines array that holds the gyro rate Functions initIMU: Initialize the IMU using SPI protocol getIMU: Sends command (taken as param) to the imu as well as the location in the relevant data array and returns data accordingly. The byte parameter refers to the command we send to the IMU (i.e. instructs which piece of data we want). updateIMUData: Retrieve data from IMU about roll angle and rate and return it

RC

Variables Define which channels are in use and what pins they’re attached to (1 for steer angle, 2 for velocity, 5 for RC/Nav mode switch, 6 for landing gear) Define the timers used for RC functions Define the steer range and forward speed (yes it’s misspelled in the code) Define the pulse times used for tracking the signal received from the RC remote Functions CalcSignal (code for all of these functions is essentially the same): Records the interrupt time (when the function was triggered to run) Checks if pin has gone high (if it has, save the time since the Arduino started up) Check if the time is within range for us to have considered it to have changed (i.e. the control was activated) If it was activated, record how much time has been outputted by the RC and put that in a variable (pulse_time) which then is converted to a PWM elsewhere in the code Restart the timer

Rear Motor

Variables Declare pins used for rear motor Declare rear motor variables (PWM, speed, whether wheel is going forward or not), rear motor speed calculation helper variables (tOld, tNew, T) and rear motor controller variables (gain and desired speed) Functions rampToPWM: Method for setting rear motor at a certain PWM, which is taken as a parameter along with the current PWM switchDirection: Switches direction of rear wheel motor getPeriod: Calculates period of wheel turning based on circumference of wheel and the number of hall sensors, as well as time passed since last tick