Skip to content

Commit

Permalink
Add App class
Browse files Browse the repository at this point in the history 
Signed-off-by: Javier Balloffet <[email protected]>
  • Loading branch information
@jballoffet
jballoffet committed Nov 19, 2023
1 parent 8ce9a62 commit 1db6993
Show file tree
Hide file tree
Showing 3 changed files with 167 additions and 151 deletions.
272 changes: 127 additions & 145 deletions andino_firmware/src/app.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,25 +64,19 @@
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "app.h"

/* Serial port baud rate */
#define BAUDRATE 57600
/* Maximum PWM signal */
#define MAX_PWM 255

#include "Arduino.h"
#include "hw.h"

/* Include definition of serial commands */
#include "commands.h"

/* Motor driver function definitions */
#include "encoder.h"
#include "hw.h"
#include "motor.h"
#include "pid.h"

/* Encoder driver function definitions */
#include "encoder.h"
// TODO(jballoffet): Move this variables and constants to a different module.

/* PID parameters and functions */
#include "pid.h"
/* Serial port baud rate */
#define BAUDRATE 57600
/* Maximum PWM signal */
#define MAX_PWM 255

/* Run the PID loop at 30 times per second */
#define PID_RATE 30 // Hz
Expand All @@ -98,8 +92,6 @@ unsigned long nextPID = PID_INTERVAL;
#define AUTO_STOP_INTERVAL 3000
long lastMotorCommand = AUTO_STOP_INTERVAL;

/* Variable initialization */

// A pair of varibles to help parse serial commands
int arg = 0;
int index = 0;
Expand All @@ -118,22 +110,103 @@ char argv2[16];
long arg1;
long arg2;

// TODO(jballoffet): Make these objects local to the main function.
andino::Motor left_motor(LEFT_MOTOR_ENABLE_GPIO_PIN, LEFT_MOTOR_FORWARD_GPIO_PIN,
LEFT_MOTOR_BACKWARD_GPIO_PIN);
andino::Motor right_motor(RIGHT_MOTOR_ENABLE_GPIO_PIN, RIGHT_MOTOR_FORWARD_GPIO_PIN,
RIGHT_MOTOR_BACKWARD_GPIO_PIN);
namespace andino {

Motor App::left_motor_(LEFT_MOTOR_ENABLE_GPIO_PIN, LEFT_MOTOR_FORWARD_GPIO_PIN,
LEFT_MOTOR_BACKWARD_GPIO_PIN);
Motor App::right_motor_(RIGHT_MOTOR_ENABLE_GPIO_PIN, RIGHT_MOTOR_FORWARD_GPIO_PIN,
RIGHT_MOTOR_BACKWARD_GPIO_PIN);

Encoder App::left_encoder_(LEFT_ENCODER_A_GPIO_PIN, LEFT_ENCODER_B_GPIO_PIN);
Encoder App::right_encoder_(RIGHT_ENCODER_A_GPIO_PIN, RIGHT_ENCODER_B_GPIO_PIN);

PID App::left_pid_controller_(30, 10, 0, 10, -MAX_PWM, MAX_PWM);
PID App::right_pid_controller_(30, 10, 0, 10, -MAX_PWM, MAX_PWM);

void App::setup() {
// Required by Arduino libraries to work.
init();

Serial.begin(BAUDRATE);

left_encoder_.init();
right_encoder_.init();

// Enable motors.
left_motor_.set_state(true);
right_motor_.set_state(true);

left_pid_controller_.reset(left_encoder_.read());
right_pid_controller_.reset(right_encoder_.read());
}

void App::loop() {
while (Serial.available() > 0) {
// Read the next character
chr = Serial.read();

// Terminate a command with a CR
if (chr == 13) {
if (arg == 1)
argv1[index] = 0;
else if (arg == 2)
argv2[index] = 0;
run_command();
reset_command();
}
// Use spaces to delimit parts of the command
else if (chr == ' ') {
// Step through the arguments
if (arg == 0)
arg = 1;
else if (arg == 1) {
argv1[index] = 0;
arg = 2;
index = 0;
}
continue;
} else {
if (arg == 0) {
// The first arg is the single-letter command
cmd = chr;
} else if (arg == 1) {
// Subsequent arguments can be more than one character
argv1[index] = chr;
index++;
} else if (arg == 2) {
argv2[index] = chr;
index++;
}
}
}

// Run a PID calculation at the appropriate intervals
if (millis() > nextPID) {
int left_motor_speed = 0;
int right_motor_speed = 0;
left_pid_controller_.compute(left_encoder_.read(), left_motor_speed);
right_pid_controller_.compute(right_encoder_.read(), right_motor_speed);
left_motor_.set_speed(left_motor_speed);
right_motor_.set_speed(right_motor_speed);
nextPID += PID_INTERVAL;
}

// TODO(jballoffet): Make these objects local to the main function.
andino::PID left_pid_controller(30, 10, 0, 10, -MAX_PWM, MAX_PWM);
andino::PID right_pid_controller(30, 10, 0, 10, -MAX_PWM, MAX_PWM);
// Check to see if we have exceeded the auto-stop interval
if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {
lastMotorCommand = millis();
left_motor_.set_speed(0);
right_motor_.set_speed(0);
left_pid_controller_.enable(false);
right_pid_controller_.enable(false);
}

// TODO(jballoffet): Make these objects local to the main function.
andino::Encoder left_encoder(LEFT_ENCODER_A_GPIO_PIN, LEFT_ENCODER_B_GPIO_PIN);
andino::Encoder right_encoder(RIGHT_ENCODER_A_GPIO_PIN, RIGHT_ENCODER_B_GPIO_PIN);
// Required by Arduino libraries to work.
if (serialEventRun) {
serialEventRun();
}
}

/* Clear the current command parameters */
void resetCommand() {
void App::reset_command() {
cmd = 0;
memset(argv1, 0, sizeof(argv1));
memset(argv2, 0, sizeof(argv2));
Expand All @@ -143,8 +216,7 @@ void resetCommand() {
index = 0;
}

/* Run a command. Commands are defined in commands.h */
int runCommand() {
void App::run_command() {
int i = 0;
char* p = argv1;
char* str;
Expand Down Expand Up @@ -181,47 +253,47 @@ int runCommand() {
Serial.println("OK");
break;
case READ_ENCODERS:
Serial.print(left_encoder.read());
Serial.print(left_encoder_.read());
Serial.print(" ");
Serial.println(right_encoder.read());
Serial.println(right_encoder_.read());
break;
case RESET_ENCODERS:
left_encoder.reset();
right_encoder.reset();
left_pid_controller.reset(left_encoder.read());
right_pid_controller.reset(right_encoder.read());
left_encoder_.reset();
right_encoder_.reset();
left_pid_controller_.reset(left_encoder_.read());
right_pid_controller_.reset(right_encoder_.read());
Serial.println("OK");
break;
case MOTOR_SPEEDS:
/* Reset the auto stop timer */
lastMotorCommand = millis();
if (arg1 == 0 && arg2 == 0) {
left_motor.set_speed(0);
right_motor.set_speed(0);
left_pid_controller.reset(left_encoder.read());
right_pid_controller.reset(right_encoder.read());
left_pid_controller.enable(false);
right_pid_controller.enable(false);
left_motor_.set_speed(0);
right_motor_.set_speed(0);
left_pid_controller_.reset(left_encoder_.read());
right_pid_controller_.reset(right_encoder_.read());
left_pid_controller_.enable(false);
right_pid_controller_.enable(false);
} else {
left_pid_controller.enable(true);
right_pid_controller.enable(true);
left_pid_controller_.enable(true);
right_pid_controller_.enable(true);
}
// The target speeds are in ticks per second, so we need to convert them
// to ticks per PID_INTERVAL
left_pid_controller.set_setpoint(arg1 / PID_RATE);
right_pid_controller.set_setpoint(arg2 / PID_RATE);
left_pid_controller_.set_setpoint(arg1 / PID_RATE);
right_pid_controller_.set_setpoint(arg2 / PID_RATE);
Serial.println("OK");
break;
case MOTOR_RAW_PWM:
/* Reset the auto stop timer */
lastMotorCommand = millis();
left_pid_controller.reset(left_encoder.read());
right_pid_controller.reset(right_encoder.read());
left_pid_controller_.reset(left_encoder_.read());
right_pid_controller_.reset(right_encoder_.read());
// Sneaky way to temporarily disable the PID
left_pid_controller.enable(false);
right_pid_controller.enable(false);
left_motor.set_speed(arg1);
right_motor.set_speed(arg2);
left_pid_controller_.enable(false);
right_pid_controller_.enable(false);
left_motor_.set_speed(arg1);
right_motor_.set_speed(arg2);
Serial.println("OK");
break;
case UPDATE_PID:
Expand All @@ -230,8 +302,8 @@ int runCommand() {
pid_args[i] = atoi(str);
i++;
}
left_pid_controller.set_tunings(pid_args[0], pid_args[1], pid_args[2], pid_args[3]);
right_pid_controller.set_tunings(pid_args[0], pid_args[1], pid_args[2], pid_args[3]);
left_pid_controller_.set_tunings(pid_args[0], pid_args[1], pid_args[2], pid_args[3]);
right_pid_controller_.set_tunings(pid_args[0], pid_args[1], pid_args[2], pid_args[3]);
Serial.print("PID Updated: ");
Serial.print(pid_args[0]);
Serial.print(" ");
Expand All @@ -248,94 +320,4 @@ int runCommand() {
}
}

namespace andino {

/* Setup function--runs once at startup. */
void setup() {
// Required by Arduino libraries to work.
init();

Serial.begin(BAUDRATE);

left_encoder.init();
right_encoder.init();

// Enable motors.
left_motor.set_state(true);
right_motor.set_state(true);

left_pid_controller.reset(left_encoder.read());
right_pid_controller.reset(right_encoder.read());
}

/* Enter the main loop. Read and parse input from the serial port
and run any valid commands. Run a PID calculation at the target
interval and check for auto-stop conditions.
*/
void loop() {
while (Serial.available() > 0) {
// Read the next character
chr = Serial.read();

// Terminate a command with a CR
if (chr == 13) {
if (arg == 1)
argv1[index] = 0;
else if (arg == 2)
argv2[index] = 0;
runCommand();
resetCommand();
}
// Use spaces to delimit parts of the command
else if (chr == ' ') {
// Step through the arguments
if (arg == 0)
arg = 1;
else if (arg == 1) {
argv1[index] = 0;
arg = 2;
index = 0;
}
continue;
} else {
if (arg == 0) {
// The first arg is the single-letter command
cmd = chr;
} else if (arg == 1) {
// Subsequent arguments can be more than one character
argv1[index] = chr;
index++;
} else if (arg == 2) {
argv2[index] = chr;
index++;
}
}
}

// Run a PID calculation at the appropriate intervals
if (millis() > nextPID) {
int left_motor_speed = 0;
int right_motor_speed = 0;
left_pid_controller.compute(left_encoder.read(), left_motor_speed);
right_pid_controller.compute(right_encoder.read(), right_motor_speed);
left_motor.set_speed(left_motor_speed);
right_motor.set_speed(right_motor_speed);
nextPID += PID_INTERVAL;
}

// Check to see if we have exceeded the auto-stop interval
if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {
lastMotorCommand = millis();
left_motor.set_speed(0);
right_motor.set_speed(0);
left_pid_controller.enable(false);
right_pid_controller.enable(false);
}

// Required by Arduino libraries to work.
if (serialEventRun) {
serialEventRun();
}
}

} // namespace andino
40 changes: 37 additions & 3 deletions andino_firmware/src/app.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,10 +29,44 @@
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#pragma once

#include "encoder.h"
#include "motor.h"
#include "pid.h"

namespace andino {

// TODO(jballoffet): Turn this into a class.
void setup();
void loop();
/// @brief This class wraps the MCU main application.
class App {
public:
/// This class only contains static members.
App() = delete;

/// @brief Configures and sets the application up. Meant to be called once at startup.
static void setup();

/// @brief Application main run loop. Meant to be called continously.
static void loop();

private:
/// Clears the current command parameters.
// TODO(jballoffet): Move this method to a different module.
static void reset_command();

/// Runs a command.
// TODO(jballoffet): Move this method to a different module.
static void run_command();

/// Motors (one per wheel).
static Motor left_motor_;
static Motor right_motor_;

/// Encoders (one per wheel).
static Encoder left_encoder_;
static Encoder right_encoder_;

/// PID controllers (one per wheel).
static PID left_pid_controller_;
static PID right_pid_controller_;
};

} // namespace andino
Loading

0 comments on commit 1db6993

Please sign in to comment.