Difference between revisions of "F17: FoxP2"

Latest revision as of 22:31, 18 May 2019

FOXP2

Abstract

This project entails the aggregated knowledge from the Embedded System courses offered at SJSU leveraged into a fully functioning self-driving vehicle. This vehicle will navigate the terrain, avoid obstacles, and relay feedback to the user. The Initialization is enabled using the custom application which will set the final destination using GPS and destination node.

Objectives & Introduction

Our objective in this project was to create a fully autonomous car that would receive a GPS destination coordinate and would be able to reach there despite any obstacles around it. We used 5 SJ One boards to communicate each other over the CAN bus where each one of the boards would be responsible for system critical responsibilities. We had 5 boards that took care of the following features:

• Gateway: Master controller that would receive data from GPS and Sensor modules and decide on next actions. Based on that it will control the Motor module to maneuver the car
• Sensor: Detects obstacles at 45, 90 and 135 degrees and sends data back to Gateway module
• Motor: Controls the front and back wheels according to Gateway module's commands. Also shows system critical data on LCD
• GPS: Uses GPS and compass to detect current location of the car. Also gets the destination and checkpoints from Bridge to send direction requests to Gateway
• Bridge: Communicates with Android App through external Bluetooth HW to coordinate locations/destinations between the App and the GPS module

Team Members & Responsibilities

• R Nikfar
  • Team Lead
  • Electrical Circuits Engineering
  • PCB design
  • Sensor testing and implementation.

• Jason Tran
  • Infrastructure
  • Sensor IO Implementation
  • Testing

• Ahsan Uddin
  • Code structure and quality
  • Git Admin
  • Motor Control

• Yuyu Chen
  • Geographical Implementation
  • Bridge Support

• Marvin Flores
  • Android Application development
  • Bridge Implementation

• Rabeel Elahi
  • Master/Gateway Controller Implementation.
  • Object Detection Implementation.

• Sophia Quan
  • LCD interface

• Michael Jaradah
  • Testing.

• Taylor Kearns
  • Testing.

• Bohan Liu
  • Master Controller.

Schedule

Parts List & Cost

Overall Design and Methodology

Rob Nikfar

Eagle Schematic

Eagle Board

Board Connection Architecture

Our team's approach to the design of this project was solely based on the integrity of its connections and communication between the nodes. Each node would handle specific parts of the car that would require an extensive amount of computing. At the center of this communication would be our design and printed PCB(printed circuit board). This board was designed in a manner that would reduce the amount of noise within the circuitry of its components, and place its nodes at an optimal location relative to the car.

Printed Circuit Board

The PCB which is at the heart of this project was designed in Eagle CAD. There were 2 iterations of this board. The first one was designed without validation and had problems with noise within the CAN communications(discussed in problems encountered section). This design lacked several necessary connections and was limited by functionality.

The Second version of the board would incorporate 4 external power outlets that supply 3.3V and 5V to external components as necessary. There would also be an external power unit with regulators that make sure that a clean power is fed to the boards if necessary. As shown in the schematic, the can transceivers are connected to the boards using the terminating resistors. This CAN bus line also connects to the DB9 connections to easily read the CAN data using the PCAN Dongle.

Before the Board was sent for printing, the rat nests had to be removed and the connections had to be routed in such a way that minimum amount of noise was created. this ensured a robust communication for the CAN bus and stable noise-free power supply for the external components such as the Ultrasonic Sensors.

Overall System Design

DBC File

A DBC file is used to define the CAN communication between all nodes. It is parsed using a Python script which then generates "generated_can.h" to be included by each node.
The following is the defined DBC file for FoxP2

VERSION ""

NS_ :
    BA_
    BA_DEF_
    BA_DEF_DEF_
    BA_DEF_DEF_REL_
    BA_DEF_REL_
    BA_DEF_SGTYPE_
    BA_REL_
    BA_SGTYPE_
    BO_TX_BU_
    BU_BO_REL_
    BU_EV_REL_
    BU_SG_REL_
    CAT_
    CAT_DEF_
    CM_
    ENVVAR_DATA_
    EV_DATA_
    FILTER
    NS_DESC_
    SGTYPE_
    SGTYPE_VAL_
    SG_MUL_VAL_
    SIGTYPE_VALTYPE_
    SIG_GROUP_
    SIG_TYPE_REF_
    SIG_VALTYPE_
    VAL_
    VAL_TABLE_

BS_:

BU_: MOTOR SENSOR GATEWAY GEOGRAPHICAL BRIDGE


BO_ 100 MOTOR_CMD: 1 GATEWAY
 SG_ MOTOR_CMD_MOMENTUM : 0|4@1+ (1,0) [0|15] "" MOTOR
 SG_ MOTOR_CMD_TURN : 4|4@1+ (1,0) [0|15] "" MOTOR

BO_ 120 RPM_VALUE_CMD: 1 MOTOR
 SG_ RPM_VALUE_CMD_WHEEL_CLICKS : 0|8@1+ (1,0) [0|0] "" GATEWAY

BO_ 140 COMPASS_VALUE_CMD: 5 GEOGRAPHICAL
 SG_ COMPASS_VALUE_CMD_VALID : 0|1@1+ (1,0) [0|1] "" BRIDGE,MOTOR
 SG_ COMPASS_VALUE_CMD_VALUE : 1|32@1+ (0.000001,0) [0.000000|360.000000] "" BRIDGE,MOTOR

BO_ 145 GPS_VALUE_CMD: 8 GEOGRAPHICAL
 SG_ GPS_VALUE_CMD_VALID : 0|1@1+ (1,0) [0|1] "" MOTOR,BRIDGE
 SG_ GPS_VALUE_CMD_BEARING : 1|29@1+ (0.000001,0) [0.000000|360.000000] "" MOTOR,BRIDGE
 SG_ GPS_VALUE_CMD_DISTANCE : 30|33@1+ (0.000001,0) [0|8000.000000] "meters" MOTOR,BRIDGE

BO_ 200 GEOGRAPHICAL_CMD: 8 GEOGRAPHICAL
 SG_ GEOGRAPHICAL_CMD_direction : 0|4@1+ (1,0) [0|15] "" BRIDGE,GATEWAY
 SG_ GEOGRAPHICAL_CMD_distance : 5|33@1+ (0.000001,0) [0|8000.000000] "meters" BRIDGE,GATEWAY

BO_ 300 SENSOR_PROX_STATUS: 7 SENSOR
 SG_ SENSOR_FRONT_DIST : 0|13@1+ (1,0) [0|0] "in" BRIDGE,GATEWAY
 SG_ SENSOR_LFRONT_DIST : 13|13@1+ (1,0) [0|0] "in" BRIDGE,GATEWAY
 SG_ SENSOR_RFRONT_DIST : 26|13@1+ (1,0) [0|0] "in" BRIDGE,GATEWAY
 SG_ SENSOR_REAR_DIST : 39|13@1+ (1,0) [0|0] "in" BRIDGE,GATEWAY

BO_ 400 BRIDGE_STOP: 1 BRIDGE
 SG_ BRIDGE_STOP_CMD : 0|1@1+ (1,0) [0|1] "" GEOGRAPHICAL

BO_ 401 GEO_ALERT: 1 GEOGRAPHICAL
 SG_ GEO_ALERT_GO_NO_CHECKPOINTS : 0|1@1+ (1,0) [0|1] "" BRIDGE

BO_ 403 BRIDGE_NUM_CHECKPOINT: 1 BRIDGE
 SG_ BRIDGE_NUM_CHECKPOINT_CMD : 0|8@1+ (1,0) [0|255] "" GEOGRAPHICAL

BO_ 404 BRIDGE_LAT_LONG: 8 BRIDGE
 SG_ BRIDGE_LAT : 0|32@1+ (0.000001,0) [36.0000000|38.000000] "degrees" GEOGRAPHICAL
 SG_ BRIDGE_LONG : 32|32@1+ (0.000001,-123) [-123.0000000|-120.000000] "degrees" GEOGRAPHICAL

BO_ 405 BRIDGE_DONE: 1 BRIDGE
 SG_ BRIDGE_DONE_CMD : 0|1@1+ (1,0) [0|1] "" GEOGRAPHICAL

BO_ 406 GEOGRAPHICAL_RESP_NUM: 1 GEOGRAPHICAL
 SG_ GEOGRAPHICAL_RESP_NUM_CMD : 0|8@1+ (1,0) [0|255] "" BRIDGE

BO_ 407 BRIDGE_GO: 1 BRIDGE
 SG_ BRIDGE_GO_CMD : 0|1@1+ (1,0) [0|1] "" GEOGRAPHICAL

BO_ 408 GEOGRAPHICAL_CURR_LAT_LONG: 8 GEOGRAPHICAL
 SG_ GEOGRAPHICAL_CURR_LAT : 0|32@1+ (0.000001,0) [36.000000|38.000000] "degrees" BRIDGE
 SG_ GEOGRAPHICAL_CURR_LONG : 32|32@1+ (0.000001,-123) [-123.000000|-120.000000] "degrees" BRIDGE


CM_ BU_ DRIVER "The driver controller driving the car";
CM_ BU_ MOTOR "The motor controller of the car";
CM_ BU_ SENSOR "The sensor controller of the car";
CM_ BO_ 100 "Sync message used to synchronize the controllers";

BA_DEF_ "BusType" STRING ;
BA_DEF_ BO_ "GenMsgCycleTime" INT 0 0;
BA_DEF_ SG_ "FieldType" STRING ;

BA_DEF_DEF_ "BusType" "CAN";
BA_DEF_DEF_ "FieldType" "";
BA_DEF_DEF_ "GenMsgCycleTime" 0;

BA_ "GenMsgCycleTime" BO_ 500 100;
BA_ "GenMsgCycleTime" BO_ 100 1000;
BA_ "GenMsgCycleTime" BO_ 101 100;
BA_ "GenMsgCycleTime" BO_ 400 100;
BA_ "GenMsgCycleTime" BO_ 200 100;
BA_ "GenMsgCycleTime" BO_ 300 100;

BA_ "FieldType" SG_ 500 DBC_TEST1_enum "DBC_TEST1_enum";
BA_ "FieldType" SG_ 100 DRIVER_HEARTBEAT_cmd "DRIVER_HEARTBEAT_cmd";


VAL_ 500 DBC_TEST1_enum 2 "DBC_TEST1_enum_val_two" 1 "DBC_TEST1_enum_val_one" ;
VAL_ 100 DRIVER_HEARTBEAT_cmd 2 "DRIVER_HEARTBEAT_cmd_REBOOT" 1 "DRIVER_HEARTBEAT_cmd_SYNC" 0 "DRIVER_HEARTBEAT_cmd_NOOP" ;

Motor Controller

Motor Controller primary tasks are as follows:

• Control the rear wheels for moving forward, backward and stop

• Control the front wheels to maneuver

• Display contents on the LCD

Drivetrain

Ahsan Uddin

ESC (Electronic Speed Control)

Our car came up Traxxas XL5 installed already. We knew that the onboard wireless receiver controls the ESC through PWM but we didn’t know the frequency and the duty cycle. So we hooked up the ESC with an oscilloscope and used the Wireless transmitter to go forward/backward and found out that the operating frequency of the ESC is 100Hz.

Traxxas XL-5 ESC + Titan 12 Motor

Then we tried implementing the same driving frequency on the SJ one board and we came across several issues:

1. Even though we were setting the SJ Board to 100Hz frequency, for some reason it was outputting ~110Hz

2. The percentage duty cycle we set in the SJ One board FW, was not exactly the same percentage that was output to the pins

3. Due to the above issue in (2), we couldn’t control the car to the finest level of speeds, either it would be stopped or it would be racing down so we had come up with different plans

Our solution to the above problems were to overclock the ESC to ~420Hz where we could control the speed limits very finitely within the scope of the PWM driving frequency of the SJ One board. Below are the scope shots of the PWM driven by the SJ One board to control the ESC

Servo (to turn the front wheels)

The Servo also uses PWM as an input to control the direction of the wheels. SJ One board has a limitation where all PWM signals need to be driven at the same frequency. Which means that as we are driving the ESC signals at ~420 Hz, we also had to drive the servo PWM signals at 420Hz. We had to do a parametric search of various PWM percentages to find out which ones would be best suited for turning the servos. Below are the scope shots of the PWM signals that SJOne board is using to drive the servo directions.

Traxxas 2075 Servo

RPM Sensor (for detecting speed)

We used Traxxas RPM sensor (6520) to detect the speed. The package came with a single magnet which, along with the RPM sensor, needed to be mounted inside the motor compartment. The idea is that everytime the motor completes 1 whole revolution, it reaches very close to the sensor. And when the sensor detects the magnet nearby, it sends out a high pulse until the magnet moves away from it. Therefore, upon detecting the X number of rising edge in Y seconds we can say that there were X revolutions/Y seconds

The schematic for hooking up the RPM sensor was challenging because of the following:

• First we came across an issue where we were able to mount the magnet on the motor but it wasn't possible to mount the RPM sensor head inside the motor compartment. Later we found out that we needed a special compartment designed by Traxxas to hold the RPM sensor inside the motor compartment

• When we were prototyping the RPM sensor on breadboard, we saw that regardless of whether we bring the magnet close to the sensor or not, the signal voltage was always high. After many researches and trail and errors we found out that the signal pin had to be connected to the VCC across a 1K resistor so that it can effectively pull it down when it detects the magnet

• The Traxxas RPM sensor package came with a single magnet and a Hall effect sensor. So we put the single magnet inside the motor compartment and it was working fine when we were manually bring the hall effect sensor close by. But when we would put the motor cover on, we would see that the RPM sensor would never trigger. We fixed the issue by putting 2 magnets on top of each other to increase the magnetic field and then we were able to see the RPM sensor work seamlessly.

Hardware Interface

• CAN bus

Motor ECU uses can1 to communicate with other ECUs. Motor primarily receives data from Master/Gateway to maneuver left/straight/right and also stop/forward/reverse the wheels. Motor also receives non-critical system data from Geographical ECU which needs to be displayed to the LCD. For the CAN configuration, we used baud rate of 100bps.

• PWM to Motor and servo

There are 3 PWM pins available on SJ One Board. As explained earlier, all PWM frequencies were set to 420Hz. The Motor PWM PIN is driven from XXX and the servo PWM pin is driven from YYY.

• RPM sensor for edge detection

The RPM input is connected to P2.6. It should be noted that there's a 1K resistor between the VCC of the RPM and the Signal Input

Motor, Servo and the RPM share the power directly from the 8.4V Traxxas battery

• UART to LCD

LCD uses UART TX/RX port to talk to the LCD. The UART is configured at 9600 bauds/sec.

Software Design

1. Initialization sequence

In the Motor ECU's init sequence we do the following in order:

• CAN Init

Motor CAN is initialized on can1 with BAUD rate 100bps and TX/RX queue sizes of 100. Then the filter is setup for 100-150 MSG IDs.

• PWM Init

2 PWMs are set at P2.0 and P2.1 at 420Hz for Motor and Servo respectively. By default, the PWMs are set to straighten up the front wheels and put the back wheels to IDLE

• LCD Init

UART is initialized at TXD2/RXD2 for LCD at 9600 baud rate

• RPM sensor Init

P2.6 PIN is initialized as input with pull down resistor and a rising edge interrupt is being installed to detect the RPM triggers

2. 1 Hz Task

1Hz task is responsible for:

• CAN Bus reset logic

In this handler we simply check if the CAN bus is off or not. If it's off, we do a BUS reset

• Handle Manual PWM (only in debug mode)

This mode is only for debugging purpose. Just by change of a #define flag, we can shutdown all CAN related code and turn on the manual mode where we can use the on board switches to control the front and the back wheels. This has been a very effective was of debugging to find out HW related issues without taking off any of the components.

• RPM check

We count the number of ticks that happened in 1 sec and reset the value of the ticks, therefore we get a revolution/second value

• Speed check

We have predefined macros of what should be a high speed value and a low speed value. If we detect the current speed values are not in limit, we upshift or downshift the gears to control the speed

• Update LCD

We get the following data and update them to the LCDs:

• • RPM data
  • Bearing data
  • Compass data

• Send CAN messages

We send CAN messages to Gateway with our current speed values

3. 10 Hz task

10Hz task is responsible for:

• Reverse State machine

In order to reverse properly, we have to follow a specific sequences of Stop and Reverse. This handler takes care of it. Details of this state machine is described below in Bugs section

4. 100 Hz task

• Handle CAN messages

This is the master handler for receiving all the CAN messages. Primarily they are motor CMDs from Gateway and compass data from Geographical for displaying on LCD.

Implementation

• Overall

The Motor ECU's main task is to make sure the motor wheels and the servo turns are always in accordance to Master ECU's commandment. Motor ECU dequeues CAN messages in 100Hz task therefore the reaction time of wheels movement is never over 10ms. Although to reach full speed from stop, and more importantly, brake from full speed, it would take the car more than ~1s due to the weight and the inertia from speed.

• Debug mode

In order to debug the motor side easily on the field we implemented a debug mode where we shut down all CAN communications and let the on board switches to control the front and the back wheels. In this mode, all the other peripherals like LCD ad RPM sensors are fully functional.

LCD

Sophia Quan

The uLCD-32PTU is a 3.2" (240x320) LCD screen with a micro SD connector, GPIO's along with I2C and serial COMMS. Workshop4 IDE was used to program the GUI, and data was transmitted from the SJOne board through UART communication. The LCD displays RPM clicks, GPS heading, bearing, distance, and also the current firmware version and branch name.

LCD Hardware Architecture

LCD Display

Note: The display here shows the angularmeter which will display the rpm, the top right box will display the bearing, heading, and distance from the GPS, and the bottom box will disply the firmware git hash, and branch.

Software Implementation

A software abstraction layer was implemented for the LCD screen consisting of init, ack/nack, and write functions.

• INIT

Since this LCD uses UART for its communication protocol, we set the baudrate at 9600bps and define the sizes of the rx/tx buffers.

• ACK & NACK

When sending data to the LCD screen, if data is successfully transmitted, the LCD screen will return a 0x06H, and if not transmitted properly, the LCD will return a 0x15H. The function to check ACK/NACK will check for these values and print to the console the proper debug message.

• WRITE

The data sent to the LCD screen is in a specified format for each object as shown below:

Angularmeter: BYTE0 - Event byte BYTE1 - Object type BYTE2 - Object index BYTE3 - Data byte 1 BYTE4 - Data byte 2 BYTE5 - Check sum

Static String Text Box: BYTE0 - Event byte BYTE1 - Object type BYTE2 - Object index BYTE3 - Data length BYTE4 - First character to be sent . . . BYTEN+4 - Last character to be sent BYTEN+5 - NULL character BYTEN+6 - Checksum

Sensors

Jason Tran and Rob Nikfar

The primary responsibility of the sensor ECU is to provide object detection capabilities.

Hardware Design

For object detection, distance sensors are used to provide proximity awareness. 3 distance sensors are placed on the front of the car (front left, front, and front right) to continuously measure the distance of the general directions. An object or obstacle can be detected when the distance of a given direction is below a threshold which indicates that the car cannot traverse in that direction and should attempt steer away from the blocked path.

Hardware Interface

Sensor Hardware Architecture and Interface

Note: Despite the names, the TX and RX pins do not function as asynchronous serial transmit and receive in the implementation above. The TX functions as a digital output that drives the line high for a short duration when the sensor wants to trigger a cascaded sensor. The RX functions as an digital input that acts as a trigger for the sensor to begin measuring a distance.

MB1010 Pinout

Software Design

The sensor software follows a modular design pattern. Sensors is the top level module that contains periodic function definitions. The CAN Interface module consists of a periodic task that is responsible for broadcasting the most up to date sensor readings over CAN. The Car Proximity module also consists of several periodic tasks that are responsible for managing the frontal sensor readings.

Sensor Software Architecture

Implementation

At 100Hz, the ECU reads the analog voltage of all 3 sensors, performs scaling on the raw ADC values to decode the voltage into inches, then stores the value in 3 circular buffers respectively. A circular buffer represents a history of inches reported by a sensor; the most recent firmware version stores the 3 most recent sensor readings into a circular buffer which represents a 30ms time frame.

At 10Hz, the ECU broadcasts the distance of all 3 sensors over CAN.

Filtering is also applied against the circular buffers. Of the data in a given circular buffer, the ECU reports the minimum distance.

There are 3 onboard LEDs that represent the state of the 3 frontal sensors. The LEDs are driven by GPIO. If, for example, the front sensor detects an object, then the ECU will assert the center onboard LED.

Geographical Controller

Yuyu Chen

The main purpose of the geographical controller is to guide the car towards its destination. This is done by processing data from the GPS and Compass module in order to get the Heading, Bearing, and Distance.
The compass will allow the user to calculate the Heading, which tells us which direction the car is pointing relative to magnetic north or simply, "Which way the car is pointing"?
The GPS will allow the user to get the current coordinate of the car, which will be useful when calculating the Bearing and Distance.
The Bearing tells us the direction from the current location to a checkpoint coordinate, and is given as the angle between the location and the current location of the car.
The Distance tells us how many meters away the current checkpoint is. Essentially, How far away are we from our destination?

The geographical controller is also responsible for:
1. Process data from GPS and Compass module.
2. Navigate to nearest checkpoints and to set destination.
3. Receive checkpoints from Bridge controller.

Hardware Design

Hardware Interface

CAN Bus
The CAN bus interface allows the geographical controller to send and receive data from other microcontrollers on the bus. For example, the geographical controller will send requests to the Gateway controller to tell it to turn right, left, or keep straight in order to reach the set checkpoints. The geographical controller will receive checkpoints from the Bridge controller in order for the geographical controller to know how to steer or how far away the destination is at.

MTK3339 Ultimate GPS

The MTK3339 Ultimate GPS module from Adafruit was chosen because it provided good results when it was used previously for a different project. The main purpose of the GPS module is to provide an accurate location of the car while the car navigates towards its destination. This is done by parsing NMEA sentences, which are transmitted by up to 22 satellites to pinpoint the current location of the module. There are various different types of NMEA sentences, such as $GPRMC and $GPGGA. The NMEA sentence that was used was $GPRMC, which provides only the recommended minimum. $GPRMC was also chosen because it is the only NMEA sentence the GPS module is able to provide data at 10Hz for real-time application. The module is interfaced via UART with a baud-rate set to 57600bps. The module also comes with a fix LED, which indicates if a GPS fix by blinking every 15 seconds, if not, it blinks every 1Hz. The module is powered by 3.3V from the SJOne board.

The $GPRMC sentence is separated by commas, which the user has to parse in order to get useful data out of the module. The most important data that the user want is the GPS coordinate, Latitude and Longitude. The strtok_r() function was used in order to parse the NMEA sentence.
Below is an example of a parsed $GPRMC sentence:

      Example: $GPRMC,225446,A,4916.45,N,12311.12,W,000.5,054.7,191194,020.3,E*68
                       1     2    3    4    5     6    7    8      9     10  11 12

CMPS11 - Tilt Compensated Compass Module
The CMPS11 - Tilt Compensated Compass module was chosen because it provides the heading value, which let the user know which way the car is facing relative to magnetic north. Not only does it provide heading, the module also has a 3-axis gyro and a 3-axis accelerometer to remove or neglect any errors while the module is tilted (going up or down a slope). Another compass module was considered before choosing this module because the old module had to be calibrated extensively in order for it to work properly. This module only needed to be calibrated one time in an environment free of magnetic interference. The heading was double-checked after the module was mounted onto the car.
The compass module is placed away from all other components of the car to minimize any magnetic interference from the motor, servo, sensors, and microcontrollers. Placing it in an environment where there is magnetic interference will cause incorrect reading of the heading and the car will navigate towards the wrong direction. The compass module is interfaced via I2C. It is powered on by 3.6-5V, but 3.3V works also.

Software Design

The Geographical Controller is designed in a way so that the code is modular and organized. Only what is needed are exposed.

           GPS                                                 Compass                                                                Geo_Sensor
bool gps_init(gps_data_ready_cb cb);                  bool cmps11_init(void);                                                   bool geo_sensor_init(void);
bool get_lat_long(float *gps_data);                   float cmps11_get_heading(void);                                           void geo_sensor_run(void);
                                                      bool cmps11_calibrate(bool start_calibration, bool end_calibration);      

The GPS library and Compass library are used by Geo_Sensor in order to calculate the Heading, Bearing, and Distance when geo_sensor_run() is called. Extern variables are used to share data between the CAN layer and Geo_Sensor.

extern float e_heading;
extern float e_bearing;
extern float e_distance;
extern float e_curr_latitude;
extern float e_curr_longitude;
extern float e_checkpoint_latitude;
extern float e_checkpoint_longitude;
extern uint8_t e_heading_valid;
extern bool e_gps_distance_valid;

Implementation

GPS
The GPS main purpose is to provide the latitude and longitude for its current location. It is interfaced via UART. This will help us calculate the bearing and distance to the destination. An example of raw NMEA string is provided in the Hardware Interface section of Geographical Controller.

Compass
The compass will provide the Heading, which tells us the direction where the car is facing. It is interfaced via I2C. The Heading value is in units of degree, from 0 degree to 360 degree. Only two registers are read (Hi and Lo) for heading value.
Register 2 & 3 are read to get the heading value

uint16_t heading = 0;
uint8_t heading_byte[2] = {0};
s_compass->readRegisters(CMPS11_ADDR, heading_16_bit_hi, &heading_byte[0], 2);   
heading = heading_byte[0] << 8 | heading_byte[1];

There is a calibration sequence in order to initiate calibration mode. 3 separate I2C transactions are needed by first writing 0xF0 to Register 0, second write of 0xF5 to Register 0, and 0xF6 to Register 0.
After entering calibration mode, the module is needs to be rotated slowly on all axis to calibrate the magnetometer. After all the LED on the module stops blinking, place the module on all 6 sides to calibrate the accelerometer.
After calibrating the compass module, 0xF8 is written to Register 0 to end calibration mode.

cmps11 register map

Geo-Sensor
This layer combines the data from the GPS (latitude and longitude) and Compass (heading). In this layer, the distance (in meter) and bearing (in degree) are calculated. The distance and bearing are calculated by the current latitude and longitude, along with the checkpoint's latitude and longitude.

The Bearing (in degrees) is calculated from the following formula (Forward Azimuth):
   bearing = atan2( sin(longDiff_rad)*cos(lat2_rad), cos(lat1_rad)*sin(lat2_rad)-sin(lat1_rad)*cos(lat2_rad)*cos(longDiff_rad))

The distance (in meters) is calculated from the following formula (Haversine Formula):
a = sin^2(latDiff_rad/2) + cos(lat1_rad) * cos(lat2_rad) * sin^2(longDiff_rad/2)
   formula: c = 2 *atan2(sqrt(a), sqrt(1-a))
   d = Radius of Earth * c

The heading, bearing, and distance values are shared with the CAN layer for further manipulation. A simple moving average with a window of 10 was implemented to filter out bad GPS parsing (corrupt data) which occur after some time. Any distance value above the moving average will mark the GPS data as invalid and is dropped.

To navigate the car in the direction of the destination, the difference of the heading and bearing will have to be approximately zero. After many test runs, the team realize that the car moves to the right when the steering is straight. To account for this, the threshold for which to turn right is decreased so that it will turn right less.

Difference = Heading - Bearing

The difference between heading and bearing will tell us the offset between the direction of the current checkpoint and the current location of the car. It should steer the difference amount in order to go in direction of the checkpoint.

The distance to mark a checkpoint as reached is 5 meters radius from the actual destination, which accounts for some GPS inaccuracy. Therefore it will reach its destination within a 10 meter diameter. 5 meters was chosen after many test runs and seems to best fit because it does not over/under-shoot the checkpoint.

geo bridge state machine

checkpoint, destination algorithm

To receive checkpoints from the Bridge controller, the Bridge controller has to send the checkpoints via CAN to the Geographical controller. This is done with a handshake protocol between the two controllers. The Bridge will first send the number of checkpoints available to the Geographical controller. Once a non-zero number is received by the Geographical controller, the Geographical controller will start receiving Latitude and Longitude pairs from the Bridge controller. After receiving a "Done" signal from the Bridge, the Geographical controller responds with the number of Latitude and Longitude pair received. The handshake state machine for the Geographical controller is the left one above.

To command the car to navigate towards the destination, a GO signal needs to be sent from the Android App. To stop the car, a STOP signal is also sent from the Android App. After receiving checkpoints from the Bridge/App and Go is received, the Geographical controller will calculate which checkpoint is the closest based on distance between the checkpoints and the car's current location. After reaching that checkpoint, mark the checkpoint as "reached" to skip that checkpoint and find the next closest checkpoint until all checkpoint has been reached. The checkpoint algorithm for the Geographical controller is the right one above.

Bridge Controller

Marvin Flores & Yuyu Chen

The Bridge controller's main purpose is to act as a bridge between the Android Application and the car. The Android app is able to send and receive data wirelessly via Bluetooth. Any data, such as sensor data and current car location, can be sent to the bridge from other controllers via CAN. The Bridge controller will send the data wirelessly to the Android app to be displayed.
The main purpose of the Bridge Controller is to send checkpoints to the Geographical controller. This will be explained in the Implementation section of the Bridge Controller.

Hardware Interface

Bluetooth module on the SJSUOne board

Since SJOne Board has an XBee interface, The Bluetooth XBee module was the easiest to interface with. A Bluetooth module is installed on the Bridge controller for communication between the SJOne board and the Android App. The Bluetooth module is interfaced via SJOne board's UART.

Software Design

In order to receive or send data, an additional command is added to the TerminalTask to handle Bluetooth Commands.

 //Pseudocode command handler, bluetoothMessage is an extern variable that is defined in the periodic_task bridge class
 if command == "signal go" -> bluetoothMessage.setGoSignal()
 else if command == "signal stop" -> bluetoothMessage.setStopSignal()
 else if command == "data_start" -> payloadSize = arg1
 else if command == "checkpoint" -> checkpoints[index].latitude = arg1, checkpoints[index].longitude = arg2
 else if command == "data_end" -> bluetoothMessage.setPayload(checkpoints, payloadSize)

Periodic Task Bridge class functions

Implementation

bridge_geo_handshake

A Bridge-Geo handshake was designed in order to send checkpoints to the Geographical controller. Checkpoints are received from the Android App after the "Checkpoints" button on the app is pressed. This will initiate sending the number of checkpoints to the Geographical controller. After sending the number of checkpoints, the Bridge will start sending checkpoints to the Geographical controller. After all checkpoints are sent, the Bridge will send a Done signal to the Geographical to notify it that it has finish sending checkpoints. After receiving a response with the correct number of checkpoints sent, the Bridge will be notified that the handshake is complete and "Go" can be pressed on the Android App. The state machine for the Bridge-Geo handshake is above.

Android Application

Marvin Flores

Main Tasks

• Generate checkpoints
• Send START and STOP commands
• Auto-connect to the Bridge module
• Display relevant information:
  • map
  • current phone location
  • current car location
  • connection heartbeat
  • checkpoint

Software Design

High Level Architecture Design

Android Application High Level Architecture Diagram

The application's high level design is simple: Use Google Maps API to get the checkpoints and forward them to the bridge controller. Once the checkpoints are sent, the START command can be executed by clicking the START button on the app. This sends a start command signal to the bridge controller which then forwards the message to the Geo Controller via CAN bus. The STOP command, however, can be executed at any point in time and has the same process as the START command.

Implementation

Commander243 Android Application - Start navigation

Commander243 Android Application - Destination reached

Android Architecture Diagram

Android applications follows the MVC pattern by default to allow developers to manage their applications easily. For this application, the Views are the MapView, Buttons, Heartbeat (ImageView), and TextViews to display extra information from the bridge. The Controller is the main fragment that contains all the logic. The Models are the data that come from the the Google Maps API and data coming from the bridge.

MapPane Fragment

The MapPane Fragment is used as the main class of the application and provides the link between the Models and the Logic of the application. MapPane Fragment contains the MapView, buttons for CHECKPOINTS, START, and STOP, as well as some extra information from the Bridge Controller such as heartbeat and sensor values.

Logic

The Logic resides in the MapPane Fragment and is a combination of different objects:

• Button Click Listeners - these listeners are attached to specific button views to execute methods when clicked.
• Message Handler - this handler is responsible for communication between the main fragment and the Bluetooth Service.
• Connection Watcher - this is a thread that runs at startup which monitors the connectivity with the Bridge Controller by checking the heartbeat message.

Bluetooth Service

This component is responsible for connecting/disconnecting with the Bridge Controller via Bluetooth. It's also responsible for sending/receiving data, as well as parsing the received data from the Bridge Controller.

MapHelper

This helper class is responsible of requesting a GET REST call to Google Maps API. It's also responsible of parsing the Json response and extract the checkpoints.

Use Cases

Auto Connect

Flow Diagram Bluetooth Auto-connect

The auto-connect feature happens right after the user opens up the application. During startup, the MapFragment sends a connect message to the MessageHandler. The MessageHandler of the BluetoothService receives it and calls the appropriate method on the BluetoothService. The method then initializes the ConnectThread class that handles the Bluetooth socket connection. Since the default Android's BluetoothListener only triggers once when gets disconnected, the author decided to create a custom Watcher to monitor the Bluetooth connection. This was done by checking the heartbeat coming from the Bridge Controller once connection is established. If the heartbeat message is lost for a given amount of time, the Watcher automatically reconnects to the bridge by resending the connect message from the MapPaneFragment.

Send Checkpoints