Skip to content

Commit

Permalink
Add command methods
Browse files Browse the repository at this point in the history 
Signed-off-by: Javier Balloffet <[email protected]>
  • Loading branch information
@jballoffet
jballoffet committed Dec 3, 2023
1 parent 1ae30c1 commit 27fdd85
Show file tree
Hide file tree
Showing 3 changed files with 172 additions and 114 deletions.
216 changes: 120 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,142 @@ 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* arg1, const char* arg2) {
(void)arg1;
(void)arg2;
Serial.println("Unknown command.");
}

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

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

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

void App::cmd_reset_encoders(const char* arg1, const char* arg2) {
(void)arg1;
(void)arg2;
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) {
int left_motor_speed = atoi(arg1);
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) {
int left_motor_pwm = atoi(arg1);
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* arg2) {
(void)arg2;

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 27fdd85

Please sign in to comment.