Project Overview
Python Image Processing
LabVIEW Implementation
Waypoint Algorithm
Hardware Modifications
Design Challenges
Sensor Implementations
Final Demonstrations and Images
Conclusion
Team Members

Team ME461 Line Drawing Robot Fall 2025:

•

•

•

Published December 18, 2025

ME461 Fall 2025 Line Drawing Robot

This robot recreates simple hand drawings processed in Python by following waypoints and actuating a pen of its own.

IntermediateShowcase (no instructions)44

Things used in this project

Hardware components

Texas Instruments LAUNCHXL-F28379D C2000 Delfino LaunchPad

Software apps and online services

Texas Instruments Code Composer Studio

LabVIEW Community Edition

Jupyter Notebook

Story

Project Overview

This project constituted the final product for ME461: Computer Control of Mechanical Systems at the University of Illinois Urbana-Champaign. The main subject of the project was a multi-purpose robot, outfitted with onboard sensors including wheel encoders, IMU distance sensors and more alongside Texas Instruments' F28379D launchpad board, the larger project objective was to use the robot to complete a given task or use sensors and controllers to engineer new types of robot functionality.

For our project we chose to develop a mobile drawing robot capable of converting simple user-generated images into physical drawings on paper. An image-processing pipeline was implemented using OpenCV to extract edge-based contours from input drawings and discretize them into ordered sets of two-dimensional points. These points were communicated to the robot via a LabVIEW interface, which streamed waypoint data and control flags in real time. Onboard, the robot tracked its position and heading using wheel encoder feedback as well as the onboard gyroscope, and a point-following control algorithm generated forward velocity and turn-rate commands to drive the robot to each waypoint. A servo-actuated pen mechanism was designed and integrated to lift and lower a marker between drawing strokes, allowing individual contours or lines to be recreated cleanly.

In total, this project builds a novel link between the programmable dynamics of the ME461 robot and practical user input. It demonstrates how even with only a small subset of sensors, simple inputs that do not resemble written code themselves can be transcribed into useful instructions for a robot, which in turn can carry out powerful real-world tasks.

Drawing Robot Tracking Waypoints and Drawing Traingles on the Ground

Python Image Processing

To translate the digitally drawn images to sets of points for the ME 461 robot, we use OpenCV, Scikit-image, and Numpy (and Matplotlib for visualization). Using the img_to_stroke_points() function, the user inputs the file path for their image, the desired number of paths per component (path), and the minimum path length. The script then uses OpenCV to read the image and additionally apply an Otsu grayscale filter. This pre-processes the image to be skeletonized using the Scikit-image skeletonize() function. The skeletonize() function creates an extrapolated graph of 1-pixel-wide edges from the grayscale image, where each edge is a set of points (or path) for our robot to follow. After points are created, a 3D numpy tensor (using the np.array() data structure) is created. The shape of the tensor is the following: [(number of edges/paths) x (number of points per path) x 2 (x, y)]. The script additionally prints the tensor in a copy-pastable format. Below is an example of converting an MS Paint star to a set of points.

1 / 4 • Original MS Paint Star

LabVIEW Implementation

The LabVIEW VI receives the tensor using a Python node. Then, it starts communication with the robot using TCPIP communication. It also starts iteration through all the points in all the contours (paths/edges) in the Python array with nested for loops. Each time the robot sends a signal saying it is ready for the next point, the send button is enabled so the user can click to send the next point along with the pen "state" and the for loops increment to the next point. The pen state (either up [0] or down [1]) is determined by LabVIEW, by checking if the next point is the start of a new path/edge. If it is, the pen state is set to lift the pen. Else, the pen state is set to lower the pen.

On the front panel, the user can see which point in the loop is about to be sent to the robot (X Coord, Y Coord), the pen state (Pen Value/Pen State), and the current position of the robot (X, Y), in both coordinates and visually as a box in a drawing. The number of contours and number of points per contour are shown for confirmation and debugging purposes.

LabVIEW VI Front Panel

Waypoint Algorithm

The final leg of the software pipeline that enables the reconstruction of a full-scale drawn image is the dynamics and waypoint algorithm onboard the ME461 robot itself. The software controlling the robot integrates real-time sensing, state estimation, communication and motion control in order to understand its current position, move towards its next position, and actuate the drawing utensil in time with these motions.

The overall waypoint algorithm is a modified version of an existing script from the UIUC systems engineering curriculum. To interface easily with the output of the Python-OpenCV image processing, waypoint following is implemented as a sequential point-to-point control approach. Target coordinates are streamed from LabVIEW and stored as the active goal for the controller. At each control update, the robot computes the relative position between its current location and the target waypoint, which provides both the distance to the target and the desired heading direction. The controller generates a forward speed command that decreases with decreasing distance (and increases with increasing distance) to the waypoint and is saturation-limited to ensure safe operation and reduce overshoot. A separate turn-rate command adjusts the robot’s heading toward the target direction. Forward motion is reduced or paused when the heading error is large to prevent sideways drift. A waypoint is considered reached once the robot enters a specified position tolerance, after which the controller advances to the next target. This method allows the robot to reliably follow both individual waypoints and closed paths with smooth and predictable motion. The following image demonstrates an excerpt of the control logic inside of the main dynamics function; it includes the definition of the aforementioned tolerances and the modified dynamics that occur when the robot is close to its final destination.

Sample Code from XY control algorithm; tolerances and logic modified from reference code

The robot software implements a simple state machine to manage its behavior in moving from one waypoint to another. LabVIEW serves as an external interface that streams waypoint coordinates and monitors robot status, but the sequencing and decision-making logic reside on the embedded controller. The robot processes one waypoint at a time, storing the received target as the active goal and executing closed-loop control until arrival at the latest waypoint is detected. State transitions on the robot govern when a new waypoint is accepted, when motion is enabled, and when the controller advances to the next target. This one-point-at-a-time structure simplifies coordination between LabVIEW and the robot, prevents missed or out-of-order commands, and ensures reliable execution of both simple and complex paths. The following images demonstrate simple state machine control flow used to initiate movement between waypoints as well as pen movements.

Code image of calling the xy_control function and passing referneces and tolerances

Example of implemented state machine and pen acutation for new connected components

Hardware Modifications

We designed a simple 3D-printed bracket that mounts to the front of the robot and holds the servo in place. The bracket positions the servo such that the marker contacts the paper at the center point between the front wheels.

An HS-311 servo was used to actuate the marker. The servo rotates to lift and lower the pen, providing clean transitions between drawing and non-drawing motion. This servo was chosen because it is readily available in the lab, and provided sufficient torque for this application.

A torsion spring was mounted directly on the servo output, and the marker is zip-tied to the spring. This creates a compliant pen mount that allows the marker to stay in contact with the paper while absorbing small height variations caused by the textured lab floor. The torsion spring provides light, consistent downward force without adding excessive friction, which helps prevent the robot from lifting or the wheels from slipping. The zip-tie attachment makes it easy to swap between different pens or markers and fine-tune performance.

Spring-Side View of Servo Mount and Compliant Torsoin Spring for Pen

Servo-Side View of Servo Mount and Compliant Torsoin Spring for Pen

Design Challenges

In developing this project, we encountered numerous design challenges that drove us to refine each dimension of our software pipeline. The first and most important challenge involved tightening the waypoint tolerances, that is, the degree of accuracy with which the robot would reach its target destination before it decided to indicate to LabVIEW that it is ready for the next command. To do this, we modified the radial distance from the target point at which the control inputs to forward and turning motion were decreased to be larger and shrunk the radius that the robot needed to achieve in order to consider itself as having arrived at a waypoint. Ultimately, both of these changes were central to achieving precise control of the robot over many points; the net effect of these changes were much slower more deliberate motions. While this decreased the speed of the robot in traversing the waypoints, it greatly increased its accuracy, allowed closed curves to be drawn, and minimized drift when drawing multiple connected components.

Another design challenge that we faced involved picture resolution, that is, the number of waypoints that we generated for a given inputted line stroke. If the stroke width of the drawn image was too thick, or if we permitted too dense of a point grid to be generated by our python scirpt, then the robot might have to traverse to a number of adjacement points. The resulting path for the robot would be a series of sharp turns or an excessive number of points that would take too long for the robot to trace. To resolve this problem we limited the number of points per contour to a reasonable value and standardized this across connected components. While this in theory limited the practical resolution of our images, it ensure a much more streamlined software workflow and ultimately produced more reliable drawn images from the robot.

In total our design process was highly iterative and featured routine changes to all elements of our software workflow. Given that our overall process moved between three distinct steps - python, LabVIEW, and C-code on the ME461 robot, it was difficult to understand if the sources of error we encountered resulted from an improper handling of inputs or a mistake in our controller. To that end, our debugging process revolved around isolating issues and honing in exploring the changes in outputs for unit changes in our controller or python procsesing code. Given more time, we would hvae continued to make updates to every element of our project with the goal of improving drawn image resolution and providing further autonomy to the overall software workflow.

Sensor Implementations

The following video provides a brief overview of the sensors and actuators used in this project. In general, only the wheel encoders, and IMU, specifically the gyro-z value was used for sensing. This equipment alone allows for a stable estimation of linear position coordinates and velocity as well as bearing by integrating the yaw rate of the vehicle. It is important to highlight how a change was made from using the wheel encoders to control and analyze linear movements and changes to bearing; wheel slippage made this method impractical.

Overview of Sensors Used and Implementation

Final Demonstrations and Images

The following images and videos compile some of the images that we attempted with our robot during testing and demonstration.

Video of Robot Tracing Two Triangles Using Data Streamed from LabVIEW

Video of Robot Tracing a Smiley Face from Image Made in MS Paint

Video of Robot Tracing "HI" and a Smiley Face

1 / 5 • Output Image

Conclusion

This project successfully demonstrated an end-to-end autonomous drawing system that integrates sensing, communication, control, and mechanical actuation. By combining wheel-encoder-based position estimation with IMU-derived heading information, the robot was able to accurately navigate between between waypoints streamed from a LabVIEW interface. The implemented point-following controller reliably detected arrival at waypoints and closure of drawn paths. Additionally, the servo-actuated pen mechanism and compliant torsion spring provided stable and precise control over drawing and non-drawing motions, enabling a clean creation of individual strokes or shape contours. In total, this project revealed the practical challenges and solutions that use real-time robotic navigation and control. However, it also demonstrated the functional power that even simple robotic control systems can accomplish. This project cultivated our skills in mechanical system programming and design, and is a springboard for future work in control design and autonomous systems.

Team Members

1 / 4 • Andrew Baumgart, Mechanical Engineering Spring '26

Custom parts and enclosures

Sketchfab still processing.

Code

//#############################################################################
// FILE:   LABstarter_main.c
//
// TITLE:  Lab Starter
//#############################################################################

// Included Files
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <string.h>
#include <math.h>
#include <limits.h>
#include "F28x_Project.h"
#include "driverlib.h"
#include "device.h"
#include "F28379dSerial.h"
#include "LEDPatterns.h"
#include "song.h"
#include "dsp.h"
#include "fpu32/fpu_rfft.h"
#include "xy.h"

#define PI          3.1415926535897932384626433832795
#define TWOPI       6.283185307179586476925286766559
#define HALFPI      1.5707963267948966192313216916398
// The Launchpad's CPU Frequency set to 200 you should not change this value
#define LAUNCHPAD_CPU_FREQUENCY 200

// ----- code for CAN start here -----
#include "F28379dCAN.h"
//#define TX_MSG_DATA_LENGTH    4
//#define TX_MSG_OBJ_ID         0  //transmit

#define RX_MSG_DATA_LENGTH    8
#define RX_MSG_OBJ_ID_1       1  //measurement from sensor 1
#define RX_MSG_OBJ_ID_2       2  //measurement from sensor 2
#define RX_MSG_OBJ_ID_3       3  //quality from sensor 1
#define RX_MSG_OBJ_ID_4       4  //quality from sensor 2
// ----- code for CAN end here -----


// Interrupt Service Routines predefinition
__interrupt void cpu_timer0_isr(void);
__interrupt void cpu_timer1_isr(void);
__interrupt void cpu_timer2_isr(void);
__interrupt void SWI_isr(void);
__interrupt void SPIB_isr(void);
__interrupt void ADCA_isr(void);
// ----- code for CAN start here -----
__interrupt void can_isr(void);
// ----- code for CAN end here -----

void setDACA(float dacouta0);
void setDACB(float dacouta1);

void setupSpib(void);
void init_eQEPs(void);
float readEncLeft(void);
float readEncRight(void);

void setEPWM2A(float controleffort);
void setEPWM2B(float controleffort);

// Count variables
uint32_t numTimer0calls = 0;
uint32_t numSWIcalls = 0;
extern uint32_t numRXA;
uint16_t UARTPrint = 0;
uint16_t LEDdisplaynum = 0;

float adc1 = 0;
float adc2 = 0;

int16_t accelx_raw = 0;
int16_t accely_raw = 0;
int16_t accelz_raw = 0;
int16_t gyrox_raw = 0;
int16_t gyroy_raw = 0;
int16_t gyroz_raw = 0;

float accelx = 0;
float accely = 0;
float accelz = 0;
float gyrox = 0;
float gyroy = 0;
float gyroz = 0;
int16_t adca0result = 0;
int16_t adca1result = 0;
float joy0 = 0;
float joy1 = 0;
int32_t adcaCount = 0;
// Needed global Variables
float accelx_offset = 0;
float accely_offset = 0;
float accelz_offset = 0;
float gyrox_offset  = 0;
float gyroy_offset  = 0;
float gyroz_offset  = 0;
float accelzBalancePoint = -.76;
int16 IMU_data[9];
uint16_t temp=0;
int16_t doneCal = 0;
float tilt_value    = 0;
float tilt_array[4] = {0, 0, 0, 0};
float gyro_value    = 0;
float gyro_array[4] = {0, 0, 0, 0};
float LeftWheel = 0;
float RightWheel = 0;
float LeftWheelArray[4] = {0,0,0,0};
float RightWheelArray[4] = {0,0,0,0};
// Kalman Filter vars
float T = 0.001;        //sample rate, 1ms
float Q = 0.01; // made global to enable changing in runtime
float R = 25000;//50000;
float kalman_tilt = 0;
float kalman_P = 22.365;
int16_t SpibNumCalls = -1;
float pred_P = 0;
float kalman_K = 0;
int32_t timecount = 0;
int16_t calibration_state = 0;
int32_t calibration_count = 0;
float LeftVelFilt = 0;
float LeftVelFilt_1 = 0;
float LeftWheel_1 = 0;
float RightVelFilt = 0;
float RightVelFilt_1 = 0;
float RightWheel_1 = 0;
float gyrorate_dot = 0;
float gyrorate_dot_1 = 0;
float gyro_value_1 = 0;
float K1 = -60;
float K2 = -4.5;
float K3 = -1.1;
float K4 = -0.1;
float uLeft = 0;
float uRight = 0;
float ubal = 0;

//Ex 4
float WhlDiff = 0;
float WhlDiff_1 = 0;
float errorDiff = 0;
float errorDiff_1 = 0;
float turnrate = 0;
float turnrate_1 = 0;
float intDiff = 0;
float intDiff_1 = 0;
float vel_WhlDiff = 0;
float vel_WhlDiff_1 = 0;
float KpDiff = 3.0;
float KiDiff = 0;//20.0;
float KdDiff = 0.08;
float turnref = 0;
float turnref_1 = 0;
float turn = 0;

//Ex5
float ForwardBackwardCommand = 0;
float eSpeed = 0;
float eSpeed_1 = 0;
float Segbot_refSpeed = 0;
float IK_eSpeed = 0;
float IK_eSpeed_1 = 0;
float KpSpeed = 0.6;//0.35;
float KiSpeed = 0;//1.5;

// ----- code for CAN start here -----
// volatile uint32_t txMsgCount = 0;
// extern uint16_t txMsgData[4];

volatile uint32_t rxMsgCount_1 = 0;
volatile uint32_t rxMsgCount_3 = 0;
extern uint16_t rxMsgData[8];

uint32_t dis_raw_1[2];
uint32_t dis_raw_3[2];
uint32_t dis_1 = 0;
uint32_t dis_3 = 0;

uint32_t quality_raw_1[4];
uint32_t quality_raw_3[4];
float quality_1 = 0.0;
float quality_3 = 0.0;

uint32_t lightlevel_raw_1[4];
uint32_t lightlevel_raw_3[4];
float lightlevel_1 = 0.0;
float lightlevel_3 = 0.0;

uint32_t measure_status_1 = 0;
uint32_t measure_status_3 = 0;

float RightDistance = 0;
float FrontDistance = 0;
float KpWallFollow = .0015;
float RightDesired = 200.0;
uint16_t rightwallfollow = 0;
float KpFrontwall = 0.0005;
float e_turn = 0;

#define AVERAGESIZE 20
float RightDistanceValues[AVERAGESIZE];
float RightDistanceAverage = 0;


//All of these are control variables for the various dynamical actions contained in this script including waypoint following and wall following
float eLeft_K= 0 ;
float refSpeed = 0;
float VLeftK = 0;;
float ILeft_K = 0;
float ILeft_K1 = 0;
float eLeft_K1 = 0;
float eRight_K= 0 ;
float VRightK = 0;;
float IRight_K = 0;
float IRight_K1 = 0;
float eRight_K1 = 0;
float K_p = 3.0;
float K_i = 25.0;

float Rwh = (0.119/2); //meters

float Wr = 0.173; // meters

float PhiR = 0;
float dotThetaAvg = 0;
float dotxRk = 0;
float dotyRk = 0;
float xRk = 0;
float yRk = 0 ;
float dotxRk_1 = 0;
float dotyRk_1 = 0;
float xRk_1 = 0;
float yRk_1 = 0 ;

float printLV1 = 0;
float printLV2 = 0;

float printLV3 = 0;
float printLV4 = 0;
float printLV5 = 0;
float printLV6 = 0;
float printLV7 = 0;
float printLV8 = 0;

extern uint16_t NewLVData;
extern float fromLVvalues[LVNUM_TOFROM_FLOATS];
extern LVSendFloats_t DataToLabView;
extern char LVsenddata[LVNUM_TOFROM_FLOATS*4+2];
extern uint16_t newLinuxCommands;
extern float LinuxCommands[CMDNUM_FROM_FLOATS];






volatile uint32_t errorFlag = 0;
// ----- code for CAN end here -----

//We could theoretically hardcode our waypoints, or pass them into an array of arbitrarily length; we would do that with a variable like this
#define NUMWAYPOINTS 8
uint16_t statePos = 0;
pose robotdest[NUMWAYPOINTS];  // array of waypoints for the robot


//ADDITIONAL GLOBALS

//  LabVIEW waypoint streaming
typedef enum { LV_WAIT_FOR_CMD=0, LV_RUN_TO_POINT=1 } lv_state_t;
volatile lv_state_t lv_state = LV_WAIT_FOR_CMD;

volatile uint16_t lv_cmd_pending = 0;   // for when a new  LV command arrives

float lv_goal_x = 0.0f;   // meters
float lv_goal_y = 0.0f;   // meters
float lv_pen_cmd = 0.0f;  // 0=UP, 1=DOWN (float from LV)

// Flag sent back to LabVIEW
volatile float lv_ready_flag = 1.0f;    // 1 = ready for new point, 0 = busy

// pen angles; these must be empirically measured or calculated depending on the servo, the mounting angle and the precise details of the torsion spring
#define PEN_UP_ANGLE    0.0f
#define PEN_DOWN_ANGLE  20.0f



float compass_angle = 0.0;

//float max_mx = 53;
//float min_mx = -92;
//float max_my = 73;
//float min_my = -86;
//float max_mz = 273;
//float min_mz = 125;

float mx_offset = (56 + -56)/2.0; //(max_mx + min_mx)/2.0
float my_offset = (110 + -3)/2.0; // (max_my + min_my)/2.0
float mz_offset = 0;//(273 + 125)/2.0; // (max_mz + min_mz)/2.0

int16_t mx_raw = 0;
int16_t my_raw = 0;
int16_t mz_raw = 0;

float mx = 0;
float my = 0;
float mz = 0;

float yawrate = 0.0;
float yaw = 0;
float yaw_1 = 0;
float yawrate_1 = 0;


//adca1 pie interrupt
__interrupt void ADCA_ISR(void) {
    adca0result = AdcaResultRegs.ADCRESULT0;
    adca1result = AdcaResultRegs.ADCRESULT1;

    // Here covert ADCIND0 to volts
    joy0 = adca0result*(3.0/4096.0);
    joy1 = adca1result*(3.0/4096.0);


    GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
    SpibRegs.SPIFFRX.bit.RXFFIL = 12;
    SpibRegs.SPITXBUF = 0xBA00;
    SpibRegs.SPITXBUF = 0x0000;
    SpibRegs.SPITXBUF = 0x0000;
    SpibRegs.SPITXBUF = 0x0000;
    SpibRegs.SPITXBUF = 0x0000;
    SpibRegs.SPITXBUF = 0x0000;
    SpibRegs.SPITXBUF = 0x0000;
    SpibRegs.SPITXBUF = 0x0000;
    SpibRegs.SPITXBUF = 0x0000;
    SpibRegs.SPITXBUF = 0x0000;
    SpibRegs.SPITXBUF = 0x0000;
    SpibRegs.SPITXBUF = 0x0000;

    adcaCount++;
    AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //clear interrupt flag
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
}

//A&J Comment  Converts a desired angle into a safe PWM duty cycle. These 
//functions limit the input range and update EPWM8 compare values to move the servo.
void setEPW8A_RCServo(float angle) {
    if(angle > 90.0) {
        angle = 90.0;
    } else if (angle < -90.0) {
        angle = -90.0;
    }

    EPwm8Regs.CMPA.bit.CMPA = ((angle + 90.0) * 5000.0/180.0 + 2500.0);
//A&J Comment - this code operates similar to earlier, with error handling to ensure the input value is between -90 and 90 degrees and then a linear mapping.
//Note that we we have chosen to be conservative in defining our duty cycles such that the only go between 8% and 12%. This is to ensure we don't overdrive the servo motor.

}

//This is the functoin that was used to control the Servo on the robot; this may depend on which onboard pins are used; more tha one servo could be controlled in future implementations
//A&J Comment - This code operates similary to  above, only this time we set the CMPB register.
void setEPW8B_RCServo(float angle) {
    if(angle > 90.0) {
           angle = 90.0;
       } else if (angle < -90.0) {
           angle = -90.0;
       }

    EPwm8Regs.CMPB.bit.CMPB = ((angle + 90.0) * 5000.0/180.0 + 2500.0);

}

void main(void)
{
    // PLL, WatchDog, enable Peripheral Clocks
    // This example function is found in the F2837xD_SysCtrl.c file.
    InitSysCtrl();

    InitGpio();

    // Blue LED on LaunchPad
    GPIO_SetupPinMux(31, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(31, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPASET.bit.GPIO31 = 1;

    // Red LED on LaunchPad
    GPIO_SetupPinMux(34, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(34, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPBSET.bit.GPIO34 = 1;

    // LED1 and PWM Pin
    GPIO_SetupPinMux(22, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(22, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPACLEAR.bit.GPIO22 = 1;

    // LED2
    GPIO_SetupPinMux(94, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(94, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPCCLEAR.bit.GPIO94 = 1;

    // LED3
    GPIO_SetupPinMux(95, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(95, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPCCLEAR.bit.GPIO95 = 1;

    // LED4
    GPIO_SetupPinMux(97, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(97, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPDCLEAR.bit.GPIO97 = 1;

    // LED5
    GPIO_SetupPinMux(111, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(111, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPDCLEAR.bit.GPIO111 = 1;

    // LED6
    GPIO_SetupPinMux(130, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(130, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPECLEAR.bit.GPIO130 = 1;

    // LED7
    GPIO_SetupPinMux(131, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(131, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPECLEAR.bit.GPIO131 = 1;

    // LED8
    GPIO_SetupPinMux(25, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(25, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPACLEAR.bit.GPIO25 = 1;

    // LED9
    GPIO_SetupPinMux(26, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(26, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPACLEAR.bit.GPIO26 = 1;

    // LED10
    GPIO_SetupPinMux(27, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(27, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPACLEAR.bit.GPIO27 = 1;

    // LED11
    GPIO_SetupPinMux(60, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(60, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPBCLEAR.bit.GPIO60 = 1;

    // LED12
    GPIO_SetupPinMux(61, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(61, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPBCLEAR.bit.GPIO61 = 1;

    // LED13
    GPIO_SetupPinMux(157, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(157, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPECLEAR.bit.GPIO157 = 1;

    // LED14
    GPIO_SetupPinMux(158, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(158, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPECLEAR.bit.GPIO158 = 1;

    // LED15
    GPIO_SetupPinMux(159, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(159, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPECLEAR.bit.GPIO159 = 1;

    // LED16
    GPIO_SetupPinMux(160, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(160, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPFCLEAR.bit.GPIO160 = 1;

    //WIZNET Reset
    GPIO_SetupPinMux(0, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(0, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPASET.bit.GPIO0 = 1;

    //ESP8266 Reset
    GPIO_SetupPinMux(1, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(1, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPASET.bit.GPIO1 = 1;

    //SPIRAM  CS  Chip Select
    GPIO_SetupPinMux(19, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(19, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPASET.bit.GPIO19 = 1;

    //DRV8874 #1 DIR  Direction
    GPIO_SetupPinMux(29, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(29, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPASET.bit.GPIO29 = 1;

    //DRV8874 #2 DIR  Direction
    GPIO_SetupPinMux(32, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(32, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPBSET.bit.GPIO32 = 1;

    GPIO_SetupPinMux(52, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(52, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPBCLEAR.bit.GPIO52 = 1;

    //DAN28027  CS  Chip Select
    GPIO_SetupPinMux(9, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(9, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPASET.bit.GPIO9 = 1;

    //MPU9250  CS  Chip Select
    GPIO_SetupPinMux(66, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(66, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPCSET.bit.GPIO66 = 1;

    //WIZNET  CS  Chip Select
    GPIO_SetupPinMux(125, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(125, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPDSET.bit.GPIO125 = 1;

    //PushButton 1
    GPIO_SetupPinMux(4, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(4, GPIO_INPUT, GPIO_PULLUP);

    //PushButton 2
    GPIO_SetupPinMux(5, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(5, GPIO_INPUT, GPIO_PULLUP);

    //PushButton 3
    GPIO_SetupPinMux(6, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(6, GPIO_INPUT, GPIO_PULLUP);

    //PushButton 4
    GPIO_SetupPinMux(7, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(7, GPIO_INPUT, GPIO_PULLUP);

    //Joy Stick Pushbutton
    GPIO_SetupPinMux(8, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(8, GPIO_INPUT, GPIO_PULLUP);

    // ----- code for CAN start here -----
    //GPIO17 - CANRXB
    GPIO_SetupPinMux(17, GPIO_MUX_CPU1, 2);
    GPIO_SetupPinOptions(17, GPIO_INPUT, GPIO_ASYNC);

    //GPIO12 - CANTXB
    GPIO_SetupPinMux(12, GPIO_MUX_CPU1, 2);
    GPIO_SetupPinOptions(12, GPIO_OUTPUT, GPIO_PUSHPULL);
    // ----- code for CAN end here -----

    for (int i = 0;i < AVERAGESIZE; i++) {
        RightDistanceValues[i] = 0;;
    }

    // ----- code for CAN start here -----
    // Initialize the CAN controller
    InitCANB();

    // Set up the CAN bus bit rate to 1000 kbps
    setCANBitRate(200000000, 1000000);

    // Enables Interrupt line 0, Error & Status Change interrupts in CAN_CTL register.
    CanbRegs.CAN_CTL.bit.IE0= 1;
    CanbRegs.CAN_CTL.bit.EIE= 1;
    // ----- code for CAN end here -----


    // Clear all interrupts and initialize PIE vector table:
    // Disable CPU interrupts
    DINT;

    // Initialize the PIE control registers to their default state.
    // The default state is all PIE interrupts disabled and flags
    // are cleared.
    // This function is found in the F2837xD_PieCtrl.c file.
    InitPieCtrl();

    // Disable CPU interrupts and clear all CPU interrupt flags:
    IER = 0x0000;
    IFR = 0x0000;

    // Initialize the PIE vector table with pointers to the shell Interrupt
    // Service Routines (ISR).
    // This will populate the entire table, even if the interrupt
    // is not used in this example.  This is useful for debug purposes.
    // The shell ISR routines are found in F2837xD_DefaultIsr.c.
    // This function is found in F2837xD_PieVect.c.
    InitPieVectTable();

    // Interrupts that are used in this example are re-mapped to
    // ISR functions found within this project
    EALLOW;  // This is needed to write to EALLOW protected registers
    PieVectTable.TIMER0_INT = &cpu_timer0_isr;
    PieVectTable.TIMER1_INT = &cpu_timer1_isr;
    PieVectTable.TIMER2_INT = &cpu_timer2_isr;
    PieVectTable.SCIA_RX_INT = &RXAINT_recv_ready;
    PieVectTable.SCIB_RX_INT = &RXBINT_recv_ready;
    PieVectTable.SCIC_RX_INT = &RXCINT_recv_ready;
    PieVectTable.SCID_RX_INT = &RXDINT_recv_ready;
    PieVectTable.SCIA_TX_INT = &TXAINT_data_sent;
    PieVectTable.SCIB_TX_INT = &TXBINT_data_sent;
    PieVectTable.SCIC_TX_INT = &TXCINT_data_sent;
    PieVectTable.SCID_TX_INT = &TXDINT_data_sent;
    PieVectTable.SPIB_RX_INT = &SPIB_isr;
    PieVectTable.EMIF_ERROR_INT = &SWI_isr;
    PieVectTable.ADCA1_INT = &ADCA_ISR;
    // ----- code for CAN start here -----
    PieVectTable.CANB0_INT = &can_isr;
    // ----- code for CAN end here -----

    EDIS;    // This is needed to disable write to EALLOW protected registers


    // Initialize the CpuTimers Device Peripheral. This function can be
    // found in F2837xD_CpuTimers.c
    InitCpuTimers();

    // Configure CPU-Timer 0, 1, and 2 to interrupt every given period:
    // 200MHz CPU Freq,                       Period (in uSeconds)
    ConfigCpuTimer(&CpuTimer0, LAUNCHPAD_CPU_FREQUENCY, 10000);
    ConfigCpuTimer(&CpuTimer1, LAUNCHPAD_CPU_FREQUENCY, 20000);
    ConfigCpuTimer(&CpuTimer2, LAUNCHPAD_CPU_FREQUENCY, 40000);

    // Enable CpuTimer Interrupt bit TIE
    CpuTimer0Regs.TCR.all = 0x4000;
    CpuTimer1Regs.TCR.all = 0x4000;
    CpuTimer2Regs.TCR.all = 0x4000;

    init_serialSCIA(&SerialA,115200);


//This is code to support hardcoded points if desired; the final implemented script translated points to the device live one at a time and DID NOT use hardcoded points
//Note that this scaling includes a conversion for .3048, the conversion from feet to meters; this i useful convention if we have a physicla reference distance such as a tile
//    robotdest[0].x = 2*0.3048;    robotdest[0].y = 1*0.3048;
//    robotdest[1].x = 4*0.3048;    robotdest[1].y = 2*0.3048;
//    //middle of bottom
//    robotdest[2].x = 4*0.3048;     robotdest[2].y = 4*0.3048;
//    //outside the course
//    robotdest[3].x = 3*0.3048;     robotdest[3].y = 6*0.3048;
//    //back to middle
//    robotdest[4].x = 1*0.3048;     robotdest[4].y = 7*0.3048;
//    robotdest[5].x = -1*0.3048;     robotdest[5].y = 5*0.3048;
//    robotdest[6].x = -2*0.3048;     robotdest[6].y = 3*0.3048;
//    robotdest[7].x = 0*0.3048;     robotdest[7].y = 0*0.3048;

    // robotdest[0].x = 3*0.3048;    robotdest[0].y = 0*0.3048;
    // robotdest[1].x = 3*0.3048;    robotdest[1].y = 3*0.3048;
    // robotdest[2].x = 0*0.3048;    robotdest[2].y = 3*0.3048;
    // robotdest[3].x = 0*0.3048;    robotdest[3].y = 0*0.3048;


    EALLOW;
    EPwm5Regs.ETSEL.bit.SOCAEN = 0; // Disable SOC on A group
    EPwm5Regs.TBCTL.bit.CTRMODE = 3; // freeze counter
    EPwm5Regs.ETSEL.bit.SOCASEL = 2; // Select Event when counter equal to PRD
    EPwm5Regs.ETPS.bit.SOCAPRD = 1; // Generate pulse on 1st event (pulse is the same as trigger)
    EPwm5Regs.TBCTR = 0x0; // Clear counter
    EPwm5Regs.TBPHS.bit.TBPHS = 0x0000; // Phase is 0
    EPwm5Regs.TBCTL.bit.PHSEN = 0; // Disable phase loading
    EPwm5Regs.TBCTL.bit.CLKDIV = 0; // divide by 1 50Mhz Clock
    EPwm5Regs.TBPRD = 50000; // Set Period to 1ms sample. Input clock is 50MHz.
    // Notice here that we are not setting CMPA or CMPB because we are not using the PWM signal
    EPwm5Regs.ETSEL.bit.SOCAEN = 1; //enable SOCA
    EPwm5Regs.TBCTL.bit.CTRMODE = 0; //unfreeze, and enter up count mode
    EDIS;

    EALLOW;
    EPwm2Regs.TBCTL.bit.CTRMODE = 0; //Lab2: Set counter mode to count up
    EPwm2Regs.TBCTL.bit.FREE_SOFT = 2; //Lab2: Set to free run
    EPwm2Regs.TBCTL.bit.PHSEN = 0; //Lab2: Disable phase loading
    EPwm2Regs.TBCTL.bit.CLKDIV = 0; //Lab2: Set clock div to 1
    EPwm2Regs.TBCTR = 0; //Lab 2: Clear CTR reg
    EPwm2Regs.TBPRD = 2500; //Lab 2: set carrier freq to to 20 khz (50 * 10^6 / (20 * 10^3))
    EPwm2Regs.CMPA.bit.CMPA = 1250; //Lab 2: Set duty cycle to 0
    EPwm2Regs.AQCTLA.bit.CAU = 1; //Lab 2: Clear output once ctr reaches compare
    EPwm2Regs.AQCTLA.bit.ZRO = 2; //Lab 2: Set output high once ctr reaches compare
    EPwm2Regs.AQCTLB.bit.CBU = 1; //Lab 2: Clear output once ctr reaches compare
    EPwm2Regs.AQCTLB.bit.ZRO = 2; //Lab 2: Set output high once ctr reaches compare
    EPwm2Regs.CMPB.bit.CMPB = 1250; //Lab 2: Set duty cycle to 0
    EPwm2Regs.TBPHS.bit.TBPHS = 0; //Lab 2: just in case
    GPIO_SetupPinMux(2, GPIO_MUX_CPU1, 1); //Lab 2: Setup GPIO2 to EPWM2A (select 1)
    GPIO_SetupPinMux(3, GPIO_MUX_CPU1, 1); //Lab 2: Setup GPIO3 to EPWM2B (select 1)
    EDIS;

    //A&J Comment - Here we once again must set up the peripheral registers to support PWM control of the servo motors.
    EALLOW;
    EPwm8Regs.TBCTL.bit.CLKDIV = 4; //A&J Comment - note that we use a non-default clock divide here such that we can reach a PWM carrier frequency of 50HZ, this would be impossible with only the 16-bit integer of TBPRD. Setting it to four is a 16x clock division which is enough to reach 50HZ.
    EPwm8Regs.TBCTL.bit.FREE_SOFT = 3;
    EPwm8Regs.TBCTL.bit.CTRMODE = 0;
    EPwm8Regs.TBCTL.bit.PHSEN = 0;
    EPwm8Regs.TBCTR = 0;
    EPwm8Regs.TBPRD = 62500;
    EPwm8Regs.CMPA.bit.CMPA = 0;
    EPwm8Regs.AQCTLA.bit.CAU = 1;
    EPwm8Regs.AQCTLA.bit.ZRO = 2;
    EPwm8Regs.TBPHS.bit.TBPHS = 0;
    EPwm8Regs.AQCTLB.bit.CBU = 1;
    EPwm8Regs.AQCTLB.bit.ZRO = 2;
    EPwm8Regs.TBPHS.bit.TBPHS = 0;
    GPIO_SetupPinMux(14, GPIO_MUX_CPU1, 1);
    GPIO_SetupPinMux(15, GPIO_MUX_CPU1, 1);
    EDIS;

    EALLOW;
    //write configurations for all ADCs ADCA, ADCB, ADCC, ADCD
    AdcaRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
    AdcbRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
    AdccRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
    AdcdRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
    AdcSetMode(ADC_ADCA, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE); //read calibration settings
    AdcSetMode(ADC_ADCB, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE); //read calibration settings
    AdcSetMode(ADC_ADCC, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE); //read calibration settings
    AdcSetMode(ADC_ADCD, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE); //read calibration settings
    //Set pulse positions to late
    AdcaRegs.ADCCTL1.bit.INTPULSEPOS = 1;
    AdcbRegs.ADCCTL1.bit.INTPULSEPOS = 1;
    AdccRegs.ADCCTL1.bit.INTPULSEPOS = 1;
    AdcdRegs.ADCCTL1.bit.INTPULSEPOS = 1;
    //power up the ADCs
    AdcaRegs.ADCCTL1.bit.ADCPWDNZ = 1;
    AdcbRegs.ADCCTL1.bit.ADCPWDNZ = 1;
    AdccRegs.ADCCTL1.bit.ADCPWDNZ = 1;
    AdcdRegs.ADCCTL1.bit.ADCPWDNZ = 1;
    //delay for 1ms to allow ADC time to power up
    DELAY_US(1000);

    //Select the channels to convert and end of conversion flag
    //Many statements commented out, To be used when using ADCA or ADCB
    //ADCA
    AdcaRegs.ADCSOC0CTL.bit.CHSEL = 2; //SOC0 will convert Channel you choose Does not have to be A0
    AdcaRegs.ADCSOC0CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    AdcaRegs.ADCSOC0CTL.bit.TRIGSEL = 0xD;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC0
    AdcaRegs.ADCSOC1CTL.bit.CHSEL = 3; //SOC1 will convert Channel you choose Does not have to be A1
    AdcaRegs.ADCSOC1CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    AdcaRegs.ADCSOC1CTL.bit.TRIGSEL = 0xD;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC1
    AdcaRegs.ADCINTSEL1N2.bit.INT1SEL = 1; //set to last SOC that is converted and it will set INT1 flag ADCA1
    AdcaRegs.ADCINTSEL1N2.bit.INT1E = 1; //enable INT1 flag
    AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //make sure INT1 flag is cleared

    //ADCB
    //AdcbRegs.ADCSOC0CTL.bit.CHSEL = ???; //SOC0 will convert Channel you choose Does not have to be B0
    //AdcbRegs.ADCSOC0CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    //AdcbRegs.ADCSOC0CTL.bit.TRIGSEL = ???; // EPWM5 ADCSOCA or another trigger you choose will trigger SOC0
    //AdcbRegs.ADCSOC1CTL.bit.CHSEL = ???; //SOC1 will convert Channel you choose Does not have to be B1
    //AdcbRegs.ADCSOC1CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    //AdcbRegs.ADCSOC1CTL.bit.TRIGSEL = ???; // EPWM5 ADCSOCA or another trigger you choose will trigger SOC1
    //AdcbRegs.ADCSOC2CTL.bit.CHSEL = ???; //SOC2 will convert Channel you choose Does not have to be B2
    //AdcbRegs.ADCSOC2CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    //AdcbRegs.ADCSOC2CTL.bit.TRIGSEL = ???; // EPWM5 ADCSOCA or another trigger you choose will trigger SOC2
    //AdcbRegs.ADCSOC3CTL.bit.CHSEL = ???; //SOC3 will convert Channel you choose Does not have to be B3
    //AdcbRegs.ADCSOC3CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    //AdcbRegs.ADCSOC3CTL.bit.TRIGSEL = ???; // EPWM5 ADCSOCA or another trigger you choose will trigger SOC3
    //AdcbRegs.ADCINTSEL1N2.bit.INT1SEL = ???; //set to last SOC that is converted and it will set INT1 flag ADCB1
    //AdcbRegs.ADCINTSEL1N2.bit.INT1E = 1; //enable INT1 flag
    //AdcbRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //make sure INT1 flag is cleared

    //ADCD
    //  AdcdRegs.ADCSOC0CTL.bit.CHSEL = ???; // set SOC0 to convert pin D0
    //  AdcdRegs.ADCSOC0CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    //  AdcdRegs.ADCSOC0CTL.bit.TRIGSEL = ???; // EPWM5 ADCSOCA will trigger SOC0
    //  AdcdRegs.ADCSOC1CTL.bit.CHSEL = ???; //set SOC1 to convert pin D1
    //  AdcdRegs.ADCSOC1CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    //  AdcdRegs.ADCSOC1CTL.bit.TRIGSEL = ???; // EPWM5 ADCSOCA will trigger SOC1
    //AdcdRegs.ADCSOC2CTL.bit.CHSEL = ???; //set SOC2 to convert pin D2
    //AdcdRegs.ADCSOC2CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    //AdcdRegs.ADCSOC2CTL.bit.TRIGSEL = ???; // EPWM5 ADCSOCA will trigger SOC2
    //AdcdRegs.ADCSOC3CTL.bit.CHSEL = ???; //set SOC3 to convert pin D3
    //AdcdRegs.ADCSOC3CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    //AdcdRegs.ADCSOC3CTL.bit.TRIGSEL = ???; // EPWM5 ADCSOCA will trigger SOC3
    //  AdcdRegs.ADCINTSEL1N2.bit.INT1SEL = ???; //set to SOC1, the last converted, and it will set INT1 flag ADCD1
    //  AdcdRegs.ADCINTSEL1N2.bit.INT1E = 1; //enable INT1 flag
    //  AdcdRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //make sure INT1 flag is cleared

    EDIS;


    // Enable DACA and DACB outputs
    EALLOW;
    DacaRegs.DACOUTEN.bit.DACOUTEN = 1; //enable dacA output-->uses ADCINA0
    DacaRegs.DACCTL.bit.LOADMODE = 0;   //load on next sysclk
    DacaRegs.DACCTL.bit.DACREFSEL = 1;  //use ADC VREF as reference voltage

    DacbRegs.DACOUTEN.bit.DACOUTEN = 1; //enable dacB output-->uses ADCINA1
    DacbRegs.DACCTL.bit.LOADMODE = 0;   //load on next sysclk
    DacbRegs.DACCTL.bit.DACREFSEL = 1;  //use ADC VREF as reference voltage
    EDIS;

    setupSpib();
    init_eQEPs();

    // Enable CPU int1 which is connected to CPU-Timer 0, CPU int13
    // which is connected to CPU-Timer 1, and CPU int 14, which is connected
    // to CPU-Timer 2:  int 12 is for the SWI.  
    IER |= M_INT1;
    IER |= M_INT6;
    IER |= M_INT8;  // SCIC SCID
    IER |= M_INT9;  // SCIA  CANB
    IER |= M_INT12;
    IER |= M_INT13;
    IER |= M_INT14;

    // ADCA interrupt
    PieCtrlRegs.PIEIER1.bit.INTx1 = 1;
    // Enable TINT0 in the PIE: Group 1 interrupt 7
    PieCtrlRegs.PIEIER1.bit.INTx7 = 1;
    // Enable SWI in the PIE: Group 12 interrupt 9
    PieCtrlRegs.PIEIER12.bit.INTx9 = 1;

    PieCtrlRegs.PIEIER6.bit.INTx3 = 1;

    // ----- code for CAN start here -----
    // Enable CANB in the PIE: Group 9 interrupt 7
    PieCtrlRegs.PIEIER9.bit.INTx7 = 1;
    // ----- code for CAN end here -----

    // ----- code for CAN start here -----
    // Enable the CAN interrupt signal
    CanbRegs.CAN_GLB_INT_EN.bit.GLBINT0_EN = 1;
    // ----- code for CAN end here -----

    //init_serialSCIB(&SerialB,115200);
    init_serialSCIC(&SerialC,115200);
    init_serialSCID(&SerialD,115200);
    // Enable global Interrupts and higher priority real-time debug events
    EINT;  // Enable Global interrupt INTM
    ERTM;  // Enable Global realtime interrupt DBGM

    // ----- code for CAN start here -----

    //    // Transmit Message
    //    // Initialize the transmit message object used for sending CAN messages.
    //    // Message Object Parameters:
    //    //      Message Object ID Number: 0
    //    //      Message Identifier: 0x1
    //    //      Message Frame: Standard
    //    //      Message Type: Transmit
    //    //      Message ID Mask: 0x0
    //    //      Message Object Flags: Transmit Interrupt
    //    //      Message Data Length: 4 Bytes
    //    //
    //    CANsetupMessageObject(CANB_BASE, TX_MSG_OBJ_ID, 0x1, CAN_MSG_FRAME_STD,
    //                           CAN_MSG_OBJ_TYPE_TX, 0, CAN_MSG_OBJ_TX_INT_ENABLE,
    //                           TX_MSG_DATA_LENGTH);

    // Measured Distance from 1
    // Initialize the receive message object 1 used for receiving CAN messages.
    // Message Object Parameters:
    //      Message Object ID Number: 1
    //      Message Identifier: 0x060b0101
    //      Message Frame: Standard
    //      Message Type: Receive
    //      Message ID Mask: 0x0
    //      Message Object Flags: Receive Interrupt
    //      Message Data Length: 8 Bytes (Note that DLC field is a "don't care"
    //      for a Receive mailbox)
    //
    CANsetupMessageObject(CANB_BASE, RX_MSG_OBJ_ID_1, 0x060b0101, CAN_MSG_FRAME_EXT,
                          CAN_MSG_OBJ_TYPE_RX, 0, CAN_MSG_OBJ_RX_INT_ENABLE,
                          RX_MSG_DATA_LENGTH);

    // Measured Distance from 2
    // Initialize the receive message object 2 used for receiving CAN messages.
    // Message Object Parameters:
    //      Message Object ID Number: 2
    //      Message Identifier: 0x060b0102
    //      Message Frame: Standard
    //      Message Type: Receive
    //      Message ID Mask: 0x0
    //      Message Object Flags: Receive Interrupt
    //      Message Data Length: 8 Bytes (Note that DLC field is a "don't care"
    //      for a Receive mailbox)
    //

    CANsetupMessageObject(CANB_BASE, RX_MSG_OBJ_ID_2, 0x060b0103, CAN_MSG_FRAME_EXT,
                          CAN_MSG_OBJ_TYPE_RX, 0, CAN_MSG_OBJ_RX_INT_ENABLE,
                          RX_MSG_DATA_LENGTH);

    // Measurement Quality from 1
    // Initialize the receive message object 2 used for receiving CAN messages.
    // Message Object Parameters:
    //      Message Object ID Number: 3
    //      Message Identifier: 0x060b0201
    //      Message Frame: Standard
    //      Message Type: Receive
    //      Message ID Mask: 0x0
    //      Message Object Flags: Receive Interrupt
    //      Message Data Length: 8 Bytes (Note that DLC field is a "don't care"
    //      for a Receive mailbox)
    //

    CANsetupMessageObject(CANB_BASE, RX_MSG_OBJ_ID_3, 0x060b0201, CAN_MSG_FRAME_EXT,
                          CAN_MSG_OBJ_TYPE_RX, 0, CAN_MSG_OBJ_RX_INT_ENABLE,
                          RX_MSG_DATA_LENGTH);

    // Measurement Quality from 2
    // Initialize the receive message object 2 used for receiving CAN messages.
    // Message Object Parameters:
    //      Message Object ID Number: 4
    //      Message Identifier: 0x060b0202
    //      Message Frame: Standard
    //      Message Type: Receive
    //      Message ID Mask: 0x0
    //      Message Object Flags: Receive Interrupt
    //      Message Data Length: 8 Bytes (Note that DLC field is a "don't care"
    //      for a Receive mailbox)
    //

    CANsetupMessageObject(CANB_BASE, RX_MSG_OBJ_ID_4, 0x060b0203, CAN_MSG_FRAME_EXT,
                          CAN_MSG_OBJ_TYPE_RX, 0, CAN_MSG_OBJ_RX_INT_ENABLE,
                          RX_MSG_DATA_LENGTH);

    //
    // Start CAN module operations
    //
    CanbRegs.CAN_CTL.bit.Init = 0;
    CanbRegs.CAN_CTL.bit.CCE = 0;

    //    // Initialize the transmit message object data buffer to be sent
    //    txMsgData[0] = 0x12;
    //    txMsgData[1] = 0x34;
    //    txMsgData[2] = 0x56;
    //    txMsgData[3] = 0x78;


    //    // Loop Forever - A message will be sent once per second.
    //    for(;;)
    //    {
    //
    //        CANsendMessage(CANB_BASE, TX_MSG_OBJ_ID, TX_MSG_DATA_LENGTH, txMsgData);
    //        txMsgCount++;
    //        DEVICE_DELAY_US(1000000);
    //    }


    // ----- code for CAN end here -----

    // IDLE loop. Just sit and loop forever (optional):
    while(1)
    {
        if (UARTPrint == 1 ) {
            //serial_printf(&SerialA,"%d, %d, %d    %d, %d, %d\n\r",accelx_raw,accely_raw,accelz_raw,gyrox_raw,gyroy_raw,gyroz_raw);
            //serial_printf(&SerialA,"D1 %.3f D2 %.3f",RightDistance,FrontDistance);
            serial_printf(&SerialA,"D1 %ld D2 %ld Avg %.2f ",dis_1,dis_3,RightDistanceAverage);
            serial_printf(&SerialA," St1 %ld St2 %ld\n\r",measure_status_1,measure_status_3);
            UARTPrint = 0;
        }
    }
}


// SWI_isr,  Using this interrupt as a Software started interrupt
__interrupt void SWI_isr(void) {

    // These three lines of code allow SWI_isr, to be interrupted by other interrupt functions
    // making it lower priority than all other Hardware interrupts.
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP12;
    asm("       NOP");                    // Wait one cycle
    EINT;                                 // Clear INTM to enable interrupts
    //  SWI ISR Code here
    //This includes all of our dynamics cmoputation for waypoint following
    VLeftK = (LeftWheel - LeftWheel_1)/0.004;
    VRightK = (RightWheel - RightWheel_1)/0.004;
    VLeftK = VLeftK * Rwh;  // meters per second
    VRightK = VRightK * Rwh;  // meters per second

    PhiR = Rwh/Wr *(RightWheel - LeftWheel);
    //    dotThetaAvg = ((VRightK + VLeftK )/2)*5.15;
    dotThetaAvg = 0.5*(VLeftK+VRightK);  // VLeftK and VRightK are in meters/s
    //dotxRk = dotThetaAvg*cos(PhiR);
    //dotyRk = dotThetaAvg*sin(PhiR);
    dotxRk = dotThetaAvg*cos(yaw);
    dotyRk = dotThetaAvg*sin(yaw);

    xRk = xRk_1 + (dotxRk+dotxRk_1)*0.002;
    yRk = yRk_1 + (dotyRk+dotyRk_1)*0.002;

    //    LeftVelFilt = 0.6*LeftVelFilt_1 + 100*LeftWheel - 100*LeftWheel_1;
    //    RightVelFilt = 0.6*RightVelFilt_1 + 100*RightWheel - 100*RightWheel_1;
    //    gyrorate_dot = 0.6*gyrorate_dot_1 + 100*gyro_value - 100*gyro_value_1;
    //
    //    ubal = -K1*tilt_value - K2*gyro_value - K3*(LeftVelFilt + RightVelFilt)/2.0 - K4*gyrorate_dot;
    //    uRight = ubal/2.0;
    //    uLeft = ubal/2.0;

    //    if ((measure_status_1 == 0) || (measure_status_1 == 2) ) {
    //        if (dis_1 > 500) {
    //            RightDistanceValues[0] = 300;
    //        } else {
    //            RightDistanceValues[0] = dis_1;
    //        }
    //        RightDistanceAverage = 0;
    //        for (int i = 0;i < AVERAGESIZE; i++) {
    //            RightDistanceAverage += RightDistanceValues[i]/AVERAGESIZE;
    //        }
    //        for (int i = AVERAGESIZE-1; i > 0; i--) {
    //            RightDistanceValues[i] = RightDistanceValues[i-1];
    //        }
    //        RightDistance = RightDistanceAverage;
    //        if (measure_status_1 == 0) {  // ignore average if measure_status_1 = 0,  good measurement
    //            RightDistance = dis_1;
    //        }
    //
    //    } else {
    //        RightDistance = 1400;
...

This file has been truncated, please download it to see its full contents.

import cv2 as cv
import numpy as np
import matplotlib.pyplot as plt
from skimage.morphology import skeletonize  # pip install scikit-image


def img_to_stroke_points(fileName, n_points_per_path=50, min_path_len=5):

    # OpenCV Otsu graysclaing
    image = cv.imread(fileName, cv.IMREAD_GRAYSCALE)
    if image is None:
        raise ValueError(f"Could not read image from {fileName}")
    height, width = image.shape
    _, binary = cv.threshold(
        image, 0, 255, cv.THRESH_BINARY_INV + cv.THRESH_OTSU
    )

    binary_norm = (binary > 0).astype(np.uint8)

    # scikit skeletonization (1 pixel-wide strokes)
    skel = skeletonize(binary_norm).astype(np.uint8)
    h, w = skel.shape
    visited = np.zeros_like(skel, dtype=bool)

    neighbors = [(-1, -1), (-1, 0), (-1, 1),
                 (0, -1),           (0, 1),
                 (1, -1),  (1, 0),  (1, 1)]

    def get_neighbors(r, c):
        pts = []
        for dr, dc in neighbors:
            rr, cc = r + dr, c + dc
            if 0 <= rr < h and 0 <= cc < w and skel[rr, cc] and not visited[rr, cc]:
                pts.append((rr, cc))
        return pts

    paths = []

    # endpoints
    for r in range(h):
        for c in range(w):
            if skel[r, c] and not visited[r, c]:
                nbr_count = 0
                for dr, dc in neighbors:
                    rr, cc = r + dr, c + dc
                    if 0 <= rr < h and 0 <= cc < w and skel[rr, cc]:
                        nbr_count += 1

                if nbr_count == 1:
                    path = []
                    stack = [(r, c)]
                    while stack:
                        cr, cc = stack.pop()
                        if visited[cr, cc]:
                            continue
                        visited[cr, cc] = True
                        path.append((cc, cr))  # (x, y) in pixel coords (y down)
                        for n in get_neighbors(cr, cc):
                            stack.append(n)

                    if len(path) >= min_path_len:
                        paths.append(np.array(path, dtype=float))

    # remaining loops/islands
    for r in range(h):
        for c in range(w):
            if skel[r, c] and not visited[r, c]:
                path = []
                stack = [(r, c)]
                while stack:
                    cr, cc = stack.pop()
                    if visited[cr, cc]:
                        continue
                    visited[cr, cc] = True
                    path.append((cc, cr))
                    for n in get_neighbors(cr, cc):
                        stack.append(n)

                if len(path) >= min_path_len:
                    paths.append(np.array(path, dtype=float))

    # lip y so origin is bottom-left, y up
    for i, path in enumerate(paths):
        # path[:,1] is image row index (0 at top); convert to bottom-origin y
        path[:, 1] = height - 1 - path[:, 1]
        paths[i] = path

    # resample each path to fixed number of points
    sampled_paths = []
    for path in paths:
        deltas = np.diff(path, axis=0)
        dists = np.hypot(deltas[:, 0], deltas[:, 1])
        cumulative = np.concatenate(([0], np.cumsum(dists)))
        total_length = cumulative[-1]

        if total_length == 0:
            sampled = np.repeat(path[:1], n_points_per_path, axis=0)
        else:
            s_locs = np.linspace(0, total_length, n_points_per_path, endpoint=True)
            sampled = np.column_stack([
                np.interp(s_locs, cumulative, path[:, 0]),
                np.interp(s_locs, cumulative, path[:, 1]),
            ])
        sampled_paths.append(sampled)

    # normalize coordinates to [0, 1]
    norm_paths = []
    for sp in sampled_paths:
        sp_norm = np.empty_like(sp, dtype=float)
        sp_norm[:, 0] = sp[:, 0] / float(width)   # x / width
        sp_norm[:, 1] = sp[:, 1] / float(height)  # y / height
        norm_paths.append(sp_norm)

    # plotting:
    # original image (y down)
    plt.figure(figsize=(6, 6))
    plt.imshow(image, cmap='gray')
    plt.title('Original (image coords)')
    plt.axis('off')
    plt.show()
    # skeleton in image coords
    plt.figure(figsize=(6, 6))
    plt.imshow(skel, cmap='gray')
    plt.title('Skeleton (image coords)')
    plt.axis('off')
    plt.show()
    # sampled paths in math coords (origin bottom-left, y up)
    plt.figure(figsize=(6, 6))
    plt.imshow(np.zeros_like(image), cmap='gray', origin='lower')
    for sp in sampled_paths:
        plt.plot(sp[:, 0], sp[:, 1], linewidth=1)
    plt.title('Sampled Stroke Paths (Math Coords)')
    plt.axis('equal')
    plt.axis('off')
    plt.show()
    # sampled points of paths (not conntected)
    plt.figure(figsize=(6, 6))
    plt.imshow(np.zeros_like(image), cmap='gray', origin='lower')
    for sp in sampled_paths:
        plt.scatter(sp[:, 0], sp[:, 1], s=5)
    plt.title('Sampled Stroke Points (Math Coords)')
    plt.axis('equal')
    plt.axis('off')
    plt.show()

    numpy_output = np.array(norm_paths)
    # printing copy-pastable output
    print(np.array_repr(numpy_output))

    return numpy_output

ME461 Fall 2025 Line Drawing Robot