XY Location of the Robot Car (UIUC ME 461)

This project began with some inspiration from the idea of rovers on a planet and gradually took form on a simplified level. There are several key components that will be discussed shortly: finding the pose of the robot car, commanding the car to move to different locations, hitting a ball, and characterizing the accuracy of the car. Ultimately, these form the basic abilities of a robot in a space operation.

An overview video of the project's parts:

The first aspect was finding the location of the robot relative to some coordinate system. This was based upon the work of Yorihisa Yamamoto, a MathWorks employee who had previously designed a balancing two-wheeled robot. Quadrature encoders are used to determine the wheel position in radians. In conjunction with the wheel radius and wheelbase, the bearing of the car can be calculated with the equations below. New values are updated every 4 ms in the interrupt function. In terms of velocity, the change in position over the period of time is accurate enough for the robot and does not require a filtered velocity.

The main component was done by modifying code provided by Dan Block and focused on finding the position of a target relative to the current position of the robot car. This was an implementation of an algorithm called Gradient Descent, or path-planning: With a defined point in a function, small steps can be taken to reach the targeted local minimum in the direction of greatest decrease. In this case, it is finding the shortest path to a target.

With an XY bearing established, the distance could be found between the two points, and the angle between could be calculated using the inverse tangent function. The normalized angle between -Pi and Pi at which the robot car is at (Phi) can then be used to define an error that will turn the car in place to a straight-line orientation with the target. This is because the quadrature encoder will only record a large distance traveled if the robot has been spun multiple times. The remainder of this code uses a proportional gain that incorporates the ‘turn’ component and distance from the target to dictate the ‘velocity’ and movement of the car. ‘Turn’ and ‘Velocity’ are passed by reference as these are continuously updated in the control law. Both the left wheel and right wheel have a different control effort as the error is taken from one wheel and added to the other for turning. More technical details can be seen in the findAngle and xy_control function.

float findAngle(float dy, float dx) // Finding angle.
{
    float angle = 0;

    if (fabs(dy) <= 0.001) {  // These are for along the axis
        if (dx >= 0.0) {
            angle = 0.0;
        } else {
            angle = PI;
        }
    } else if (fabs(dx) <= 0.001) {
        if (dy > 0.0) {
            angle = HALFPI;
        } else {
            angle = -HALFPI;
        }
    } else {
        angle = atan2(dy,dx);   // If not along the axis exactly, then it will simply compute
    }
    return angle;
}

Proportional Control for Turn Component: Vref is simply 1 ft/s or less depending on the minimum value between the two. The turnerror is the angular difference between the current angle and the desired angle of the target. The 'turn' that gets inputted into the control law is then simply scaled up by a value.

*Vref = dir*MIN(dist,1);    // Note the use of pointers. These values get used outside of this function, but this allows for them to be updated and then reused outside each time.

    if (fabsf(*Vref) > 0.0) { // Proportional Control for the turn value. This gets input into the error equations in CPUTIMER1_isr.
        // if robot 1 tile away from target use a scaled KP value.
        *turn = (*Vref*2)*turnerror;
    } else {
        // normally use a Kp gain of 2
        *turn = 2*turnerror;
    }

Moving to Singular Point:

Moving to Two Points:

As seen in these videos, the car does a good job of reaching the targets and also moving to additional targets. The robot car will drive a speed of 1 ft/s before slowing and the xy_control function will return TRUE when it reaches an acceptable radius from the target. It will stop moving once it hits the target’s actual radius. Multiple targets were reached by creating an array of XY coordinates. When the function returned TRUE, the next set of coordinates was passed to immediately command the robot in a new direction. The robot will begin to turn in place before completely reaching the target depending on the radius value. This function is continuously called and it is important that conditional statements operate on what the function returns.

if (dist < target_radius_near) {
        target_near = TRUE;  // This is the functions return value. Once this is reach the condition is satisfied that it could move to anotehr point. Otherwise it will keep going normally.
        // Arrived to the target's (X,Y)
        if (dist < target_radius) {
            dir = 0.0F;  // When the target is reached (last one), then the car will stop moving
            turnerror = 0.0F;
        } else {
            // if we overshot target, we must change direction. This can cause the robot to bounce back and forth when
            // remaining at a point.
            if (fabsf(turnerror) > HALFPI) {
                dir = -dir;
            }
            turnerror = 0;
        }
    } else {
        target_near = FALSE;  // Default rreturn value
    }

XY array that is the only thing that needs to be modified:

'Target' either returns true or false depending on the returned value from the xy_control function. The conditional statement allows for the index of the array to be increased as long as the end of the array has not been reached. Since the function is called continuously, it will eventually stop the car when the final target is reached.

#define XYLENGTH 8
float xDesired[XYLENGTH] = {4.0, 4.0, 0.0, 0.0, 4.0, 4.0, 0.0, 0.0};
float yDesired[XYLENGTH] = {0.0, -4.0, -4.0, 0.0, 0.0, -4.0, -4.0, 0.0};

target = xy_control(&Vref, &turn, 1.0, xPosition, yPosition, xDesired[target_counter], yDesired[target_counter], Phi, .1, .2);  // Calling the function every time this interrupt is called
    if (target == 1) {   // The robot car has entered into the acceptable radius. It will either continue onto the actual radius and stop or redirect to a new point
        if (target_counter < XYLENGTH - 1) {   // Passing new points up until the end of the array. The new point is far away enough that the function will immediately return 0 for a new point
            target_counter++;

Something fun I did was to attach an RC servo motor to the PCB board and this was seen in the cover image. A paddle was made from cardboard and attached to the RC servo. While using tape would not be the most optimal way of doing so, this was a prototype to simply test an idea. By changing the compare value of the PWM, I was able to change the angle of the RC servo to sweep through. It was limited in power and a bit difficult sometimes as the robot car isn't perfectly accurate. Having the robot car swing forward into the x-axis before hitting the ball provided a little bit of momentum.

As a correction to the video, the PWM control for the RC servo operates on a 50 Hz frequency, or 20 ms period. The frequency is divided by 16 when the CLKDIV register is set to 4 and is needed to obtain the correct period. The bottom limit of the 3% duty cycle is now 1875 counts of the 62500.

GPIO_SetupPinMux(14, GPIO_MUX_CPU1, 1);
    GPIO_SetupPinMux(15, GPIO_MUX_CPU1, 1);
    // Initializing all the registers of of epwm8 for the RC servo.
    EPwm8Regs.TBCTL.bit.CTRMODE = 0;
    EPwm8Regs.TBCTL.bit.FREE_SOFT = 2;
    EPwm8Regs.TBCTL.bit.PHSEN = 0;
    EPwm8Regs.TBCTL.bit.CLKDIV = 4;  // wanting to divide by 16. 3 bits  1 0 0. No way to be 100....
    EPwm8Regs.TBCTR = 0;
    EPwm8Regs.TBPRD = 62500; //20 ms period would mean a count of 1,000,000. But a unsigned 16 bits can only go up to 65535
    EPwm8Regs.CMPA.bit.CMPA = 1875; // initially setting this to be 8% of the new duty cycle. This will hold it at one steady value.
    EPwm8Regs.CMPB.bit.CMPB = 1875;
    EPwm8Regs.AQCTLA.bit.CAU = 1;
    EPwm8Regs.AQCTLB.bit.CBU = 1;
    EPwm8Regs.AQCTLA.bit.ZRO = 2;
    EPwm8Regs.AQCTLB.bit.ZRO = 2;
    EPwm8Regs.TBPHS.bit.TBPHS = 0;

Hitting a Ball:

The final component of this project was determining how the robot will drift over time. In the array for defining the XY coordinates that the car will travel to, it was made to drive in a square up to 3 times around and with a square size of two or four feet. A video of one of the testing cases can be seen below.

Characterizing Drift and Accuracy:

The car begins to drift noticeably from the paper targets on the floor that represent the corners of the box. It is traveling in a clockwise direction, and the path traveled begins to rotate more and more in a clockwise fashion as the distance traveled by the car increases. In this case, the error should compound with the greatest value at the end of the drive. Using the front-middle position of the car as a reference to the floor, I measured the starting and final stopping point for drifting. The results are displayed in the chart below.

The drift of the robot car increases almost exponentially as the total distance traveled and the number of turns increases with the squares. More data points and test runs would need to be done to confirm this trend as it is possible for human error for this. The angle deviation from the theoretical 90 degrees (not plotted) increases from less than 5 degrees at the 1st square cycle to just over 30 degrees at the end. This over-rotation could simply be due to the accuracy limitations of this specific implementation but could also be due to the wheels slipping on the wood floor.

Overall, this was a great way to have a simple implementation of an algorithm that sometimes does get utilized for robots and machine learning in the real world. It was also interesting to see that the robot car began to drift so much even with a simple environment and that it is an important factor to consider in its design.

Reference for Gradient Descent:

https://ml-cheatsheet.readthedocs.io/en/latest/gradient_descent.html