•

Jason Rubadue

Created April 23, 2019

Solar Charging Station

A standalone station that demonstrates power electronics and autonomous navigation

Advanced160

Things used in this project

Hardware components

Texas Instruments TI Robotics System Learning Kit TI-RSLK

Adafruit Lithium Ion Polymer Battery; Rechargeable; 3.7 V; 2.5 Ah; 2x2.55x0.3"(51x65x8mm)

Rechargeable ICR 14500 Li-ion Battery 3.7V 1500mAh For Torch Headlamp 6F9

Texas Instruments Voltage Protection with Automatic Cell Balance For 2-Cell Li-Ion Batteries, OVP=4.35V

Texas Instruments BQ24650 (ACTIVE) High Efficiency Synchronous Switch-Mode Charger Controller – Solar Battery Charger

Texas Instruments BQ24172 (ACTIVE) 1.6-MHz synchronous switch-mode Li-Ion and Li-Polymer stand-alone battery charger

TSMP58000: IR Sensors

DFRobot URM37: Ultrasonic Sensors

Newpowa 10 Watts 12 Volts Polycrystalline Solar Panel 10W 12V High Efficiency Module RV Marine Boat Off Grid

Software apps and online services

Texas Instruments Code Composer Studio

Story

The TI-RSLK robot is an educational robotics kit offered through Texas Instruments (TI) is a kit that offers a variety of labs to teach basics in robotics. The kit is used in computer science and electrical and computer engineering courses at various universities.

But what if there could be an add-on to this kit that could teach different engineering concepts? This is the question we tried to answer.

We are a group of 6 electrical and computer engineering students at the University of Colorado Boulder and for our senior project we designed the TI-RSLK solar charging station.

This add-on to TI’s existing robotics kit is designed for students by students to teach power electronics and solar harvesting with hands-on learning. With the growing population and changing environment the world is in dire need of power engineers thinking about how to better incorporate sustainable energy practices into the power grid. Up until now, many engineering programs have relied on a text-book approach to teaching engineering concepts and focused mainly on theory rather than practice, but we are beginning to learn as a society that the best way to teach engineering is with hands-on projects applying the concepts instead of with exams.

Our project is split into two parts: power and autonomous navigation. Within navigation, using encoders with ultrasonic sensors and IR sensors allow the robot to find its way back to the station autonomously. Power is split into 4 subsections: the solar panel, the station, the station PCB, and the robot PCB. These four parts work together to charge the robot batteries in a period of less than a day.

The purpose of the navigation system is to make the TI-RSLK robot autonomous. Using ultrasonic sensors and IR emitters, the robot can avoid obstacles and find it's way back to the station. The software flow will be as follows:

software flow of navigation

Each subsystem is described in more detail below.

IR Emitters

Each IR emitter is driven by a square wave which is a multiple of a base frequency. These signals are generated using a timer on the station MSP432. The timer increments at 3 MHz and an interrupt is called when this timer reaches a capture value of 10. The ISR increments a counter and resets the timer. Whenever the counter value is evenly divisible by a multiple of 2, the ISR toggles the corresponding GPIO pins connected to IR emitters.

The robot, after undocking from the station, drives around avoiding obstacles until a low battery voltage is detected. It then orients itself toward the station and drives towards it, avoiding any obstacles encountered along the way. It aligns itself more precisely to the docking port and docks until its battery is fully charged. Once its battery is charged, it begins driving around randomly again. More detailed descriptions of each navigation algorithm in relation to the sensors used are given below.

Returning to Station

The robot initially drives forward some distance, which is tracked by encoders. It then turns and drives another distance. Using the two distances and angle measured thus far, the distance to the station, which forms a triangle with the previous distances, is calculated using the law of cosines. The law of sines is used with this new angle and the previously measured data to calculate the angle to the station.

Robot path and equations

As long as the robot is continuously performing these calculations using the encoder data, it always knows its distance and angle from the station, even as it avoids obstacles. The dynamic nature of the algorithm ensures that calculating the distance and angle to the station is always the simple case of calculating the side and angle of a triangle.

It should be noted that the angles the robot turns and the angles used in triangle calculations are not actually the same. A simple equation is used in the code to convert between the two.

Angle comparison and angle conversion

Avoiding Obstacles

When the robot sees an obstacle with its front-facing ultrasonic sensor while driving along, it stops and checks for the presence of obstacles to its left and right using its side-mounted ultrasonic sensors. It turns in the direction where there are no obstacles (right if there are obstacles in both or neither directions) until a side-mounted ultrasonic sensor sees the object previously in front of the robot.

The robot then drives forward until the side sensor no longer sees the object, turns 90°, and drives until the side sensor sees the object again and then no longer sees it again. At this point the robot has avoided the obstacle and continues either driving around randomly or heading toward the station.

Obstacle Avoidance

When navigating with the side-mounted ultrasonic sensors, the robot always drives a small extra distance after the sensor no longer sees an obstacle to account for the fact that the side sensors aren’t mounted on the back of the robot.

Aligning to Docking Port

The robot has two angled IR receivers mounted on its front. When it determines from encoder data that it should be close to the station, it first turns until no IR signal is detected. It then turns again and, once IR is detected, records its current angle using encoder data. It continues turning until IR is not detected again.

At this point, the robot checks the frequency of the IR signal being received. If it is the frequency for the front of the station, the angle recorded earlier is subtracted from the current one and halved to find the angle the robot needs to turn back to orient towards the station. The robot turns this angle and drives until it is mechanically guided and docked to the station. Otherwise, the robot drives forward until IR isn’t detected and repeats this process as many times as needed until the station is found.

Robot docking

The above section describes the design that we utilized for each section of navigation. However, we encountered some problems and were not able to debug in our time with this project and there is plenty of room for further development.

1. The ultrasonic sensors have difficulty detecting obstacles that are not completely flat faced. If some sort of mapping algorithm was developed for obstacle avoidance, this would significantly improve navigation. Furthermore, using IR sensors in combination with the bump-sensors included with the TI-RSLK kit could potentially simplify the navigation process and/or improve it significantly.

2. Battling encoder slippage. We wrote an algorithm that significantly improves the effects of encoder slippage, however, this is a problem that can be eliminated by not relying on the encoder count to perform operations. The robot also incorrectly calculates angles when attempting to drive in a triangle.

3. Improving the docking algorithm. First, the dock function that we wrote is messy and should probably be rewritten in a more concise manner. The docking algorithm is missing functionality to navigate in front of the station and gets stuck in an infinite loop. This both causes the robot to continue running even once it has docked and also the robot loses the IR signal. Furthermore, the robot uses a timer to leave the station and this should be changed to sensing that the batteries are fully charged.

4. The station MSP doesn't open the connection to the robot when it should (this could be an issue with incorrect sample conversions into code).

Power

The power system consists of two printed circuit boards and a solar panel which allow for solar charging to occur. First, the solar panel is connected to the station board which utilizes Maximum power point tracking to optimize the power that charges the station batteries (two flat-pack lithium ion batteries). Then, the station batteries charge the robot board and the cylindrical lithium ion batteries which power the TI-RSLK robot. An overview of the power system is shown below:

solar charging station

robot board

1 / 6

Maximum Power Point Tracking (BQ24650)

An important part of a system that uses solar charging is the inclusion of a maximum power point tracker (MPPT) because the amount of sunlight is always changing. An MPPT varies a load as sunlight conditions change to extract the maximum power out of a panel. We determined that the BQ24650 is a good MPPT to use for our station board because it is both an MPPT and a battery manager. The robot batteries do not charge straight from the solar panel because the power circumvents the MPPT and the solar panel outputs at the battery voltage. The MPPT algorithm is also hardware programmable. In addition, the battery manager contains overvoltage protection and thermal shutdown to regulate battery voltage and current.

BQ24650: MPPT

To implement the BQ24650 (solar battery charger), we mostly followed the application schematic at the end of the datasheet because we found that the protections in place would work for our application. The equations from the BQ24650 datasheet that we worked out were:

MPPT equations

Lithium Ion Battery Charger (BQ24130)

To implement the BQ24130, we also primarily followed the application schematic at the end of the datasheet, but left the CELL pin floating to configure the chip for a 2-cell input. Additionally, we chose the sense resistor so that the charging current of the robot batteries is limited to 1A.

Lithium ion charger equations

Station MSP

The MSP432 microcontroller on the station is used to disconnect the station and robot batteries once they become fully charged as well as send square waves of various frequencies to the LED emitters. We wrote all code for the microcontroller in embedded C with the Code Composer Studio IDE.

Solar Panel

In order to meet our requirement of reliably charging our station battery in one full day, we chose to use a 25 W, 12 V solar panel from Newpowa. This is larger than our calculated power needs, as the solar panel will be significantly less efficient than the listed power. Solar panels usually only reach 80% of their specified peak power, and that is only reached when the sun is directly overhead. When the sun is at other angles to the solar panel, the power output is significantly lower. By buying a larger solar panel, we do not have to try to track the sun through the sky, which would require a lot of mechanical components. Overall, the larger solar panel will help us ensure we meet our requirements, even under less-than-ideal conditions, and we can consider reducing the panel size in later iterations of the design if we find the wattage to be excessive. When choosing the type of panel, the three main types of solar panels were considered: monocrystalline, polycrystalline and thin film. Monocrystalline was chosen, as it is the most efficient of the three, and not much more expensive. By having a more efficient panel, we can reduce panel physical size, which will reduce shipping costs and overall system size. The Newpowa panel has dimensions of 22.67in x 14.01in x 0.91in, and a weight of 4.23 lbs.

Solar Panel specifications

We tested our solar panel on a cloudy day and computed the following IV curve:

Even on a cloudy day, one amp of current draw is seen from the solar panel which is adequate current to charge the station batteries.

Station Design

Basic station design

The station will host our solar panel, power transfer connectors, and IR emitter. The front panel of the station will be 3D-printed using ABS filament, and the rest of the station will be constructed using clear acrylic. This will cut down on print time, along with allowing students to see the PCB and other components inside the station. The front panel features a guidance funnel to help with docking by guiding the robot to the power connector. This design was based off of similar docking designs for other robots. In addition, the station is smaller than the solar panel and sits inside the lip on the edge of the panel. By doing this, the rain will drip off the side of the panel rather than running into the station, thus keeping all the electronics dry.

Docking: Power Transfer Connector

power transfer

In order to transfer power between the station and robot, we implemented a mechanical spring clamp connection as seen in Figure 8. On the station PCB, we had four conductive pads, one pair for each of the positive and negative terminals of the batteries. Using ⅛” copper sheeting, we created rails that are soldered to the station pads. These rails will open out into funnels, which guide the robot’s bent copper loops into the terminals. As the loops enter the terminal, the loops bend slightly, making a tight connection. Testing was performed to ensure the connection could transfer power at the desired voltage and current, and to make sure the connections worked mechanically and will withstand repeated use.

Power Improvements and Opportunity for Further Development

Similar to navigation, we were unable to successfully implement our full charging system. Opportunities to improve charging are described below:

1. Choosing better batteries. The station lithium ion flatpack batteries worked very well, however, the cylindrical lithium ion batteries we used for the robot batteries did not work. We purchased four batteries and once we implemented all of our power circuitry, the batteries were drained below their safe operating point in a very short amount of time. The batteries used in this project should be further researched.

2. Exploring MPPT. Optimizing this chip with our panel is something we did not have a chance to implement. Once the station is fully functional, this can be explored by charging the station under different conditions and testing how the MPPT can be used to control the power draw from the solar panel.

3. Improving copper connection. Future groups should look into refining the copper docking connection by making it easier for the robot to dock. At times, the momentum of the robot would push it up over the contacts, which we solved by putting copper over the top of the contacts. The robot would still sometimes not dock correctly, though, so finding a better way to funnel in the connections would be a good thing to look into.

4. Improving station PCB. Once the robot was docked with the station, we encountered a problem where the LEDs on the station indicated that charging was occurring when no charging was occurring. This can be debugged by looking at FETs with board under power and potentially replace with FETs used on BQ24650 EVM. Otherwise, the station board can be made operational with power supply first and then with solar panel. Next, get the MSP interfacing with current sense chip and op-amp buffer to monitor station battery voltage and current, have MSP turn off and on hotswaps to output rails at critical battery states, and use FTDI chip and MSP to log battery current and voltage data on computer and use Python (or your favorite coding language) to plot station battery charge/discharge in real-time.

5. Improving robot PCB. The robot boards is not fully functional. Possible next steps are to run a test in which robot batteries charge through the robot board, watch battery charge current and voltage as it goes from a “charging” to “charged” state, monitor status LED of BQ24130, and add a resistor divider and op-amp circuit to monitor robot battery voltage on launchpad MSP.

6. Flesh out power budget. Determine the power draw of each subsystem and different charging modes of standard operation.

Robot Power Circuitry

This is a schematic for the power circuitry of the robot. The power (7.4V) from the station batteries inputs through the docking interface to the robot battery manager, the BQ24130. Again, the surrounding components (resistors, capacitors, transistors, etc) are used to program the chip to ensure that chip is ready to take the incoming 7.4V and use it to charge the 2 cell li-ion battery pack on the robot. The battery pack on the robot is also cell balanced with the same BQ29200 that is on the station battery pack. The system out line brings power to the robot motor board, which powers the motors, and runs through a voltage regulator on the motor board to power the MSP432 at 5V. Because there is a voltage regulator on the motor board, there is no need for an additional power converter in this robot power circuitry.

Station Power Schematic

This schematic is for the power circuitry of the solar charging station. The 12V Newpowa solar panel inputs to the Vin input of the BQ24650 MPPT and battery manager chip. The surrounding components (resistors, capacitors, transistors, etc) are used to program the chip to convert the incoming 12V into 7.4V that is used to charge the 2 cell li-ion battery pack. In parallel with the battery pack is the cell balancing chip (the BQ29200) that ensures the 2 cells of the battery pack do not charge/discharge each other. Connected to the battery is the docking interface that will contact the docking interface of the robot. Once connected, the station battery pack will discharge and power will go to the battery managing chip on the robot, the BQ24130. When disconnected, the batteries on the station will charge from the solar panel until they are full. An additional component that is not shown is the station MSP which controls the IR pulsed signal and power transfer.

Code

Texas Instruments
Robotics System Learning Kit
Solar Charging System Source Code

Authors: Nick Brubaker and Tristan Davolt



Station Code

station.h/.c - main program for station, makes station emit IR signals and connect or disconnect batteries from docking prongs


Robot Code

robot.h/.c - main program for robot, makes robot repeatedly drive around randomly and the return to station to charge
dock.h/.c - contains functions for docking robot with station
getFreq.h/.c - contains functions for measuring frequency
motorAdvanced.h/.c - contains funtions for constantly calculating robot's distance and angle from station
urm37.h/.c - ultrasonic sensor driver functions

/* --COPYRIGHT--,BSD
 * Copyright (c) 2017, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * --/COPYRIGHT--*/
/******************************************************************************
 * measRobotBat.h
 *
 * Author: Nick Brubaker
 *
 * Date: 3/13/2019
 *
 * Description: Declarations of functions for measuring
 *              the robot's battery voltage.
 * *******************************************************************************/

#ifndef MEAS_ROBOT_BAT_H
#define MEAS_ROBOT_BAT_H

/* DriverLib Includes */
#include <ti/devices/msp432p4xx/driverlib/driverlib.h>

/* Standard Includes */
#include <stdint.h>
#include <stdbool.h>
#include <string.h>

#define CONVERT_ROBOT_VOLTS 0.000302124     // Converts ADC value to robot battery voltage

volatile uint16_t ADCResult;   // To store ADC result


/* Configure ADC with P5.5 input, output stored in ADC result,
 * and mode as continuous single-channel */
void config_ADC();

/* Calculates and returns the measured battery voltage as a double. */
double getBatVoltage();

//******************************************************************************
//
//  ADC interrupt vector service routine - called whenever a conversion is
//  completed and placed in ADC_MEM0
//
//******************************************************************************
void ADC14_IRQHandler(void);

#endif

/* --COPYRIGHT--,BSD
 * Copyright (c) 2017, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * --/COPYRIGHT--*/
/*******************************************************************************
 * motorAdvanced.c
 *
 * Author: Nick Brubaker
 *
 * Date: 3/20/2019
 *
 * Revision: 1
 *
 * Description: Implementations of advanced motor functions for dynamically
 *              adjusting left motor speed and updating distance and angle
 *              from station while driving forward and/or turning.
 *              In some strange, not understood cases an incorrect angle
 *              to the station is calculated.
 ******************************************************************************/


#include <motorAdvanced.h>


/* Calculates robot's distance from station. */
double distanceToStation(double firstDistance, double secondDistance, double firstAngleRadians)
{
    return sqrt((firstDistance*firstDistance) + (secondDistance*secondDistance) -(2*firstDistance*secondDistance*cos(firstAngleRadians)));
}

/* Calculates robot's angle from station. */
double angleRadiansToStation(double firstDistance, double distanceToStation, double firstAngleRadians)
{
    return asin((firstDistance/distanceToStation)*sin(firstAngleRadians));
}


/* Drive straight forward, comparing left and right
 * encoder values and adjusting both motor's speed if needed.*/
void Motor_Corrected_Forward()
{

    /* Previous encoder measurement */
    uint32_t lastRightSteps;
    uint32_t lastLeftSteps;

    /* Encoder steps since last measurement */
    uint32_t newRightSteps;
    uint32_t newLeftSteps;

    Motor_Forward(leftDuty, rightDuty); // Start driving forward

    /* Get encoder steps */
    lastRightSteps = RightSteps;
    lastLeftSteps = LeftSteps;
    Tachometer_Get(&LeftTach[z], &LeftDir, &LeftSteps, &RightTach[z], &RightDir, &RightSteps);
    newRightSteps = RightSteps - lastRightSteps;
    newLeftSteps = LeftSteps - lastLeftSteps;

    /* Adjust left motor's speed depending on how left and right steps compare */
    if (newLeftSteps < newRightSteps)
    {
        leftDuty += 1;
        rightDuty -= 1;
        Motor_Forward(leftDuty, rightDuty);
    }
    else if(newLeftSteps > newRightSteps)
    {
        leftDuty -= 1;
        rightDuty += 1;
        Motor_Forward(leftDuty, rightDuty);
    }
}


/* The robot drives forward and then calculates its new distance from the station. */
uint8_t Motor_Corrected_Forward_Steps(double distanceSteps, double *stationDistanceSteps,
                                   uint32_t moduleFront, uint32_t moduleLeft, uint32_t moduleRight)
{
    uint8_t returnVal = DEST_REACHED;

    Tachometer_Clear();
    RightSteps = 0;
    while((RightSteps < distanceSteps)
            && ((getUrm37Distance(moduleFront) < 3) || (getUrm37Distance(moduleFront) >= 10))
            && ((getUrm37Distance(moduleLeft) < 3) || (getUrm37Distance(moduleLeft) >= 6))
            && ((getUrm37Distance(moduleRight) < 3) || (getUrm37Distance(moduleRight) >= 6)))
    {
        while((RightSteps < distanceSteps)
                && ((getUrm37Distance(moduleFront) < 3) || (getUrm37Distance(moduleFront) >= 10))
                && ((getUrm37Distance(moduleLeft) < 3) || (getUrm37Distance(moduleLeft) >= 6))
                && ((getUrm37Distance(moduleRight) < 3) || (getUrm37Distance(moduleRight) >= 6)))
        {
            Motor_Corrected_Forward();
            Tachometer_Get(LeftTach, &LeftDir, &LeftSteps,
                             RightTach, &RightDir, &RightSteps);

        }
    }

    Motor_Stop();

    *stationDistanceSteps += RightSteps;

    /* Check if obstacle seen from front sensor */
    if ((getUrm37Distance(moduleFront) > 2) && (getUrm37Distance(moduleFront) <= 10))
    {
        returnVal = OBST_SEEN;
    }

    /* Check if obstacle seen from left sensor */
    if ((getUrm37Distance(moduleLeft) > 2) && (getUrm37Distance(moduleLeft) <= 6))
    {
        returnVal = OBST_SEEN;
    }

    /* Check if obstacle seen from right sensor */
    if ((getUrm37Distance(moduleRight) > 2) && (getUrm37Distance(moduleRight) <= 6))
    {
        returnVal = OBST_SEEN;
    }

    return returnVal;
}

/* Turn left, comparing left and right
 * encoder values and adjusting both motor's speed if needed.*/
void Motor_Corrected_Left()
{

    /* Previous encoder measurement */
    uint32_t lastRightSteps;
    uint32_t lastLeftSteps;

    /* Encoder steps since last measurement */
    uint32_t newRightSteps;
    uint32_t newLeftSteps;

    Motor_Left(leftDuty, rightDuty); // Start driving forward

    /* Get encoder steps */
    lastRightSteps = RightSteps;
    lastLeftSteps = LeftSteps;
    Tachometer_Get(&LeftTach[z], &LeftDir, &LeftSteps, &RightTach[z], &RightDir, &RightSteps);
    newRightSteps = RightSteps - lastRightSteps;
    newLeftSteps = LeftSteps - lastLeftSteps;

    /* Adjust left motor's speed depending on how left and right steps compare */
    if (abs(newLeftSteps) < abs(newRightSteps))
    {
        leftDuty += 1;
        rightDuty -= 1;
        Motor_Left(leftDuty, rightDuty);
    }
    else if(abs(newLeftSteps) > abs(newRightSteps))
    {
        leftDuty -= 1;
        rightDuty += 1;
        Motor_Left(leftDuty, rightDuty);
    }
}

/* Turn right, comparing left and right
 * encoder values and adjusting both motor's speed if needed.*/
void Motor_Corrected_Right()
{

    /* Previous encoder measurement */
    uint32_t lastRightSteps;
    uint32_t lastLeftSteps;

    /* Encoder steps since last measurement */
    uint32_t newRightSteps;
    uint32_t newLeftSteps;

    Motor_Right(leftDuty, rightDuty); // Start driving forward

    /* Get encoder steps */
    lastRightSteps = RightSteps;
    lastLeftSteps = LeftSteps;
    Tachometer_Get(&LeftTach[z], &LeftDir, &LeftSteps, &RightTach[z], &RightDir, &RightSteps);
    newRightSteps = RightSteps - lastRightSteps;
    newLeftSteps = LeftSteps - lastLeftSteps;

    /* Adjust left motor's speed depending on how left and right steps compare */
    if (abs(newLeftSteps) < abs(newRightSteps))
    {
        leftDuty += 1;
        rightDuty -= 1;
        Motor_Right(leftDuty, rightDuty);
    }
    else if(abs(newLeftSteps) > abs(newRightSteps))
    {
        leftDuty -= 1;
        rightDuty += 1;
        Motor_Right(leftDuty, rightDuty);
    }
}


/* The robot turns some degrees, drives forward some distance,
 * and then calculates its distance and angle from the station. */
uint8_t turnDriveUpdate(double turnAngleDegrees, double distanceSteps,
                       double *stationAngleDegrees, double *stationDistanceSteps,
                       uint32_t moduleFront, uint32_t moduleLeft, uint32_t moduleRight)
{
    uint8_t returnVal = DEST_REACHED;
    double firstAngleDegrees = 0;   // Angle in degrees between path to station and new path
    double newStationDistanceSteps;  // New distance in steps to the station

    /* Left turn */
    if (turnAngleDegrees >= 0)
    {
        firstAngleDegrees = (*stationAngleDegrees - turnAngleDegrees);

        if (firstAngleDegrees >= 0)
        {
            Tachometer_Clear();

            RightSteps = 0;

            while(RightSteps < (TURN_DEG_TO_WHEEL_STEPS*turnAngleDegrees))
            {
                Motor_Corrected_Left();
                Tachometer_Get(LeftTach, &LeftDir, &LeftSteps,
                               RightTach, &RightDir, &RightSteps);
            }

            Motor_Stop();

            Tachometer_Clear();
            RightSteps = 0;

            /* Drive forward */
            while((RightSteps < distanceSteps)
                    && ((getUrm37Distance(moduleFront) < 3) || (getUrm37Distance(moduleFront) >= 10))
                    && ((getUrm37Distance(moduleLeft) < 3) || (getUrm37Distance(moduleLeft) >= 6))
                    && ((getUrm37Distance(moduleRight) < 3) || (getUrm37Distance(moduleRight) >= 6)))
            {
                while((RightSteps < distanceSteps)
                        && ((getUrm37Distance(moduleFront) < 3) || (getUrm37Distance(moduleFront) >= 10))
                        && ((getUrm37Distance(moduleLeft) < 3) || (getUrm37Distance(moduleLeft) >= 6))
                        && ((getUrm37Distance(moduleRight) < 3) || (getUrm37Distance(moduleRight) >= 6)))
                {
                    Motor_Corrected_Forward();
                    Tachometer_Get(LeftTach, &LeftDir, &LeftSteps,
                                   RightTach, &RightDir, &RightSteps);
                }
            }

            Motor_Stop();

            /* Check if obstacle seen from front sensor */
            if ((getUrm37Distance(moduleFront) > 2) && (getUrm37Distance(moduleFront) <= 10))
            {
                returnVal = OBST_SEEN;
            }

            /* Check if obstacle seen from left sensor */
            if ((getUrm37Distance(moduleLeft) > 2) && (getUrm37Distance(moduleLeft) <= 6))
            {
                returnVal = OBST_SEEN;
            }

            /* Check if obstacle seen from right sensor */
            if ((getUrm37Distance(moduleRight) > 2) && (getUrm37Distance(moduleRight) <= 6))
            {
                returnVal = OBST_SEEN;
            }

            /* Calculate and update distance and angle to station */

            newStationDistanceSteps = distanceToStation(*stationDistanceSteps, RightSteps,
                                                        (DEG_TO_RAD*firstAngleDegrees));

            *stationAngleDegrees = (180 - (RAD_TO_DEG*angleRadiansToStation(*stationDistanceSteps,
                                    newStationDistanceSteps, (DEG_TO_RAD*firstAngleDegrees))));
            *stationDistanceSteps = newStationDistanceSteps;
        }

        else
        {
            firstAngleDegrees = abs(firstAngleDegrees);

            Tachometer_Clear();

            RightSteps = 0;
            while(RightSteps < (TURN_DEG_TO_WHEEL_STEPS*turnAngleDegrees))
            {
                Motor_Corrected_Left();
                Tachometer_Get(LeftTach, &LeftDir, &LeftSteps,
                               RightTach, &RightDir, &RightSteps);
            }

            Motor_Stop();

            Tachometer_Clear();

            /* Drive forward */
            RightSteps = 0;
            while((RightSteps < distanceSteps)
                    && ((getUrm37Distance(moduleFront) < 3) || (getUrm37Distance(moduleFront) >= 10))
                    && ((getUrm37Distance(moduleLeft) < 3) || (getUrm37Distance(moduleLeft) >= 6))
                    && ((getUrm37Distance(moduleRight) < 3) || (getUrm37Distance(moduleRight) >= 6)))
            {
                while((RightSteps < distanceSteps)
                        && ((getUrm37Distance(moduleFront) < 3) || (getUrm37Distance(moduleFront) >= 10))
                        && ((getUrm37Distance(moduleLeft) < 3) || (getUrm37Distance(moduleLeft) >= 6))
                        && ((getUrm37Distance(moduleRight) < 3) || (getUrm37Distance(moduleRight) >= 6)))
                {
                    Motor_Corrected_Forward();
                    Tachometer_Get(LeftTach, &LeftDir, &LeftSteps,
                                   RightTach, &RightDir, &RightSteps);
                }
            }

            Motor_Stop();

            /* Check if obstacle seen from front sensor */
            if ((getUrm37Distance(moduleFront) > 2) && (getUrm37Distance(moduleFront) <= 10))
            {
                returnVal = OBST_SEEN;
            }

            /* Check if obstacle seen from left sensor */
            if ((getUrm37Distance(moduleLeft) > 2) && (getUrm37Distance(moduleLeft) <= 6))
            {
                returnVal = OBST_SEEN;
            }

            /* Check if obstacle seen from right sensor */
            if ((getUrm37Distance(moduleRight) > 2) && (getUrm37Distance(moduleRight) <= 6))
            {
                returnVal = OBST_SEEN;
            }

            /* Calculate and update distance and angle to station */

            newStationDistanceSteps = distanceToStation(*stationDistanceSteps, RightSteps,
                                                        (DEG_TO_RAD*firstAngleDegrees));

            *stationAngleDegrees = (180 + (RAD_TO_DEG*angleRadiansToStation(*stationDistanceSteps,
                                    newStationDistanceSteps, (DEG_TO_RAD*firstAngleDegrees))));
            *stationDistanceSteps = newStationDistanceSteps;
        }

        return returnVal;
    }

    /* Right turn */
    else
    {
        firstAngleDegrees = (360 - (*stationAngleDegrees + abs(turnAngleDegrees)));

        if (firstAngleDegrees >= 0)
        {
            Tachometer_Clear();

            LeftSteps = 0;

            while(LeftSteps < abs(TURN_DEG_TO_WHEEL_STEPS*turnAngleDegrees))
            {
                Motor_Corrected_Right();
                Tachometer_Get(LeftTach, &LeftDir, &LeftSteps,
                               RightTach, &RightDir, &RightSteps);
            }

            Motor_Stop();

            Tachometer_Clear();
            LeftSteps = 0;

            /* Drive forward */
            while((LeftSteps < distanceSteps)
                    && ((getUrm37Distance(moduleFront) < 3) || (getUrm37Distance(moduleFront) >= 10))
                    && ((getUrm37Distance(moduleLeft) < 3) || (getUrm37Distance(moduleLeft) >= 6))
                    && ((getUrm37Distance(moduleRight) < 3) || (getUrm37Distance(moduleRight) >= 6)))
            {
                while((LeftSteps < distanceSteps)
                                    && ((getUrm37Distance(moduleFront) < 3) || (getUrm37Distance(moduleFront) >= 10))
                                    && ((getUrm37Distance(moduleLeft) < 3) || (getUrm37Distance(moduleLeft) >= 6))
                                    && ((getUrm37Distance(moduleRight) < 3) || (getUrm37Distance(moduleRight) >= 6)))
                {
                    Motor_Corrected_Forward();
                    Tachometer_Get(LeftTach, &LeftDir, &LeftSteps,
                                   RightTach, &RightDir, &RightSteps);
                }
            }

            Motor_Stop();

            /* Check if obstacle seen from front sensor */
            if ((getUrm37Distance(moduleFront) > 2) && (getUrm37Distance(moduleFront) <= 10))
            {
                returnVal = OBST_SEEN;
            }

            /* Check if obstacle seen from left sensor */
            if ((getUrm37Distance(moduleLeft) > 2) && (getUrm37Distance(moduleLeft) <= 6))
            {
                returnVal = OBST_SEEN;
            }

            /* Check if obstacle seen from right sensor */
            if ((getUrm37Distance(moduleRight) > 2) && (getUrm37Distance(moduleRight) <= 6))
            {
                returnVal = OBST_SEEN;
            }

            /* Calculate and update distance and angle to station */

            newStationDistanceSteps = distanceToStation(*stationDistanceSteps, LeftSteps,
                                                        (DEG_TO_RAD*firstAngleDegrees));

            *stationAngleDegrees = (180 + (RAD_TO_DEG*angleRadiansToStation(*stationDistanceSteps,
                                    newStationDistanceSteps, (DEG_TO_RAD*firstAngleDegrees))));
            *stationDistanceSteps = newStationDistanceSteps;


            Tachometer_Clear();
        }

        else
        {
            firstAngleDegrees = abs(firstAngleDegrees);

            Tachometer_Clear();
            LeftSteps = 0;

            while(LeftSteps < abs(TURN_DEG_TO_WHEEL_STEPS*turnAngleDegrees))
            {
                Motor_Corrected_Right();
                Tachometer_Get(LeftTach, &LeftDir, &LeftSteps,
                               RightTach, &RightDir, &RightSteps);
            }

            Motor_Stop();

            Tachometer_Clear();
            LeftSteps = 0;

            /* Drive forward */
            while((LeftSteps < distanceSteps)
                    && ((getUrm37Distance(moduleFront) < 3) || (getUrm37Distance(moduleFront) >= 10))
                    && ((getUrm37Distance(moduleLeft) < 3) || (getUrm37Distance(moduleLeft) >= 6))
                    && ((getUrm37Distance(moduleRight) < 3) || (getUrm37Distance(moduleRight) >= 6)))
            {
                while((LeftSteps < distanceSteps)
                                    && ((getUrm37Distance(moduleFront) < 3) || (getUrm37Distance(moduleFront) >= 10))
                                    && ((getUrm37Distance(moduleLeft) < 3) || (getUrm37Distance(moduleLeft) >= 6))
                                    && ((getUrm37Distance(moduleRight) < 3) || (getUrm37Distance(moduleRight) >= 6)))
                {
                    Motor_Corrected_Forward();
                    Tachometer_Get(LeftTach, &LeftDir, &LeftSteps,
                                   RightTach, &RightDir, &RightSteps);
                }
            }

            Motor_Stop();

            /* Check if obstacle seen from front sensor */
            if ((getUrm37Distance(moduleFront) > 2) && (getUrm37Distance(moduleFront) <= 10))
            {
                returnVal = OBST_SEEN;
            }

            /* Check if obstacle seen from left sensor */
            if ((getUrm37Distance(moduleLeft) > 2) && (getUrm37Distance(moduleLeft) <= 6))
            {
                returnVal = OBST_SEEN;
            }

            /* Check if obstacle seen from right sensor */
            if ((getUrm37Distance(moduleRight) > 2) && (getUrm37Distance(moduleRight) <= 6))
            {
                returnVal = OBST_SEEN;
            }

            /* Calculate and update distance and angle to station */

            newStationDistanceSteps = distanceToStation(*stationDistanceSteps, LeftSteps,
                                                        (DEG_TO_RAD*firstAngleDegrees));

            *stationAngleDegrees = (180 - (RAD_TO_DEG*angleRadiansToStation(*stationDistanceSteps,
                                    newStationDistanceSteps, (DEG_TO_RAD*firstAngleDegrees))));
            *stationDistanceSteps = newStationDistanceSteps;


            Tachometer_Clear();
        }

        return returnVal;

    }
}

/* --COPYRIGHT--,BSD
 * Copyright (c) 2017, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * --/COPYRIGHT--*/
/*******************************************************************************
 * motorAdvanced.h
 *
 * Author: Nick Brubaker
 *
 * Date: 3/20/2019
 *
 * Revision: 1
 *
 * Description: Declarations of advanced motor functions for dynamically
 *              adjusting left motor speed and updating distance and angle
 *              from station while driving forward and/or turning.
 *              In some strange, not understood cases an incorrect angle
 *              to the station is calculated.
 ******************************************************************************/

#ifndef _MOTOR_ADVANCED_H
#define _MOTOR_ADVANCED_H


#define CHECK_DELAY 0 // value to increment up to before comparing left and right encoder values
#define TACHBUFF 10 // number of elements in tachometer array

#define DEST_REACHED 0  // Intended destination reached
#define OBST_SEEN 1    // Obstacle blcking path

/* Conversions */
#define DEG_TO_RAD 0.017453
#define RAD_TO_DEG 57.29578
#define WHEEL_STEPS_TO_TURN_DEG 0.5
#define TURN_DEG_TO_WHEEL_STEPS 2


#include <stdint.h>
#include <stdint.h>
#include <math.h>
#include "../inc/Motor.h"
#include "../inc/Tachometer.h"
#include "../inc/Clock.h"
#include "urm37.h"


uint16_t LeftTach[TACHBUFF];             // tachometer period of left wheel (number of 0.0833 usec cycles to rotate 1/360 of a wheel rotation)
enum TachDirection LeftDir;              // direction of left rotation (FORWARD, STOPPED, REVERSE)
int32_t LeftSteps;                       // number of tachometer steps of left wheel (units of 220/360 = 0.61 mm traveled)
uint16_t RightTach[TACHBUFF];            // tachometer period of right wheel (number of 0.0833 usec cycles to rotate 1/360 of a wheel rotation)
enum TachDirection RightDir;             // direction of right rotation (FORWARD, STOPPED, REVERSE)
int32_t RightSteps;                      // number of tachometer steps of right wheel (units of 220/360 = 0.61 mm traveled)
int z;  // index in tachometer array

/* Duty cycles */
uint16_t leftDuty;
uint16_t rightDuty;


/* Calculates robot's distance from station.
 *  ~param firstDistance is the first distance the robot drove from the station
 *   in encoder steps.
 *  ~param secondDistance is the second distance the robot drove from the station
 *   in encoder steps.
 *  ~param firstAngleRadians is the angle the robot turned, in radians,
 *   before driving forward the second time.
 * Returns a double representing the robot's current angle from the station in radians. */
double distanceToStation(double firstDistance, double secondDistance, double firstAngleRadians);

/* Calculates robot's angle from station. Calculation requires
 * robot's distance from station so functions should only be called after  distanceToStation().
 *  ~param firstDistance is the first distance the robot drove from the station
 *   in encoder steps.
 *  ~param distanceToStation is the robot's current distance from the station in encoder steps.
 *  ~param firstAngleRadians is the angle the robot turned, in radians,
 *   before driving forward the second time.
 * Returns a double representing the robot's current angle from the station in radians. */
double angleRadiansToStation(double firstDistance, double distanceToStation, double firstAngleRadians);


/* Drive straight forward, comparing left and right
 * encoder values and adjusting both motor's speed if needed.
 *      ~param duty is the duty cycle of both wheels (0 to 14,998)
 * In order for the correction to continually happen, this function must
 * be continually called */
void Motor_Corrected_Forward();

/* The robot drives forward and then calculates its new distance from the station.
 * Assumes the robot is facing the station or at an 180 degree angle from the station.
 *  ~param distanceSteps is the distance in steps to drive forward.
 *  ~param stationDistanceSteps is a pointer to the robot's current
 *   distance in steps from the station.
 *  ~param moduleFront is the UART module connected to the forward-facing
 *   ultrasonic sensor
 *  ~param moduleLeft is the UART module connected to the left-facing
 *   ultrasonic sensor
 *  ~param moduleRight is the UART module connected to the right-facing
 *   ultrasonic sensor
 * Returns 0 (DEST_REACHED) if specified distance forward reached and
 * 1 (OBST_SEEN) if a path-blocking object was detected with the ultrasonic sensor. */
uint8_t Motor_Corrected_Forward_Steps(double distanceSteps, double *stationDistanceSteps,
                                   uint32_t moduleFront, uint32_t moduleLeft, uint32_t moduleRight);

/* Turn left, comparing left and right
 * encoder values and adjusting both motor's speed if needed.
 * In order for the correction to continually happen, this function must
 * be continually called */
void Motor_Corrected_Left();

/* Turn right, comparing left and right
 * encoder values and adjusting both motor's speed if needed.
 * In order for the correction to continually happen, this function must
 * be continually called */
void Motor_Corrected_Right();


/* The robot turns some degrees, drives forward some distance,
 * and then calculates its distance and angle from the station.
 *  ~param turnAngleDegrees is the angle in degrees to turn.
 *      A positive value indicates a left turn and a negative value
 *      indicates a right turn.
 *  ~param distanceSteps is the distance in steps to drive forward.
 *  ~param stationAngleDegrees is a pointer to the robot's current
 *   left angle in degrees from the station.
 *  ~param stationDistanceSteps is a pointer to the robot's current
 *   distance in steps from the station.
 *  ~param moduleFront is the UART module connected to the forward-facing
 *   ultrasonic sensor
 *  ~param moduleLeft is the UART module connected to the left-facing
 *   ultrasonic sensor
 *  ~param moduleRight is the UART module connected to the right-facing
 *   ultrasonic sensor
 * In some strange cases an incorrect angle to the station is calculated.
 * Returns 0 (DEST_REACHED) if specified distance forward reached and
 * 1 (OBST_SEEN) if a path-blocking object was detected with the ultrasonic sensor. */
uint8_t turnDriveUpdate(double turnAngleDegrees, double distanceSteps,
                       double *stationAngleDegrees, double *stationDistanceSteps,
                       uint32_t moduleFront, uint32_t moduleLeft, uint32_t moduleRight);


#endif

/* --COPYRIGHT--,BSD
 * Copyright (c) 2017, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * --/COPYRIGHT--*/
/*******************************************************************************
 * robot.c
 *
 * Author: Nick Brubaker
 *
 * Date: 4/25/2019
 *
 * Description: Main program that runs on the robot. The robot drives forward
 *              to leave the station, turns and drives randomly thrice
 *              (stopping before hitting any obstacles encountered),
 *              heads back using the calculated distance and angle from the
 *              station (avoiding any obstacles encountered), uses its IR receiver
 *              to precisely dock with the station, and waits 30 minutes to
 *              charge its batteries before leaving the station again.
 ******************************************************************************/

#include <stdint.h>
#include <stdio.h>
#include <math.h>
#include <time.h>
#include "motorAdvanced.h"
#include "avoidAdvanced.h"
#include "dock.h"
#include "getFreq.h"


#include "msp.h"
#include "../inc/Clock.h"
#include "../inc/CortexM.h"
#include "../inc/LaunchPad.h"
#include "../inc/Motor.h"
#include "../inc/Nokia5110.h"
#include "../inc/Tachometer.h"
#include "../inc/TimerA1.h"
#include "../inc/TA3InputCapture.h"
#include "../inc/TExaS.h"
#include "../inc/FlashProgram.h"
#include "../inc/Bump.h"

double statAngleVal;
double statDistVal;


void main(void)
{

    int upperBoundDis = 1000;     // upper bound for random number generation: reliant on current position vs. boundary
    int lowerBoundDis = 100;     // lower bound for random number generation: default 15 cm, but can be changed
    int upperBoundAng = 320;
    int lowerBoundAng = 30;
    double randDistance = 0;    // random distance in m
    double randAngle = 0;       // random turn angle in degrees


    Clock_Init48MHz();                     // set system clock to 48 MHz
    Tachometer_Init();
    Motor_Init();
    EnableInterrupts();
    configUrm37(EUSCI_A0_BASE);
    configUrm37(EUSCI_A1_BASE);
    configUrm37(EUSCI_A2_BASE);

    statAngleVal = 180;
    statDistVal = 0;

    double *angleFromStat = &statAngleVal;
    double *distFromStat = &statDistVal;

    uint8_t i = 0;

    /* Initial duty cycles */
    leftDuty = 2000;
    rightDuty = 2000;

    while(1)
    {
      // Venture out from station
      Clock_Delay1ms(5000);
      Motor_Corrected_Forward_Steps(2000, distFromStat, EUSCI_A0_BASE, EUSCI_A1_BASE, EUSCI_A2_BASE);

      // Turn and drive randomly 3 times
      for(i = 0; i < 3; i++)
      {
          // Randomize random number generator
          srand(*distFromStat);

          // Get random values
          randDistance = (rand() % (upperBoundDis - lowerBoundDis + 1)) + lowerBoundDis;
          randAngle = (rand() % (upperBoundAng - lowerBoundAng + 1)) + lowerBoundAng;

          // Do a random turn and drive
          turnDriveUpdate(randAngle, randDistance, angleFromStat, distFromStat,
                                EUSCI_A0_BASE, EUSCI_A1_BASE, EUSCI_A2_BASE);
      }

      /* Turn and drive towards front of station */
      uint8_t driveResult = turnDriveUpdate(*angleFromStat, *distFromStat, angleFromStat, distFromStat,
                      EUSCI_A0_BASE, EUSCI_A1_BASE, EUSCI_A2_BASE);

      /* Avoid obstacles encountered and continue toward station */
      while(*distFromStat > 300)
      {
          if(driveResult == OBST_SEEN)
          {
              turnDriveUpdate(90, 500, angleFromStat, distFromStat,
                                    EUSCI_A0_BASE, EUSCI_A1_BASE, EUSCI_A2_BASE);
              driveResult = turnDriveUpdate(*angleFromStat, *distFromStat, angleFromStat, distFromStat,
                                    EUSCI_A0_BASE, EUSCI_A1_BASE, EUSCI_A2_BASE);
          }

          else
          {
              driveResult = Motor_Corrected_Forward_Steps(-*distFromStat, distFromStat, EUSCI_A0_BASE, EUSCI_A1_BASE, EUSCI_A2_BASE);
          }
      }

      dockCount = 0;

      // Dock with station
      dock();

      // Wait 30 minutes (to charge batteries) and then repeat
      Clock_Delay1ms(1800000);
}

/* --COPYRIGHT--,BSD
 * Copyright (c) 2017, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * --/COPYRIGHT--*/
/******************************************************************************
 * Robotics System Learning Kit
 * Solar Charging System
 * Station Code
 *
 * Description: Code that runs on the station MSP432.
 *  Generates signals for IR emitters and opens/closes connection
 *  between station and robot. Station isn't connecting to robot
 *  when it should, maybe something is wrong with the conversion of ADC values?
 *
 * Author: Nick Brubaker
 * Version: 1.0
 * Date: 2/25/19
 * *******************************************************************************/

/* DriverLib Includes */
#include <ti/devices/msp432p4xx/driverlib/driverlib.h>

/* Standard Includes */
#include <stdint.h>
#include <stdbool.h>
#include <string.h>

#define COMP_VAL 25  // Timer interrupt value
#define CONVERT_STATION_VOLTS 0.000568294     // Converts ADC value to station battery voltage
#define CONVERT_ROBOT_AMPS 0.000122058 // Converts ADC value to robot battery current
#define MIN_STATION_VOLTS 6         // Disconnect robot battery if station battery below this voltage
#define MIN_ROBOT_AMPS 0.01            // Disconnect robot battery if current draw below this current

static uint16_t ADCResult[2];   // To store ADC results

static uint8_t IRCount; // Counter that increments when timer overflows (for IR signals)
static uint32_t powerCount; // Counter that increments when timer overflows (for connection to robot battery)

static double stationVolts = 0;   // Station battery voltage
static double robotAmps = 0;   // Robot current draw

/* Timer Configuration Parameters*/
const Timer_A_ContinuousModeConfig continuousModeConfig =
{

        TIMER_A_CLOCKSOURCE_ACLK,           // ACLK Clock Source
        TIMER_A_CLOCKSOURCE_DIVIDER_1,      // ACLK/1 = 131.072 KHz
        TIMER_A_TAIE_INTERRUPT_DISABLE,     // Disable Timer ISR
        TIMER_A_DO_CLEAR                    // Clear Counter

};

const Timer_A_CompareModeConfig compareModeConfig =
{

        TIMER_A_CAPTURECOMPARE_REGISTER_1,          // Register for compared value
        TIMER_A_CAPTURECOMPARE_INTERRUPT_ENABLE,    // Enable Interrupt
        TIMER_A_OUTPUTMODE_OUTBITVALUE,             // Not using output mode
        COMP_VAL                                    // Interrupt triggered when this value reached

};


/* Configure pins:
 * - 2.7 as output for power management
 * - 2.3, 2.4, 2.5, and 2.6 as outputs for IR emitters */
void config_GPIO()
{

    /* Configure pin 1.0 as output and set high for testing */
    MAP_GPIO_setAsOutputPin(GPIO_PORT_P1, GPIO_PIN0);
    MAP_GPIO_setOutputHighOnPin(GPIO_PORT_P1, GPIO_PIN0);

    /* Configure pins 2.3, 2.4, 2.5, 2.6, and 2.7 as outputs */
    MAP_GPIO_setAsOutputPin(GPIO_PORT_P2, GPIO_PIN3 | GPIO_PIN4 | GPIO_PIN5 | GPIO_PIN6 | GPIO_PIN7);

    /* Set all used pins initially high */
    MAP_GPIO_setOutputHighOnPin(GPIO_PORT_P2, GPIO_PIN3 | GPIO_PIN4 | GPIO_PIN5 | GPIO_PIN6 | GPIO_PIN7);

    return;

}


/* Configure timer for continous mode with overflow interrupts
 * and source clock ACLK */
void config_timer()
{

    /* Starting and enabling ACLK (128kHz) */
    MAP_CS_setReferenceOscillatorFrequency(CS_REFO_128KHZ);
    MAP_CS_initClockSignal(CS_ACLK, CS_REFOCLK_SELECT, CS_CLOCK_DIVIDER_1);

    /* Configuring continuous mode and compare value */
    MAP_Timer_A_configureContinuousMode(TIMER_A0_BASE, &continuousModeConfig);
    MAP_Timer_A_initCompare(TIMER_A0_BASE, &compareModeConfig);

    /* Enabling interrupts */
    MAP_Interrupt_enableInterrupt(INT_TA0_N);

    /* Starting the Timer_A0 in continuous mode */
    MAP_Timer_A_startCounter(TIMER_A0_BASE, TIMER_A_CONTINUOUS_MODE);

    return;

}


/* Configure ADC with P5.5 and P5.4 as inputs, output stored in ADC result,
 * and mode as continuous multi-channel */
void config_ADC()
{
    /* Zero-filling result location */
    memset(ADCResult, 0x00, 2 * sizeof(uint16_t));

    /* Initializing ADC (MCLK/1/1) */
    MAP_ADC14_enableModule();
    MAP_ADC14_initModule(ADC_CLOCKSOURCE_MCLK, ADC_PREDIVIDER_1, ADC_DIVIDER_1,
            0);

    /* Configuring P5.5 and P5.4 for Analog In */
    MAP_GPIO_setAsPeripheralModuleFunctionInputPin(GPIO_PORT_P5,
            GPIO_PIN5 | GPIO_PIN4, GPIO_TERTIARY_MODULE_FUNCTION);


    /* Configuring ADC Memory (ADC_MEM0 and ADC_MEM1 (A0 and A1)  with repeat) */
    MAP_ADC14_configureMultiSequenceMode(ADC_MEM0, ADC_MEM1, true);
    MAP_ADC14_configureConversionMemory(ADC_MEM0,
            ADC_VREFPOS_AVCC_VREFNEG_VSS,
            ADC_INPUT_A0, false);
    MAP_ADC14_configureConversionMemory(ADC_MEM1,
            ADC_VREFPOS_AVCC_VREFNEG_VSS,
            ADC_INPUT_A1, false);

    /* Enabling the interrupt when a conversion on channel 0 and 1
     * is complete and enabling conversions */
    MAP_ADC14_enableInterrupt(ADC_INT0);
    MAP_ADC14_enableInterrupt(ADC_INT1);

    /* Enabling Interrupts */
    MAP_Interrupt_enableInterrupt(INT_ADC14);
    MAP_Interrupt_enableMaster();

    /* Setting up the sample timer to automatically step through the sequence
     * convert.
     */
    MAP_ADC14_enableSampleTimer(ADC_AUTOMATIC_ITERATION);

    /* Triggering the start of the sample */
    MAP_ADC14_enableConversion();
    MAP_ADC14_toggleConversionTrigger();

    return;
}


int main(void)
{
    // Stop Watchdog
    MAP_WDT_A_holdTimer();

    // Configure GPIO
    config_GPIO();

    // Configure timer
    config_timer();

    // Configure ADC
    //config_ADC();

    // Enabling interrupts and going to sleep
    MAP_Interrupt_enableSleepOnIsrExit();

    // Enabling MASTER interrupts
    MAP_Interrupt_enableMaster();

    // Go into low power mode in an infinite loop and wait for interrupts
    while(1)
    {
        MAP_PCM_gotoLPM0();
    }

    while(1);
}

//******************************************************************************
//
//  TIMERA interrupt vector service routine
//
//******************************************************************************
void TA0_N_IRQHandler(void)
{
    MAP_Timer_A_clearCaptureCompareInterrupt(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_1);

    powerCount++; // Increment power counter
    IRCount++; // Increment IR counter

    /* Toggle GPIO pins based on counter value */

    MAP_GPIO_toggleOutputOnPin(GPIO_PORT_P2, GPIO_PIN3); // ~2000 Hz

    if (IRCount % 2 == 0)
    {
        MAP_GPIO_toggleOutputOnPin(GPIO_PORT_P2, GPIO_PIN4); // ~1000 Hz
    }

    if (IRCount % 4 == 0)
    {
        MAP_GPIO_toggleOutputOnPin(GPIO_PORT_P2, GPIO_PIN5); // ~500 Hz
    }

    if (IRCount % 8 == 0)
    {
        MAP_GPIO_toggleOutputOnPin(GPIO_PORT_P2, GPIO_PIN6); // ~250 Hz
        IRCount = 0;
    }

    /* Update compare value */
    MAP_Timer_A_setCompareValue(TIMER_A0_BASE, TIMER_A_CAPTURECOMPARE_REGISTER_1,
                                MAP_Timer_A_getCounterValue(TIMER_A0_BASE) + COMP_VAL);

}


//******************************************************************************
//
//  ADC interrupt vector service routine - called whenever a conversion is
//  completed and placed in ADC_MEM0 or ADC_MEM1
//
//******************************************************************************
void ADC14_IRQHandler(void)
{

    uint64_t status;

    status = MAP_ADC14_getEnabledInterruptStatus();
    MAP_ADC14_clearInterruptFlag(status);

    if(status & (ADC_INT0 | ADC_INT1))   // Check if conversion is from valid source
    {
        ADCResult[0] = MAP_ADC14_getResult(ADC_MEM0);
        ADCResult[1] = MAP_ADC14_getResult(ADC_MEM1);

        stationVolts = ADCResult[0] * CONVERT_STATION_VOLTS;
        robotAmps = ADCResult[1] * CONVERT_ROBOT_AMPS;

        /* Diconnect robot battery from station battery if current or voltage low enough */
        if ((stationVolts < MIN_STATION_VOLTS) ||
                (robotAmps < MIN_ROBOT_AMPS))
        {
            MAP_GPIO_setOutputLowOnPin(GPIO_PORT_P2, GPIO_PIN7);
            MAP_GPIO_setOutputLowOnPin(GPIO_PORT_P1, GPIO_PIN0);

            for(powerCount = 0; powerCount < 1000000; powerCount++); // Wait a while

            MAP_GPIO_setOutputHighOnPin(GPIO_PORT_P2, GPIO_PIN7);
            MAP_GPIO_setOutputHighOnPin(GPIO_PORT_P1, GPIO_PIN0);
        }
    }

}

/* --COPYRIGHT--,BSD
 * Copyright (c) 2017, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * --/COPYRIGHT--*/
/*******************************************************************************
 * urm37.c
 *
 * Author: Nick Brubaker
 *
 * Date: 11/26/2018
 *
 * Description:  URM37 Driver function implementations.
 *
 ******************************************************************************/

/* Standard Includes */
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <stdio.h>

/* Other Includes */
#include <urm37.h>

//![Simple UART Config]
/* UART Configuration Parameter. These are the configuration parameters to
 * make the eUSCI A UART module to operate with a 9600 baud rate. These
 * values were calculated using the online calculator that TI provides
 * at:
 *http://software-dl.ti.com/msp430/msp430_public_sw/mcu/msp430/MSP430BaudRateConverter/index.html
 */

/* Configures UART for receiving urmDistance data from URM37 */
void configUrm37(uint32_t module)
{
    /* For storing values representing RX and TX pins */
    uint16_t  port;
    uint16_t  rxPin;
    uint16_t  txPin;
    uint16_t  modInterrupt; // Needed to configure interrupts for an instance

    /* Determine pins from module */
    switch(module) {

       case EUSCI_A0_BASE  :
          port = GPIO_PORT_P1;
          rxPin = GPIO_PIN2;
          txPin = GPIO_PIN3;
          modInterrupt = INT_EUSCIA0;
          break;

       case EUSCI_A1_BASE  :
          port = GPIO_PORT_P2;
          rxPin = GPIO_PIN2;
          txPin = GPIO_PIN3;
          modInterrupt = INT_EUSCIA1;
          break;

       case EUSCI_A2_BASE  :
          port = GPIO_PORT_P3;
          rxPin = GPIO_PIN2;
          txPin = GPIO_PIN3;
          modInterrupt = INT_EUSCIA2;
          break;

       case EUSCI_A3_BASE  :
          port = GPIO_PORT_P9;
          rxPin = GPIO_PIN6;
          txPin = GPIO_PIN7;
          modInterrupt = INT_EUSCIA3;
          break;

       default :
           printf("Invalid module chosen.");
           while(1);
    }


    /* Halting WDT  */
    MAP_WDT_A_holdTimer();

    /* Setting pins to UART mode */
    MAP_GPIO_setAsPeripheralModuleFunctionInputPin(port,
            rxPin | txPin, GPIO_PRIMARY_MODULE_FUNCTION);

    /* Setting DCO to 12MHz */
    CS_setDCOCenteredFrequency(CS_DCO_FREQUENCY_12);

    //![Simple UART Config]
    /* UART Configuration Parameter. These are the configuration parameters to
     * make the eUSCI A UART module to operate with a 9600 baud rate. These
     * values were calculated using the online calculator that TI provides
     * at:
     *http://software-dl.ti.com/msp430/msp430_public_sw/mcu/msp430/MSP430BaudRateConverter/index.html
     */
    const eUSCI_UART_Config uartConfig =
    {
            EUSCI_A_UART_CLOCKSOURCE_SMCLK,          // SMCLK Clock Source
            78,                                     // BRDIV = 78
            2,                                       // UCxBRF = 2
            0,                                       // UCxBRS = 0
            EUSCI_A_UART_NO_PARITY,                  // No Parity
            EUSCI_A_UART_LSB_FIRST,                  // LSB First
            EUSCI_A_UART_ONE_STOP_BIT,               // One stop bit
            EUSCI_A_UART_MODE,                       // UART mode
            EUSCI_A_UART_OVERSAMPLING_BAUDRATE_GENERATION  // Oversampling
    };
    //![Simple UART Example]

    /* Configuring UART Module */
    MAP_UART_initModule(module, &uartConfig);

    /* Enable UART module */
    MAP_UART_enableModule(module);

    /* Enabling interrupts */
    MAP_UART_enableInterrupt(module, EUSCI_A_UART_RECEIVE_INTERRUPT);
    MAP_Interrupt_enableInterrupt(modInterrupt);
    MAP_Interrupt_enableMaster();

    i = 0;
    urmDistance = 0;
}

/* Gets current urmDistance (in cm) from sensor to object in front of sensor */
uint16_t getUrm37Distance(uint32_t module)
{
    /* Request urmDistance reading */
    MAP_UART_transmitData(module, 0x22); // Request urmDistance data
    MAP_UART_transmitData(module, 0x00); // No special config
    MAP_UART_transmitData(module, 0x00); // No special config
    MAP_UART_transmitData(module, 0x22); // Standard request ending

    while(i < 4); // Wait until all data has been received
    i = 0;

    return urmDistance * TO_CM_CONVERSION_FACTOR;
}

/* EUSCI A0 UART ISR - Stores received urmDistance */
void EUSCIA0_IRQHandler(void)
{
    uint32_t status = MAP_UART_getEnabledInterruptStatus(EUSCI_A0_BASE);

    MAP_UART_clearInterruptFlag(EUSCI_A0_BASE, status);

    /* If UART interrupt received */
    if(status & EUSCI_A_UART_RECEIVE_INTERRUPT_FLAG)
    {
        uint8_t received;

        switch(i) {

            case 0  :
                received = MAP_UART_receiveData(EUSCI_A0_BASE); // 0x22

                urmDistance = 0;
                i++;
                break;

            case 1  :
                received = MAP_UART_receiveData(EUSCI_A0_BASE); // MSB of urmDistance

                urmDistance |= received;
                urmDistance = urmDistance << 8;
                i++;
                break;

            case 2  :
                received = MAP_UART_receiveData(EUSCI_A0_BASE); // LSB of urmDistance

                urmDistance |= received;
                i++;
                break;

            case 3  :
                received = MAP_UART_receiveData(EUSCI_A0_BASE); // SUM

                i++;
                break;

            default :
                perror("Sensor data incorrectly received.");
                //while(1);
        }
    }
}

/* EUSCI A1 UART ISR - Stores received urmDistance */
void EUSCIA1_IRQHandler(void)
{
    uint32_t status = MAP_UART_getEnabledInterruptStatus(EUSCI_A1_BASE);

    MAP_UART_clearInterruptFlag(EUSCI_A1_BASE, status);

    /* If UART interrupt received */
    if(status & EUSCI_A_UART_RECEIVE_INTERRUPT_FLAG)
    {
        uint8_t received;

        switch(i) {

            case 0  :
                received = MAP_UART_receiveData(EUSCI_A1_BASE); // 0x22

                urmDistance = 0;
                i++;
                break;

            case 1  :
                received = MAP_UART_receiveData(EUSCI_A1_BASE); // MSB of urmDistance

                urmDistance |= received;
                urmDistance = urmDistance << 8;
                i++;
                break;

            case 2  :
                received = MAP_UART_receiveData(EUSCI_A1_BASE); // LSB of urmDistance

                urmDistance |= received;
                i++;
                break;

            case 3  :
                received = MAP_UART_receiveData(EUSCI_A1_BASE); // SUM

                i++;
                break;

            default :
                perror("Sensor data incorrectly received.");
        }
    }
}

/* EUSCI A2 UART ISR - Stores received urmDistance */
void EUSCIA2_IRQHandler(void)
{
    uint32_t status = MAP_UART_getEnabledInterruptStatus(EUSCI_A2_BASE);

    MAP_UART_clearInterruptFlag(EUSCI_A2_BASE, status);

    /* If UART interrupt received */
    if(status & EUSCI_A_UART_RECEIVE_INTERRUPT_FLAG)
    {
        uint8_t received;

        switch(i) {

            case 0  :
                received = MAP_UART_receiveData(EUSCI_A2_BASE); // 0x22

                urmDistance = 0;
                i++;
                break;

            case 1  :
                received = MAP_UART_receiveData(EUSCI_A2_BASE); // MSB of urmDistance

                urmDistance |= received;
                urmDistance = urmDistance << 8;
                i++;
                break;

            case 2  :
                received = MAP_UART_receiveData(EUSCI_A2_BASE); // LSB of urmDistance

                urmDistance |= received;
                i++;
                break;

            case 3  :
                received = MAP_UART_receiveData(EUSCI_A2_BASE); // SUM

                i++;
                break;

            default :
                perror("Sensor data incorrectly received.");
        }
    }
}

/* EUSCI A3 UART ISR - Stores received urmDistance */
void EUSCIA3_IRQHandler(void)
{
    uint32_t status = MAP_UART_getEnabledInterruptStatus(EUSCI_A3_BASE);

    MAP_UART_clearInterruptFlag(EUSCI_A3_BASE, status);

    /* If UART interrupt received */
    if(status & EUSCI_A_UART_RECEIVE_INTERRUPT_FLAG)
    {
        uint8_t received;

        switch(i) {

            case 0  :
                received = MAP_UART_receiveData(EUSCI_A3_BASE); // 0x22

                urmDistance = 0;
                i++;
                break;

            case 1  :
                received = MAP_UART_receiveData(EUSCI_A3_BASE); // MSB of urmDistance

                urmDistance |= received;
                urmDistance = urmDistance << 8;
                i++;
                break;

            case 2  :
                received = MAP_UART_receiveData(EUSCI_A3_BASE); // LSB of urmDistance

                urmDistance |= received;
                i++;
                break;

            case 3  :
                received = MAP_UART_receiveData(EUSCI_A3_BASE); // SUM

                i++;
                break;

            default :
                perror("Sensor data incorrectly received.");
        }
    }
}

/* --COPYRIGHT--,BSD
 * Copyright (c) 2017, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * --/COPYRIGHT--*/
/*******************************************************************************
 * urm37.h
 *
 * Author: Nick Brubaker
 *
 * Date: 11/26/2018
 *
 * Description:  URM37 Driver functions for configuring serial communication
 * with the URM37 and receiving the sensor's distance data.
 *
 ******************************************************************************/

#ifndef _URM37_H
#define _URM37_H

#define TO_CM_CONVERSION_FACTOR 0.718

/* DriverLib Includes */
#include <ti/devices/msp432p4xx/driverlib/driverlib.h>

uint8_t i; // For interpreting individual bytes received
volatile uint16_t urmDistance; // Stores distance data from sensor

/* Configures UART for receiving distance data from the URM37.
 *  ~ param module is the instance of the eUSCI A (UART) module.
 *    Each instance uses a different pair of pins for RX and TX.
 *    Valid instances and the pins they map to are:
          - EUSCI_A0_BASE (P1.2 RX, P1.3 TX)
          - EUSCI_A1_BASE (P2.2 RX, P2.3 TX)
          - EUSCI_A2_BASE (P3.2 RX, P3.3 TX)
          - EUSCI_A3_BASE (P9.6 RX, P9.7 TX) */
void configUrm37(uint32_t module);

/* Gets current distance from sensor to object in front of sensor.
 *  ~ param module is the instance of the eUSCI A (UART) module.
 *    Each instance uses a different pair of pins for RX and TX.
 *    Valid instances and the pins they map to are:
          - EUSCI_A0_BASE (P1.2 RX, P1.3 TX)
          - EUSCI_A1_BASE (P2.2 RX, P2.3 TX)
          - EUSCI_A2_BASE (P3.2 RX, P3.3 TX)
          - EUSCI_A3_BASE (P9.6 RX, P9.7 TX)
 * Returns an 16 bit value in centimeters.
 * Values of 2 or 47054 mean the sensor didn't correctly receive an
 * echo of the signal it sent out (objects are too close or far away) */
uint16_t getUrm37Distance(uint32_t module);

/* EUSCI UART ISRs - Store and interpret distance as it is received */
void EUSCIA0_IRQHandler(void);
void EUSCIA1_IRQHandler(void);
void EUSCIA2_IRQHandler(void);
void EUSCIA3_IRQHandler(void);

#endif

/* --COPYRIGHT--,BSD
 * Copyright (c) 2017, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * --/COPYRIGHT--*/
/*******************************************************************************
 * dock.c
 *
 * Author: Tristan Davolt (Edited by Nick Brubaker)
 *
 * Date: 4/3/2019
 *
 * Description: Implementations of functions the robot uses to dock with the
 *              charging station once getting close. These functions are
 *              incomplete and messy, we recommend they be rewritten
 *              from scratch.
 ******************************************************************************/


#include "dock.h"


/* Helper function for dock to find the front of the station. */
int findFront(double frequency)
{

    float freqFrontLow = 100;
    float freqFrontHigh = 200;
    float freqBackLow = 0;
    float freqBackHigh = 0;
    float freqLeftLow = 0;
    float freqLeftHigh = 0;
    float freqRightLow = 0;
    float freqRightHigh = 0;

    if(frequency <= freqFrontHigh && frequency >= freqFrontLow)
        return 0;
    else if (frequency <= freqRightHigh && frequency >= freqRightLow) {
        //do navigate to front
        return 0;
    }
    else if (frequency <= freqLeftHigh && frequency >= freqLeftLow) {
        //do navigate to front
        return 0;
    }
    else if (frequency <= freqBackHigh && frequency >= freqBackLow) {
        //do navigate to front
    }
    else {
        return -1;
    }
    return 0;
}

/* Finds the nearest IR signal, centers with the dock, drives to front of dock, and docks. */
void dock(void)
{

  uint16_t LeftDuty = 2500;                // duty cycle of left wheel (0 to 14,998) Default to 5009
  uint16_t RightDuty = 2500;               // duty cycle of right wheel (0 to 14,998) Default to 5000

  uint16_t LeftTach[TACHBUFF];             // tachometer period of left wheel (number of 0.0833 usec cycles to rotate 1/360 of a wheel rotation)
  enum TachDirection LeftDir;              // direction of left rotation (FORWARD, STOPPED, REVERSE)
  int32_t LeftSteps;                       // number of tachometer steps of left wheel (units of 220/360 = 0.61 mm traveled)
  uint16_t RightTach[TACHBUFF];            // tachometer period of right wheel (number of 0.0833 usec cycles to rotate 1/360 of a wheel rotation)
  enum TachDirection RightDir;             // direction of right rotation (FORWARD, STOPPED, REVERSE)
  int32_t RightSteps;                      // number of tachometer steps of right wheel (units of 220/360 = 0.61 mm traveled)
  float turnDegrees = 0;                   // degree of robot turn (units of steps*WHEELTOANGLE
  int32_t absRightSteps = 0;
  double freqCheck = 0;
  float firstTurn = 0;
  float secondTurn = 0;

  Clock_Init48MHz();                     // set system clock to 48 MHz
  Tachometer_Init();
  Motor_Init();
  EnableInterrupts();
  configFreqMeas(GPIO_PORT_P1, GPIO_PIN5);
  resetFreq(GPIO_PORT_P1);

  highCount1 = 0;
  int z = 0;
  int doneFlag = 0;
  freqCheck = getFreq(GPIO_PORT_P1);
  while(doneFlag == 0) {
      Motor_Left(LeftDuty/1.5,RightDuty/1.5);
      if (highCount1 == 1) {
          Tachometer_Clear();
          while (turnDegrees <= 45) {
              Tachometer_Get(&LeftTach[z], &LeftDir, &LeftSteps, &RightTach[z], &RightDir, &RightSteps);
              absRightSteps = abs(RightSteps);
              turnDegrees = absRightSteps*WHEELTOANGLE;
          }
          doneFlag = 1;
          Motor_Stop();
          Tachometer_Clear();
          turnDegrees = 0;
          Clock_Delay1ms(DELAY);
      }
  }

  while(1){

      if(highCount1 == 1) {
          Motor_Right(LeftDuty/1.5,RightDuty/1.5);
          while (highCount1 == 1) {
              highCount1 = 0;
              Tachometer_Get(&LeftTach[z], &LeftDir, &LeftSteps, &RightTach[z], &RightDir, &RightSteps);
              freqCheck = getFreq(GPIO_PORT_P1);
              absRightSteps = abs(RightSteps);
              turnDegrees = absRightSteps*WHEELTOANGLE;
              Clock_Delay1ms(DELAY);
          }
          Motor_Stop();
          firstTurn = turnDegrees;
          Tachometer_Clear();
          Clock_Delay1ms(DELAY);
          turnDegrees = 0;

          Motor_Left(LeftDuty/1.5,RightDuty/1.5);
          while (turnDegrees <= firstTurn) {
              Tachometer_Get(&LeftTach[z], &LeftDir, &LeftSteps, &RightTach[z], &RightDir, &RightSteps);
              absRightSteps = abs(RightSteps);
              turnDegrees = absRightSteps*WHEELTOANGLE;
          }
          Motor_Stop();
          Clock_Delay1ms(DELAY);
          Motor_Left(LeftDuty/1.5,RightDuty/1.5);
          while (highCount1 == 1) {
              highCount1 = 0;
              Tachometer_Get(&LeftTach[z], &LeftDir, &LeftSteps, &RightTach[z], &RightDir, &RightSteps);
              freqCheck = getFreq(GPIO_PORT_P1);
              absRightSteps = abs(RightSteps);
              turnDegrees = absRightSteps*WHEELTOANGLE;
              Clock_Delay1ms(DELAY);
          }
          Motor_Stop();
          secondTurn = turnDegrees;
          Tachometer_Clear();
          Clock_Delay1ms(DELAY);
          turnDegrees = 0;

          Motor_Right(LeftDuty/1.5,RightDuty/1.5);
          while (turnDegrees <= ((firstTurn + (secondTurn-firstTurn))/2)) {
              Tachometer_Get(&LeftTach[z], &LeftDir, &LeftSteps, &RightTach[z], &RightDir, &RightSteps);
              absRightSteps = abs(RightSteps);
              turnDegrees = absRightSteps*WHEELTOANGLE;
          }
          Motor_Stop();
          Tachometer_Clear();
          Clock_Delay1ms(DELAY);

          Motor_Backward(LeftDuty,RightDuty);
          while (absRightSteps <= MULTIPLIER*ROTATIONSPERMETER) {
              Tachometer_Get(&LeftTach[z], &LeftDir, &LeftSteps, &RightTach[z], &RightDir, &RightSteps);
              absRightSteps = abs(RightSteps);
          }
          Motor_Stop();
      }
  }
}

/* --COPYRIGHT--,BSD
 * Copyright (c) 2017, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * --/COPYRIGHT--*/
/*******************************************************************************
 * dock.h
 *
 * Author: Tristan Davolt (Edited by Nick Brubaker)
 *
 * Date: 4/3/2019
 *
 * Description: Declarations of functions the robot uses to dock with the
 *              charging station once getting close. These functions are
 *              incomplete and messy, we recommend they be rewritten
 *              from scratch.
 ******************************************************************************/


#ifndef DOCK_H_
#define DOCK_H_

#define STEPTODEG 1.27324
#define WHEELTOANGLE .525
#define DESIREDMAX 120                   // maximum rotations per minute
#define DESIREDMIN  30                   // minimum rotations per minute (works poorly at 30 RPM due to 16-bit timer overflow)
#define TACHBUFF 10                      // number of elements in tachometer array
#define ROTATIONSPERMETER 1590           // number of right wheel rotations to make 1 meter (in theory)
#define DELAY 1                     // delay time (units?)
#define RADTODEG 57.29578
#define MULTIPLIER 0.1
#define NOSIGNAL 0


#include <stdint.h>
#include <stdio.h>
#include <math.h>

#include "msp.h"
#include "../inc/Clock.h"
#include "../inc/CortexM.h"
#include "../inc/LaunchPad.h"
#include "../inc/Motor.h"
#include "../inc/Nokia5110.h"
#include "../inc/Tachometer.h"
#include "../inc/TimerA1.h"
#include "../inc/TA3InputCapture.h"
#include "../inc/TExaS.h"
#include "../inc/FlashProgram.h"
#include "../inc/Bump.h"
#include "getFreq.h"


/* Helper function for dock to find the front of the station.
 * Incomplete...
 * Returns 0 if front of station found and -1 if unable to find front of station. */
int findFront(double frequency);

/* Finds the nearest IR signal, centers with the dock, drives to front of dock, and docks.
 * Currently loops forever and gets stuck if IR signal is lost.
 * This function was hastily written before a deadline so it is hard to follow. We recommend
 * rewritting it from scratch. */
void dock(void);


#endif /* DOCK_H_ */

/* --COPYRIGHT--,BSD
 * Copyright (c) 2017, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * --/COPYRIGHT--*/
/*******************************************************************************
 * getFreq.c
 *
 * Author: Nick Brubaker
 *
 * Date: 3/2/2019
 *
 * Description: Function implementations for measuring the frequency
 *              of a port 1 or port 2 input pin signal.
 *
 ******************************************************************************/

#include <getFreq.h>

/* Configures a pin as input and enables GPIO interrupts. */
void configFreqMeas(uint8_t port, uint16_t pin)
{
    /* Halting the Watchdog */
    MAP_WDT_A_holdTimer();

    /* Enable SysTick and set period */
    MAP_SysTick_enableModule();
    MAP_SysTick_setPeriod(16777216);

    uint8_t portArrayIndex; // Index in arrays for the port being configured
    uint8_t portInterrupt; // Needed to configure interrupts for a port

    /* Map input port to array index and interrupt enable */
    if(port == GPIO_PORT_P1)
    {
        portArrayIndex = PORT1_ARRAY_INDEX;
        portInterrupt = INT_PORT1;

    }

    else if (port == GPIO_PORT_P2)
    {
        portArrayIndex = PORT2_ARRAY_INDEX;
        portInterrupt = INT_PORT2;
    }

    else
    {
        perror("Invalid port chosen");
        while(1);
    }

    /* Store pins in global arrays */
    pins[portArrayIndex] = pin;

    /* Configure pins as input */
    MAP_GPIO_setAsInputPin(port, pins[portArrayIndex]);

    /* Enabling interrupts */
    MAP_GPIO_clearInterruptFlag(port, pins[portArrayIndex]);
    MAP_GPIO_enableInterrupt(port, pins[portArrayIndex]);
    MAP_Interrupt_enableInterrupt(portInterrupt);

    /* Enabling MASTER interrupts */
    MAP_Interrupt_enableMaster();
}

/* Gets the most recently measured frequency on the configured pin of the specified port. */
double getFreq(uint8_t port)
{
    if(port == GPIO_PORT_P1)
    {
        return (double)(1/((double)(period[PORT1_ARRAY_INDEX])/(double)(MCLK_TO_SEC)));
    }

    else if (port == GPIO_PORT_P2)
    {
        return (double)(1/((double)(period[PORT2_ARRAY_INDEX])/(double)(MCLK_TO_SEC)));
    }

    else
    {
        perror("Invalid port chosen");
        while(1);
    }
}

/* Reset values to 1. */
void resetFreq(uint8_t port)
{
    if (port == GPIO_PORT_P1)
    {
        period[PORT1_ARRAY_INDEX] = 1;
    }
    else if (port == GPIO_PORT_P2)
    {
        period[PORT2_ARRAY_INDEX] = 1;
    }
}

/* GPIO ISRs */
void PORT1_IRQHandler(void)
{
    uint32_t status;

    status = MAP_GPIO_getEnabledInterruptStatus(GPIO_PORT_P1);
    MAP_GPIO_clearInterruptFlag(GPIO_PORT_P1, status);

    if(status & pins[PORT1_ARRAY_INDEX])
    {
        period[PORT1_ARRAY_INDEX] =
                SYSTIC_START - SysTick->VAL; // Calculate and store period


        SysTick->VAL = SYSTIC_START;   // Reset SysTic value
        highCount1 = 1;
    }
}

void PORT2_IRQHandler(void)
{
    uint32_t status;

    status = MAP_GPIO_getEnabledInterruptStatus(GPIO_PORT_P2);
    MAP_GPIO_clearInterruptFlag(GPIO_PORT_P2, status);

    if(status & pins[PORT2_ARRAY_INDEX])
    {
        period[PORT2_ARRAY_INDEX] =
                SYSTIC_START - SysTick->VAL; // Calculate and store period


        SysTick->VAL = SYSTIC_START;   // Reset SysTic value
    }
}

/* --COPYRIGHT--,BSD
 * Copyright (c) 2017, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * --/COPYRIGHT--*/
/*******************************************************************************
 * getFreq.h
 *
 * Author: Nick Brubaker
 *
 * Date: 3/2/2019
 *
 * Description: Functions for measuring the frequency of a port 1 or port 2
 *              input pin signal. Frequency measurement can only be configured
 *              and used on two pins at a time with one pin on each port.
 *
 ******************************************************************************/

#ifndef _GET_FREQ_H
#define _GET_FREQ_H

/* Indexes in arrays for different pins */
#define PORT1_ARRAY_INDEX 0
#define PORT2_ARRAY_INDEX 1

#define SYSTIC_START 0xFFFFFF //SysTic starting value
#define MCLK_TO_SEC 3000000 // Convert SysTics (3 MHz) to seconds

/* DriverLib Includes */
#include "ti/devices/msp432p4xx/inc/msp.h"
#include <ti/devices/msp432p4xx/driverlib/driverlib.h>
#include <stdio.h>

/* Each index is for a different pin */
uint16_t pins[2]; // Stores pins
volatile uint32_t period[2]; // Stores most recently calculated periods (SysTics)
double highCount1;
uint32_t dockCount;

/* Configures a pin as input and enables GPIO interrupts.
 *  ~ param port is used to select the port with the pin.
 *    Valid macros for ports have the format GPIO_PORT_PX.
 *  ~ param pin selects the pin.
 *    Valid macros for pins have the format GPIO_PINX.
 * Only use ports 1 and 2 (the only ones with interrupts). */
void configFreqMeas(uint8_t port, uint16_t pin);


/* Gets the most recently measured frequency on the
 * configured pin of the specified port.
 *  ~ param port is used to select the port with the pin.
 *    Valid macros for ports have the format GPIO_PORT_PX.
 *  ~ param pin selects the pin.
 *    Valid macros for pins have the format GPIO_PINX.
 * Returns a double representing frequency in Hz. */
double getFreq(uint8_t port);

/* Resets frequency values to 1 (no signal) */
void resetFreq(uint8_t port);

/* GPIO ISRs */
void PORT1_IRQHandler(void);
void PORT2_IRQHandler(void);


#endif

/* --COPYRIGHT--,BSD
 * Copyright (c) 2017, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * --/COPYRIGHT--*/
/******************************************************************************
 * measRobotBat.c
 *
 * Author: Nick Brubaker
 *
 * Date: 3/13/2019
 *
 * Description: Implementations of functions for
 *              measuring the robot's battery voltage.
 * *******************************************************************************/

#include "measRobotBat.h"

/* Configure ADC  */
void config_ADC()
{
    /* Setting Flash wait state */
    MAP_FlashCtl_setWaitState(FLASH_BANK0, 1);
    MAP_FlashCtl_setWaitState(FLASH_BANK1, 1);

    /* Setting DCO to 48MHz  */
    MAP_PCM_setPowerState(PCM_AM_LDO_VCORE1);
    MAP_CS_setDCOCenteredFrequency(CS_DCO_FREQUENCY_48);

    /* Enabling the FPU for floating point operation */
    MAP_FPU_enableModule();
    MAP_FPU_enableLazyStacking();

    //![Single Sample Mode Configure]
    /* Initializing ADC (MCLK/1/4) */
    MAP_ADC14_enableModule();
    MAP_ADC14_initModule(ADC_CLOCKSOURCE_MCLK, ADC_PREDIVIDER_1, ADC_DIVIDER_4,
            0);

    /* Configuring GPIOs (5.5 A0) */
    MAP_GPIO_setAsPeripheralModuleFunctionInputPin(GPIO_PORT_P5, GPIO_PIN5,
    GPIO_TERTIARY_MODULE_FUNCTION);

    /* Configuring ADC Memory */
    MAP_ADC14_configureSingleSampleMode(ADC_MEM0, true);
    MAP_ADC14_configureConversionMemory(ADC_MEM0, ADC_VREFPOS_AVCC_VREFNEG_VSS,
    ADC_INPUT_A0, false);

    /* Configuring Sample Timer */
    MAP_ADC14_enableSampleTimer(ADC_MANUAL_ITERATION);

    /* Enabling/Toggling Conversion */
    MAP_ADC14_enableConversion();
    MAP_ADC14_toggleConversionTrigger();
    //![Single Sample Mode Configure]

    /* Enabling interrupts */
    MAP_ADC14_enableInterrupt(ADC_INT0);
    MAP_Interrupt_enableInterrupt(INT_ADC14);

    return;
}

/* Calculates and returns the measured battery voltage as a double. */
double getBatVoltage()
{
    return CONVERT_ROBOT_VOLTS*((double)ADCResult);
}

//******************************************************************************
//
//  ADC interrupt vector service routine
//
//******************************************************************************
void ADC14_IRQHandler(void)
{

    uint64_t status;

    status = MAP_ADC14_getEnabledInterruptStatus();
    MAP_ADC14_clearInterruptFlag(status);

    if(status & ADC_INT0)   // Check if conversion is from valid source
    {
        ADCResult = MAP_ADC14_getResult(ADC_MEM0); // Move sampled value to global variable
    }

    MAP_ADC14_toggleConversionTrigger();
}

Credits

The Killer Watts

1 project • 0 followers

Contact

Jason Rubadue

4 projects • 9 followers

Texas Instruments University Marketing Manager for the US West

Contact

Comments

Please log in or sign up to comment.

Embed the widget on your own site

Solar Charging Station

Solar Charging Station

Things used in this project

Hardware components

Software apps and online services

Story

Navigation

IR Emitters

Navigation Algorithms

Returning to Station

Avoiding Obstacles

Aligning to Docking Port

Navigation Improvements and Opportunity for Further Development

Power

Maximum Power Point Tracking (BQ24650)

Lithium Ion Battery Charger (BQ24130)

Station MSP

Solar Panel

Station Design

Docking: Power Transfer Connector

Power Improvements and Opportunity for Further Development

Schematics

Robot Power Circuitry

Station Power Schematic

Station PCB

Robot PCB

Code

README.txt

Measuring Robot Battery Header

Advanced Motor

Header file for Advanced Motor

Robot Code

Station Code

UART Configuration

UART Configuration Header

Docking Code

Docking Header File

Measuring Frequency

Header File for Frequency Code

Measuring Robot Battery

Credits

The Killer Watts

Jason Rubadue

Comments

Related channels and tags