Skip to content

Commit

Permalink
Add command methods (#187)
Browse files Browse the repository at this point in the history 
* Add command methods

Signed-off-by: Javier Balloffet <javier.balloffet@gmail.com>

* Address comments

Signed-off-by: Javier Balloffet <javier.balloffet@gmail.com>

---------

Signed-off-by: Javier Balloffet <javier.balloffet@gmail.com>
  • Loading branch information
@jballoffet
jballoffet committed Dec 26, 2023
1 parent ec31147 commit a46a3b0
Show file tree
Hide file tree
Showing 3 changed files with 160 additions and 114 deletions.
204 changes: 108 additions & 96 deletions andino_firmware/src/app.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,10 +93,6 @@ char cmd;
char argv1[16];
char argv2[16];

// The arguments converted to integers
long arg1;
long arg2;

namespace andino {

Motor App::left_motor_(Hw::kLeftMotorEnableGpioPin, Hw::kLeftMotorForwardGpioPin,
Expand Down Expand Up @@ -199,114 +195,130 @@ void App::reset_command() {
cmd = 0;
memset(argv1, 0, sizeof(argv1));
memset(argv2, 0, sizeof(argv2));
arg1 = 0;
arg2 = 0;
arg = 0;
index = 0;
}

void App::run_command() {
int i = 0;
char* p = argv1;
char* str;
int pid_args[4];
arg1 = atoi(argv1);
arg2 = atoi(argv2);

switch (cmd) {
case GET_BAUDRATE:
Serial.println(Constants::kBaudrate);
break;
case ANALOG_READ:
Serial.println(analogRead(arg1));
case Commands::kReadAnalogGpio:
cmd_read_analog_gpio(argv1, argv2);
break;
case DIGITAL_READ:
Serial.println(digitalRead(arg1));
case Commands::kReadDigitalGpio:
cmd_read_digital_gpio(argv1, argv2);
break;
case ANALOG_WRITE:
analogWrite(arg1, arg2);
Serial.println("OK");
case Commands::kReadEncoders:
cmd_read_encoders(argv1, argv2);
break;
case DIGITAL_WRITE:
if (arg2 == 0)
digitalWrite(arg1, LOW);
else if (arg2 == 1)
digitalWrite(arg1, HIGH);
Serial.println("OK");
case Commands::kResetEncoders:
cmd_reset_encoders(argv1, argv2);
break;
case PIN_MODE:
if (arg2 == 0)
pinMode(arg1, INPUT);
else if (arg2 == 1)
pinMode(arg1, OUTPUT);
Serial.println("OK");
case Commands::kSetMotorsSpeed:
cmd_set_motors_speed(argv1, argv2);
break;
case READ_ENCODERS:
Serial.print(left_encoder_.read());
Serial.print(" ");
Serial.println(right_encoder_.read());
case Commands::kSetMotorsPwm:
cmd_set_motors_pwm(argv1, argv2);
break;
case RESET_ENCODERS:
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);
} else {
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 / Constants::kPidRate);
right_pid_controller_.set_setpoint(arg2 / Constants::kPidRate);
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());
// 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);
Serial.println("OK");
break;
case UPDATE_PID:
/* Example: "u 30:20:10:50" */
while ((str = strtok_r(p, ":", &p)) != NULL) {
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]);
Serial.print("PID Updated: ");
Serial.print(pid_args[0]);
Serial.print(" ");
Serial.print(pid_args[1]);
Serial.print(" ");
Serial.print(pid_args[2]);
Serial.print(" ");
Serial.println(pid_args[3]);
Serial.println("OK");
case Commands::kSetPidsTuningGains:
cmd_set_pid_tuning_gains(argv1, argv2);
break;
default:
Serial.println("Invalid Command");
cmd_unknown(argv1, argv2);
break;
}
}

void App::cmd_unknown(const char*, const char*) { Serial.println("Unknown command."); }

void App::cmd_read_analog_gpio(const char* arg1, const char*) {
const int pin = atoi(arg1);
Serial.println(analogRead(pin));
}

void App::cmd_read_digital_gpio(const char* arg1, const char*) {
const int pin = atoi(arg1);
Serial.println(digitalRead(pin));
}

void App::cmd_read_encoders(const char*, const char*) {
Serial.print(left_encoder_.read());
Serial.print(" ");
Serial.println(right_encoder_.read());
}

void App::cmd_reset_encoders(const char*, const char*) {
left_encoder_.reset();
right_encoder_.reset();
left_pid_controller_.reset(left_encoder_.read());
right_pid_controller_.reset(right_encoder_.read());
Serial.println("OK");
}

void App::cmd_set_motors_speed(const char* arg1, const char* arg2) {
const int left_motor_speed = atoi(arg1);
const int right_motor_speed = atoi(arg2);

// Reset the auto stop timer.
lastMotorCommand = millis();
if (left_motor_speed == 0 && right_motor_speed == 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);
} else {
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
// Constants::kPidRate.
left_pid_controller_.set_setpoint(left_motor_speed / Constants::kPidRate);
right_pid_controller_.set_setpoint(right_motor_speed / Constants::kPidRate);
Serial.println("OK");
}

void App::cmd_set_motors_pwm(const char* arg1, const char* arg2) {
const int left_motor_pwm = atoi(arg1);
const int right_motor_pwm = atoi(arg2);

// Reset the auto stop timer.
lastMotorCommand = millis();
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(left_motor_pwm);
right_motor_.set_speed(right_motor_pwm);
Serial.println("OK");
}

void App::cmd_set_pid_tuning_gains(const char* arg1, const char*) {
int i = 0;
char arg[20];
char* str;
int pid_args[4];

// Example: "u 30:20:10:50".
strcpy(arg, arg1);
char* p = arg;
while ((str = strtok_r(p, ":", &p)) != NULL) {
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]);
Serial.print("PID Updated: ");
Serial.print(pid_args[0]);
Serial.print(" ");
Serial.print(pid_args[1]);
Serial.print(" ");
Serial.print(pid_args[2]);
Serial.print(" ");
Serial.println(pid_args[3]);
Serial.println("OK");
}

} // namespace andino
32 changes: 32 additions & 0 deletions andino_firmware/src/app.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,38 @@ class App {
// TODO(jballoffet): Move this method to a different module.
static void run_command();

/// Callback method for an unknown command (default).
// TODO(jballoffet): Parse arguments within callback method.
static void cmd_unknown(const char* arg1, const char* arg2);

/// Callback method for the `Commands::kReadAnalogGpio` command.
// TODO(jballoffet): Parse arguments within callback method.
static void cmd_read_analog_gpio(const char* arg1, const char* arg2);

/// Callback method for the `Commands::kReadDigitalGpio` command.
// TODO(jballoffet): Parse arguments within callback method.
static void cmd_read_digital_gpio(const char* arg1, const char* arg2);

/// Callback method for the `Commands::kReadEncoders` command.
// TODO(jballoffet): Parse arguments within callback method.
static void cmd_read_encoders(const char* arg1, const char* arg2);

/// Callback method for the `Commands::kResetEncoders` command.
// TODO(jballoffet): Parse arguments within callback method.
static void cmd_reset_encoders(const char* arg1, const char* arg2);

/// Callback method for the `Commands::kSetMotorsSpeed` command.
// TODO(jballoffet): Parse arguments within callback method.
static void cmd_set_motors_speed(const char* arg1, const char* arg2);

/// Callback method for the `Commands::kSetMotorsPwm` command.
// TODO(jballoffet): Parse arguments within callback method.
static void cmd_set_motors_pwm(const char* arg1, const char* arg2);

/// Callback method for the `Commands::kSetPidsTuningGains` command.
// TODO(jballoffet): Parse arguments within callback method.
static void cmd_set_pid_tuning_gains(const char* arg1, const char* arg2);

/// Motors (one per wheel).
static Motor left_motor_;
static Motor right_motor_;
Expand Down
38 changes: 20 additions & 18 deletions andino_firmware/src/commands.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,24 +62,26 @@
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#pragma once

/* Define single-letter commands that will be sent by the PC over the
serial link.
*/
namespace andino {

#ifndef COMMANDS_H
#define COMMANDS_H
/// @brief CLI commands.
struct Commands {
/// @brief Reads an analog GPIO.
static constexpr char kReadAnalogGpio{'a'};
/// @brief Reads a digital GPIO.
static constexpr char kReadDigitalGpio{'d'};
/// @brief Reads the encoders tick count values.
static constexpr char kReadEncoders{'e'};
/// @brief Sets the encoders ticks count to zero.
static constexpr char kResetEncoders{'r'};
/// @brief Sets the motors speed [ticks/s].
static constexpr char kSetMotorsSpeed{'m'};
/// @brief Sets the motors PWM value [duty range: 0-255].
static constexpr char kSetMotorsPwm{'o'};
/// @brief Sets the PIDs tuning gains [format: "kp:kd:ki:ko"].
static constexpr char kSetPidsTuningGains{'u'};
};

#define ANALOG_READ 'a'
#define GET_BAUDRATE 'b'
#define PIN_MODE 'c'
#define DIGITAL_READ 'd'
#define READ_ENCODERS 'e'
#define MOTOR_SPEEDS 'm'
#define MOTOR_RAW_PWM 'o'
#define RESET_ENCODERS 'r'
#define UPDATE_PID 'u'
#define DIGITAL_WRITE 'w'
#define ANALOG_WRITE 'x'

#endif
} // namespace andino

0 comments on commit a46a3b0

Please sign in to comment.