20. Project development¶

Update on what system I’m using¶

After looking at all of my options, I’ve decided that GPS is going to be the best system for this project, beacuse the wagon will be outside and interference shouldn’t be a risk. I will be following the GPS tutorial that I referenced in my implications and applications link. However, I plan to start by learning each system individually before following the tutorial.

This week I updated my final project progress, and used time management tools to plan out the next 9 days of my project.

What’s Done¶

For more details of everything done, see the referenced week or my final project tab

CNC’d Wagon, planning on making original design but do have one that I can use in case the other doesn’t get done, see CNC week.
Starting electronics: Time of flight (see input week and electronics production), using bluetooth module, controlling motors (see output week)

-Little progress on coding, but I know what libraries I will use

What needs to be done¶

-No progress on GPS or compass module, but have found tutorials.

Everything is breadboarded right now, need to design and mill boards
Need to design electronics for mounting
Need to redesign a scaled version of the wheels and tires, as well as mold and cast them

Gantt Chart¶

I created a Gantt chart with eveything still left to do, and the timeframes for each task.

Timing¶

Access to lab from 4-8 per day with 8 days left = 40 hours in lab. Here’s how I plan to divide my time

Wagon Redesign and Milling: 5 Hours

Wheels and Tires Milling, + molding and casting: 8 hours

Designing electronics mounts: 2 Hours

Electronics: GPS module and compass: 10 Hours

Electronics: Time of flight and Motors: 3 hours

Electronics: Designing and Milling Boards: 5 Hours

Programming: 4 Hours

Systems Integration, Wire Mangagment: 3 Hours

I plan to create my slide and video outside of lab time, and will get some other work done outside of Lab Time as well to leave extra time.

HC-05 Bluetooth¶

Right now, I am trying to This tutorial by How-To Mechatronics to connect a seeed studio with an HC-05 bluetooth module to my laptop using arduino and processing. This way, I will first be able to test my interface from my laptop before developing an interface for a mobile device.

Starting Fresh, Reconsiderations.¶

After not being able to finish in the summer, I’ve had some time to step back and look at my project from a few different angles, and have made a lot of reconsiderations.

New Seeeds¶

Over the summer our lab got more of the Seeed studio XIAO series, this time with the NRF52840, ESP32C3, and the SAMD 21 chips. Because the NRF52840 and the ESP32C3 versions of the XIAO have built-in bluetooth, I am going to use one of these boards going forward.

Small mechanical reconsideration¶

With my current setup, I was intending for my caster wheel to be the “Front” of the wagon with 2 wheels in the back. However, this means that the caster wheel would be unstable over rough terrain and the motors would have to account for more variation. So instead, the caster wheel will now be the back, and I will mount the ToF sensor on the backside of the wagon.

Electrical changes¶

Rather than just relying on sensor data for controlling the motors, I will also be using rotary encoders attatched to the motors to recieve their speed.

Developing a steering system- Motor Control Progression¶

I’ve decided that instead of going straight to controlling both motors at once using the PID loop, I’ll do the following progression:

Control the motors using the L298n
Control a single motor with a PID loop
Control a single motor to steer with a PID loop
Control Both motors to steer with a PID loop

Motor control with L298n¶

The first type of control I did was controlling both motors using the l298N to control a heading. I used the l298N to turn one motor forwards and one motor backwards after the rotary encoders recorded a certain amount of steps. I then used the orientation sensor to continue the wagon straight after performing a 90 degree again.

I used the following code to perform this:

#include <ESP32Encoder.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>

Adafruit_BNO055 bno = Adafruit_BNO055();

// Constants for motor control
const int ENA = 9;  // Enable A pin of the L298N motor driver
const int IN1 = 8;   // IN1 pin of the L298N motor driver
const int IN2 = 7;   // IN2 pin of the L298N motor driver
const int ENB = 44;  // Enable B pin of the L298N motor driver
const int IN3 = 6;   // IN3 pin of the L298N motor driver
const int IN4 = 5;   // IN4 pin of the L298N motor driver

// Constants for encoder pins
const int LEFT_ENCODER_A = 1;   // Channel A pin of left motor encoder
const int LEFT_ENCODER_B = 2;   // Channel B pin of left motor encoder
const int RIGHT_ENCODER_A = 3;  // Channel A pin of right motor encoder
const int RIGHT_ENCODER_B = 4;  // Channel B pin of right motor encoder

// Constants for step counts
const int FORWARD_STEPS = 1000;   // Number of steps to move forward
const int TURN_STEPS = 500;       // Number of steps for a 90-degree turn

// Variables to track encoder counts
volatile long leftCount = 0;
volatile long rightCount = 0;

void setup() {
  Serial.begin(115200);

  // Initialize motor control pins
  pinMode(ENA, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(ENB, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  // Initialize encoder pins
  ESP32Encoder leftEncoder;
  ESP32Encoder rightEncoder;
  leftEncoder.attachFullQuad(LEFT_ENCODER_A, LEFT_ENCODER_B);
  rightEncoder.attachFullQuad(RIGHT_ENCODER_A, RIGHT_ENCODER_B);

  // Set starting count value after attaching
  leftEncoder.setCount(0);
  rightEncoder.setCount(0);

  // Start the motors
  analogWrite(ENA, 255);  // Left motor at full speed
  analogWrite(ENB, 255);  // Right motor at full speed

  // Move forward for FORWARD_STEPS
  moveForward(FORWARD_STEPS);

  // Perform a 90-degree turn using BNO055 sensor
  turn90Degrees();

  // Move forward for another FORWARD_STEPS
  moveForward(FORWARD_STEPS);
}

void loop() {
  // Empty loop, everything is done in setup
}

void moveForward(int steps) {
  // Reset encoder counts
  leftCount = 0;
  rightCount = 0;

  while (leftCount < steps || rightCount < steps) {
    // Move forward
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
  }

  // Stop the motors
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}

void turn90Degrees() {
  // Read initial orientation angle
  imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
  float initialHeading = euler.x();

  // Perform turn
  while (abs(euler.x() - initialHeading) < 90) {
    // Turn left (adjust as needed for your robot)
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);

    // Read current orientation angle
    euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
  }

  // Stop the motors
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}

// Interrupt service routines for encoder counts
void IRAM_ATTR leftEncoderISR() {
  leftCount++;
}

void IRAM_ATTR rightEncoderISR() {
  rightCount++;
}

Here is a video of the code:

Controlling a single motor with a PID loop: Understanding PID¶

A PID loop is a type of closed loop system, which has the goal a process reach a set point (desired value). The process variable is controlled by a control system (or a compensator), which changes the actuator output, which is the device that finds the error in the system, and then drives the change. A PID loop finds 3 types of responses: Proportianal response, Integral response, and Derivitve response. Proportional response finds the ration of the output response to the error signal. In my case, if the error of my heading has a proportional magnitude of 10, and I have my proportional gain set to 5, then the response will be 50. Integral response sums the amount of error over time, this allows accumulated error such as from sensor drift or electrical noise to be collected. The derivitive response causes the output to decrease if the variable is increasing rapidly, in my case if something caused the wagon to oscilate the derivitve response would be used to help correct it. Each of these responses can be tuned by changing the order of magnitude of change that each response will create.

Alt text

A Block Diagram of a closed control Loop

Alt text

A block diagram of a PID loop

Controlling a single motor with a PID loop: programming¶

With some help from chatGPT to learn how to implement a PID loop in C++, I created the following code that uses the rotarty encoders to set the motors to a certain position. In this case, the feedback and control come from the same source:

#include <L298N.h>
#include <ESP32Encoder.h>

#define CLK 9 // CLK ENCODER
#define DT 8  // DT ENCODER

ESP32Encoder encoder;

const unsigned int IN1 = 44;
const unsigned int IN2 = 7;
const unsigned int EN = 3;

L298N motor(EN, IN1, IN2);

const float Kp = 1.0; // Proportional gain
const float Ki = 0.0; // Integral gain
const float Kd = 0.0; // Derivative gain

const float setpoint = 1000.0; // Target position
float error, integral = 0, derivative, last_error = 0;
float output;
bool firstIteration = true;

void setup()
{
  encoder.attachHalfQuad(DT, CLK);
  encoder.setCount(0);
  Serial.begin(115200);

  motor.setSpeed(70);
}

void loop()
{
  long newPosition = encoder.getCount() / 2;
  error = setpoint - newPosition;

  // Skip integral term update in the first iteration
  if (!firstIteration) {
    integral += error;
  }
  firstIteration = false;

  derivative = error - last_error;
  output = Kp * error + Ki * integral + Kd * derivative;

  // Limit the output to the motor speed range
  output = constrain(output, -255, 255);

  motor.setSpeed(abs(output));

  if (output > 0)
  {
    motor.backward();
  }
  else if (output < 0)
  {
    motor.forward();
  }
  else
  {
    motor.stop();
  }

  last_error = error;

  // Print debugging information
  Serial.print("Setpoint: ");
  Serial.print(setpoint);
  Serial.print(", Position: ");
  Serial.print(newPosition);
  Serial.print(", Error: ");
  Serial.print(error);
  Serial.print(", Output: ");
  Serial.println(output);

  delay(100); // Adjust as needed based on your control loop frequency
}

Controlling a single motor to steer using a PID loop¶

After getting the motor to hold a position, my next goal was to make the motor hold a heading instead. To do this I edited the original PID code to use the heading as an input instead. One problem that I encoutered at first was that when the heading was outside of 255, a value wasn’t able to be returned. That’s because the PWM values are limited from -255 to 255. To fix this, I had to make a function to scale the values to fit this.

#include <L298N.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <ESP32Encoder.h>

#define CLK 9 // CLK ENCODER
#define DT 8  // DT ENCODER

ESP32Encoder encoder;
Adafruit_BNO055 bno = Adafruit_BNO055(55);

const unsigned int IN1 = 44;
const unsigned int IN2 = 7;
const unsigned int EN = 3;

L298N motor(EN, IN1, IN2);

const float Kp = 1.0; // Proportional gain
const float Ki = 0.0; // Integral gain
const float Kd = 0.0; // Derivative gain

float heading_setpoint = 0.0; // Target heading
float heading_current = 0.0;  // Current heading

float error, integral = 0, derivative, last_error = 0;
float output;

void setup()
{
  encoder.attachHalfQuad(DT, CLK);
  encoder.setCount(0);
  Serial.begin(115200);

  motor.setSpeed(70);

  // Initialize the BNO055 sensor
  if (!bno.begin())
  {
    Serial.println("Could not find a valid BNO055 sensor, check wiring!");
    while (1)
      ;
  }
  bno.setExtCrystalUse(true);
}

void loop()
{
  // Read the current heading from the BNO055 sensor
  sensors_event_t event;
  bno.getEvent(&event);
  heading_current = event.orientation.x;

  error = heading_setpoint - heading_current;

  integral += error;
  derivative = error - last_error;
  output = Kp * error + Ki * integral + Kd * derivative;

  // Limit the output to the motor speed range
  output = constrain(output, -255, 255); /// Function to scale the motor output 

  motor.setSpeed(abs(output));

  if (output > 0)
  {
    motor.forward();
  }
  else if (output < 0)
  {
    motor.backward();
  }
  else
  {
    motor.stop();
  }

  last_error = error;

  // Print debugging information
  Serial.print("Setpoint: ");
  Serial.print(heading_setpoint);
  Serial.print(", Heading: ");
  Serial.print(heading_current);
  Serial.print(", Error: ");
  Serial.print(error);
  Serial.print(", Output: ");
  Serial.println(output);

  delay(100); // Adjust as needed based on your control loop frequency
}

Last update: June 29, 2024