Odometry - June 06, 2023

Introduction

In the last entry we decided which programming paradigm and programming architecture we will be using.

Following the architecture we developed in the last entry, we can create a basic UML diagram showing the structure of the various classes we will use for our first testbed robot:

RobotClassUML

Below you can see what the UML diagram would look like in code. The below code was the product of a few hours of debugging; the largest consumer of my time while developing this code was making sure that the implementation of the math behind the odometry was correct.

Robot.h - 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

#pragma once
using namespace vex;

class Robot{
public:

  // Class constructor
  Robot();

  // Drivetrain motors
  motor FR = motor(PORT9, ratio18_1, false);
  motor BR = motor(PORT10, ratio18_1, false);
  motor FL = motor(PORT2, ratio18_1, true);
  motor BL = motor(PORT1, ratio18_1, true);
  
  // Odometry object
  Odometry odometry = Odometry();
};

Odometry.h - 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

#pragma once
using namespace vex;

class Odometry{
public:

    Odometry();

    TrackingWheel fr_fwd = TrackingWheel(rotation(PORT5), 3.25, true);
    TrackingWheel fl_fwd = TrackingWheel(rotation(PORT6), 3.25);
    TrackingWheel b_s = TrackingWheel(rotation(PORT8), 3.25);

    inertial inert = inertial(PORT11, left);

    // radius of 
    double strafe_radius = 4.5;

    // Variables to calculate delta_theta
    double previous_heading = 0;
    double current_heading = 0;
    
    // Variables for cartesian coordinates
    double x = 0;
    double y = 0;
    double internal_h_rad = 0;
    double h = 0;
    
    // Member functions
    void Update();
    void Callibrate(double _x = 0, double _y = 0, double _h = 0);
    void SetPosition(double _x, double _y, double _h);
};

Robot.cpp - 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46

#include "vex.h"
using namespace vex;

Odometry::Odometry() {
    // Constructor
}

void Odometry::Update() {

    // Get change in encoder position in inches
    double fr_fwd_travel = fr_fwd.get_travel();
    double fl_fwd_travel = fl_fwd.get_travel();
    double b_s_travel = b_s.get_travel();

    // Calculate delta_theta.
    previous_heading = current_heading;
    current_heading = degToRad(inert.rotation(deg));
    double delta_theta = current_heading - previous_heading;

    // Calculate fwd and strafe travel distances; (x and y respectively.)
    double fwd_travel = (fr_fwd_travel + fl_fwd_travel) / 2.0;
    double strafe_travel = b_s_travel + delta_theta * strafe_radius;

    // Rotate the point (fwd, strafe) to convert from a local coordinate shift to a global coordinate shift.
    rotatePoint(fwd_travel, strafe_travel, previous_heading);

    // Update each location variable. We keep things in radians internally because the math is nicer this way.
    internal_h_rad = wrapAngleRad(current_heading);
    h = radToDeg(internal_h_rad);
    x += fwd_travel;
    y += strafe_travel;
}

void Odometry::Callibrate(double _x, double _y, double _h) {

    // Reset inertial sensor.
    inert.calibrate();
    inert.setHeading(_h, deg);
    inert.setRotation(_h, deg);

    // Apply changes to odometry location.
    x = _x;
    y = _y;
    h = _h;
    internal_h_rad = degToRad(h);
}

Entry Signatures -


Ayla Clark
Caleb Carlson
Tucker Nielson
Thomas Reid