motor_control.cpp

#include "motor_control.h"
#include <wiringPi.h>
#include <softPwm.h>
#include <stdexcept>
#include <string>

// Define the PWM range for softPwm
static const int PWM_RANGE = 100;

void init_gpio() {
    if (wiringPiSetupGpio() == -1) {
        throw std::runtime_error("Failed to initialize GPIO using wiringPi.");
    }
    // Pin initialization is handled in the MotorDriver constructor.
}

void cleanup_gpio() {
    // You may choose to set all used pins to LOW here if needed.
}

// MotorDriver implementation
MotorDriver::MotorDriver(int in1, int in2, int en)
    : in1_pin(in1), in2_pin(in2), en_pin(en)
{
    pinMode(in1_pin, OUTPUT);
    pinMode(in2_pin, OUTPUT);
    pinMode(en_pin, OUTPUT);
    digitalWrite(in1_pin, LOW);
    digitalWrite(in2_pin, LOW);
    // Initialize software PWM on the enable pin.
    softPwmCreate(en_pin, 0, PWM_RANGE);
}

void MotorDriver::set_speed(int speed) {
    if (speed > 0) {
        digitalWrite(in1_pin, HIGH);
        digitalWrite(in2_pin, LOW);
        softPwmWrite(en_pin, speed);
    } else if (speed < 0) {
        digitalWrite(in1_pin, LOW);
        digitalWrite(in2_pin, HIGH);
        softPwmWrite(en_pin, -speed);
    } else {
        digitalWrite(in1_pin, LOW);
        digitalWrite(in2_pin, LOW);
        softPwmWrite(en_pin, 0);
    }
}

void MotorDriver::stop() {
    set_speed(0);
}

void mecanum_drive(MotorDriver &lf, MotorDriver &lr, MotorDriver &rf, MotorDriver &rr, const std::string &command, int speed) {
    if (command == "forward") {
        lf.set_speed(speed);
        lr.set_speed(speed);
        rf.set_speed(speed);
        rr.set_speed(speed);
    } else if (command == "backward") {
        lf.set_speed(-speed);
        lr.set_speed(-speed);
        rf.set_speed(-speed);
        rr.set_speed(-speed);
    } else if (command == "rotate_left") {
        lf.set_speed(-speed);
        lr.set_speed(-speed);
        rf.set_speed(speed);
        rr.set_speed(speed);
    } else if (command == "rotate_right") {
        lf.set_speed(speed);
        lr.set_speed(speed);
        rf.set_speed(-speed);
        rr.set_speed(-speed);
    } else if (command == "stop") {
        lf.stop();
        lr.stop();
        rf.stop();
        rr.stop();
    } else {
        lf.stop();
        lr.stop();
        rf.stop();
        rr.stop();
    }
}