Training Projects

These training projects are meant to familiarize new members with the theory behind the bike and the bike itself.

DC Motor Control Project

• 1: Test DC Motor using Arduino

Reference Circuit: https://learn.adafruit.com/adafruit-arduino-lesson-13-dc-motors/overview

• 2: One DOF Helicopter: Build a chopstick-fan that stays horizontal when disturbed

Reference: ECE 4760 Lab http://people.ece.cornell.edu/land/courses/ece4760/labs/f2018/lab4_helicopter.html

ROS Arduino Project

ROS stands for the Robotic Operating System, and despite the name is not an operating system. It provides a framework for programs in different languages running on different platforms to communicate. A running program is called a ROS node. Nodes can be started and stopped independently, and there must be a ROS core running (a special kind of node) for communication to occur.

One way nodes can communicate is via a ROS topic. A node will use a topic to publish messages, and other nodes may subscribe to the topic. A given topic's messages always have the same message type (i.e. data type). There are many possible data types; many will be familiar to you from previous programming languages.

For these programs, you will need the Arduino IDE. This open-source software allows you to write code and upload it to the board. Click here to download it.

Part I: Blink light as commanded by ROS topic

This program will allow an LED on the bike to be controlled by a ROS topic. It will subscribe to a topic called /led, and whenever a boolean value appears the program will set the state of the LED to that value. You may need a new library to compile ROS code. If #include <ros.h> compiles without an error, then you can skip this step. Otherwise, download the file "ros_lib.zip" in the bike drive folder. In the Arduino IDE, go to the menu item "Sketch > Include Library > Add .ZIP Library..." and navigate to where you downloaded "ros_lib.zip", and select it.

You may also need the library for the Due. Go to the "Tools > Board" submenu. If an entry called "Arduino Due (Programming Port)" appears all the way at the bottom, then you can skip this step. Otherwise, in the same submenu ("Tools > Board"), click on "Boards Manager..." at the top. Type "Due" into the search bar, and install the package that comes up. Now, from the "Tools > Board" menu, select "Arduino Due (Programming Port)", and plug the wire from the programming port on the Due into a USB port on your computer. You should now be able to upload code to the Due.

Let's write some code. Start a new program in the Arduino IDE ("File > New"), and add the following lines to it, at the very top of the file, outside of the functions that are already there:

#include <ros.h>
#include <std_msgs/Bool.h>

#define RED_LED 22
ros::NodeHandle_<ArduinoHardware, 1, 3, 500, 500> nh;

bool led_state = false; // Variable for whether the LED is on 
void updateInstruction(const std_msgs::Bool&data) {
   led_state = data.data;
}
ros::Subscriber<std_msgs::Bool> led_sub("/led", &updateInstruction);

Let's go over this code. The #include line includes other source files into the current source file. For header files and standard files, use #include <file_name> and for user defined files, use #include "file_name". Including ros.h includes all the headers necessary to use the most common public pieces of the ROS system and including std_msgs/Bool.h includes the std_msgs/Bool message. Next, the #define directive creates a macro, which is the association of an identifier or parameterized identifier with a token string. The compiler will then substitute the token string for each occurrence of the identifier in the source file. In this code, we are substituting every instance of RED_LED with 22. The line ros::NodeHandle_<ArduinoHardware, 1, 3, 500, 500> nh creates a NodeHandle, which handles all node initialization and communication. The arguments set the NodeHandle to have 1 publisher, 3 subscribers, 500 bytes for input buffer, and 500 bytes for output buffer. We then write a function updateInstruction() that will store the data in the message in the variable led_state. Finally, using the line ros::Subscriber<std_msgs::Bool> led_sub("/led", &updateInstruction), we create a subscriber to the \led topic that calls the updateInstruction() function whenever a new message arrives.

In the setup() function, paste this code:

nh.initNode();
nh.subscribe(led_sub);
Serial.begin(115200);
pinMode(RED_LED, OUTPUT);

Let's go over this section of code. nh.initNode() initializes the ROS node. You can only have one node in a rosserial device so you can only call initNode() once. In the next line, we subscribe to the led_sub subscriber created earlier. Serial.begin(115200) sets the data rate in bits per second for serial data transmission. For more information about begin() see here. Then we set the pin RED_LED to behave as an output.

Finally, in the loop() function, paste this code:

digitalWrite(RED_LED, led_state);
nh.spinOnce();

Since the RED_LED pin has been configured as an OUTPUT, digitalWrite(RED_LED, led_state) will set the voltage of the pin to the corresponding value: 5V for true and 0V for false. Because ROS pushes the subscriber callbacks onto a queue without calling them immediately, writing nh.spinOnce() will tell ROS to process all the callbacks. Here is more information about callbacks and spinning.

Try compiling and uploading to the Due. It shouldn't do anything. Remember to shut off the wheels while uploading, otherwise they may spin. Now we need to write the code on the Pi that will send instructions via the /led topic.

Now, on the Raspberry PI, to start ROS and connect to the Due, write: $ . start.sh serial_only &

To turn the LED on, you can send this command (replace true with false to turn it off): $ rostopic pub /led std_msgs/Bool "data: true"

Now, let's write a ROS node. It will be written in Python. You can put this anywhere, but by convention it'll go in ~/ros_ws/src/bike/scripts (note: check this path, might not be correct). Save it as led_node.py:

import rospy
from std_msgs.msg import Bool
import time
rospy.init_node("led")
pub = rospy.Publisher("/led", Bool, queue_size=10)
state = True
while True: pub.publish(state); state = not state; time.sleep(1)

And then you can run it with python led_node.py. The LED should blink on and off every second.

Part II: Rotate wheel as commanded by ROS topic

Our goal: to pass in a float between 0 and 360 and have the front wheel go to that location

Note: Use type std_msgs/Float32 (instead of std_msgs/Bool)

#include <std_msgs/Float32.h>

Instructions:

1. Go to front-wheel and download all five files into a folder on your computer and name it as front-wheel.

2. Select all five files in the folder and open them together (in Arduino) (for reference on Arduino set-up, see also: Part I of ROS Arduino Training Projects).

3. There are only two parts in the file called front-wheel.ino that need to be edited.

First, insert the following code before void setup():

     #include <math.h>
     #include <ros.h>
     #include <std_msgs/Float32.h>
     ros::NodeHandle_<ArduinoHardware, 1, 3, 500, 500> nh;
     float fwheel_angle = 0.0; //Variable for whether the LED is on
     void updateInstruction(const std_msgs::Float32& data) {
     fwheel_angle = data.data;
     }
     ros::Subscriber<std_msgs::Float32> fwheel_sub("/fwheel", &updateInstruction);
  

This will allow Arduino to import necessary libraries, be connected to the hardware, and let ROS subscribe to that setting.

Then, write the following two lines into the body of the void loop() at the bottom of front-wheel.ino:

     readFrontWheelState();
     float fw_pos_controller(fwheel_angle*M_PI/180);
    

For understanding of the part above, there are related parts of the current bike code that will be of assistance:

1. Go to ROS_arduino_wrapper (Wrapper File Description).

2. ROS_arduino_wrapper.ino:

In loop(), it calls frontWheelControl with two arguments: the desired velocity of the wheel, and the current position of the wheel.

//Loop variables
int blinkState = HIGH;

void loop() {
  numTimeSteps++;
  navOrRC();
  analogWrite(PWM_rear, forward_speed);
  l_start = micros();

  float encoder_position = updateEncoderPosition(); //output is current position wrt front zero
  roll_t imu_data = updateIMUData();
 
  float desiredVelocity = balanceController(((1) * (imu_data.roll_angle)), (1) * imu_data.roll_rate, encoder_position); //*****PUT IN OFFSET VALUE BECAUSE THE IMU IS READING AN ANGLE OFF BY +.16 RADIANS
  // frontWheelControl also calls a function that sends the PWM signal to the front motor
  // frontWheelControl will update the pid_controller_data.data array with new info
  float current_vel = frontWheelControl((-1) * desiredVelocity, encoder_position); 
  //DESIRED VELOCITY SET TO NEGATIVE TO MATCH SIGN CONVENTION BETWEEN BALANCE CONTROLLER AND

3. FrontWheel.cpp(Front Wheel Description):

In frontWheelControl, it takes the desired velocity and calculates the desired position

float frontWheelControl(float desiredVelocity, float current_pos) {
  unsigned long current_t = micros();
  float desired_pos = eulerIntegrate(desiredVelocity, current_pos);
  // The PID_Controller function will actually rotate the front motor!
  float current_vel = PID_Controller(desired_pos, relativePos, x_offset, current_t, previous_t, oldPosition, pid_controller_data_array);
  previous_t = current_t;
  oldPosition = relativePos - x_offset;
}

In PID_Controller, it takes the desired position and sends a command to the front wheel

float PID_Controller(float desired_pos, signed int x, signed int x_offset, 
 unsigned long current_t, unsigned long previous_t, signed int oldPosition,
 float *pid_controller_data) {

 //when y value changes (when wheel passes index tick) print absolute position of the wheel now to see if encoder absolute position
 //is drifting
 float current_pos = (((x - x_offset) * 0.02197 * M_PI)/180); //Angle (rad)
 pid_controller_data[0] = current_pos;

 //write PID controller based off of error signal received from encoder
 //P term
 //calculate position error (rad)
 float pos_error = desired_pos - current_pos ;
 pid_controller_data[1] = desired_pos;

 //scaled positional error
 //position scaling factor K_p = 100/(M_PI/2) found by taking 100 (100 being max pwm value I want to reach), 
 //and dividing by theoretical max absolute value of angle (Pi/2). 
 //This means with angles in that range, 100 will be the max PWM value outputted to the motor
 float sp_error =  (K_p*pos_error);
 pid_controller_data[3] = sp_error;

 //D term
 //calculate velocity error
 float current_vel = (((((x-x_offset)-oldPosition)*0.02197*1000000*M_PI/180.0)/(current_t-previous_t)));   //Angular Speed(rad/s)
 pid_controller_data[2] = current_vel;
 
 //calculate the value of the current time step in microseconds
 //unsigned long delta_t = 2000;
 
 // the value of the velocity error will be negative of the current velocity (in order to resist current direction of motion). 
 //Calculated as target_velocity - current_velocity where target velocity is always 0
 //scaled velocity error
 float sv_error =  (-K_d*current_vel)  ;  
 pid_controller_data[4] = sv_error;

 float total_error =  sp_error + sv_error; //Total error: scaled velocity and positional errors
 pid_controller_data[5] = total_error;

 oldPosition = x-x_offset; 

 set_front_motor_velocity((int)total_error);
 return current_vel;

}

Which is controlled by two parts:

* using digitalWrite to send a direction to the pin DIR:

/*
 * Set a motor velocity. velocity ranges from -100 to 100. Positive means
 * clockwise; negative means counterclockwise.
 */
void set_front_motor_velocity(int velocity) {
  if (velocity > 0) {
    digitalWrite(DIR, LOW);
  } else {
    digitalWrite(DIR, HIGH);
  }

* using analogWrite to send a velocity to the pin PWM_front:

  int abs_velocity = abs(velocity);
  // Limit to 100, or roughly 40% duty cycle
  if(abs_velocity > 100) {
    abs_velocity = 100;
  }
  analogWrite(PWM_front, abs_velocity);
}

GPS Project

Reading from csv files

In order to parse the nav data from csv files, you can use Python's csv library. However, a much easier method is to use the pandas library, allowing you to store the file with read_csv, and access data using the given field names. The field names given in the csv don't explain the data, however, descriptions can be found in the bike code.

Graphing with matplotlib

The next step of this project is to graph the data from the csv files using Python's matplotlib library. This can be done by storing the latitude and longitude values in arrays, and using scatter to generate a scatterplot of the points.

Implementing Dead Reckoning

Dead Reckoning is an important filtering technique that inductively defines position using previous position, velocity, and heading data. The general equation is (x_k,y_k) = (a+x_k-1, b+y_k-1), where x_k and y_k represents lat and long at time k, and a and b are constants defined by a=speed cos(heading) timestep and b=speed cos(heading) timestep. In essence, the equation takes the bikes previous position, calculates the change in x and y, and sums the two. This way, if GPS data is innacurate, we have another way to calculate position. On its own, Dead Reckoning isn't perfect, as the sensors that calculate velocity and the heading are also subject to inaccuracies. What we're going to do with this data is find some way to average it with the GPS data to come up with a good estimate of the position of the bike given as much information as possible. This idea of finding the right balance between the observed GPS position and inductively defined position is important for understanding the Kalman Filter and calculating the gain.