self_balance.ino

#include <Arduino.h>
#include <MKRIMU.h>
#include <MKRMotorCarrier.h>
#define INTERRUPT_PIN 6

// Rx inputs
#define CH1_PIN 15 // Steering
#define CH2_PIN 16
#define CH3_PIN 17 // Throttle
#define CH4_PIN 20
#define CH5_PIN 20
#define CH6_PIN 21

// Raw Rx input values
float ch1, ch2, ch3, ch4, ch5, ch6;

#define STEERING_DEADBAND 5
#define DEADBAND_THRESHOLD 5
#define PIVOT_THRESHOLD 32

float elapsed_time, time, previous_time;
float pid_output, error, previous_error;
float pid_p = 20;
float pid_i = 20;
float pid_d = 0;

// PID Constants
float kp = 25;
float ki = 0;
float kd = 0.8;

float desired_angle = 0.0;

void setup()
{
    // Serial port initialization
    Serial.begin(115200);

    // Set up the motor carrier shield
    if (controller.begin())
    {
        Serial.print("MKR Motor Shield connected, firmware version ");
        Serial.println(controller.getFWVersion());
    }
    else
    {
        Serial.println("MKRMotorCarrier: Couldn't connect! Is the red led blinking? You may need to update the firmware with FWUpdater sketch");
        while (1)
            ;
    }

    // Reboot the motor controller; brings every value back to default
    controller.reboot();
    delay(500);

    // Turn off all motors
    M1.setDuty(0);
    M2.setDuty(0);
    M3.setDuty(0);
    M4.setDuty(0);

    // Set up the IMU shield
    if (!IMU.begin())
    {
        Serial.println("MKRIMU: Failed to initialize IMU!");
        while (1)
            ;
    }

    time = millis();
}

void loop()
{
    previous_time = time;
    time = millis();
    elapsed_time = (time - previous_time) / 1000;

    // Read the values frmo the receiver
    ch1 = pulseIn(CH1_PIN, HIGH, 25000);
    ch2 = pulseIn(CH2_PIN, HIGH, 25000);
    ch3 = pulseIn(CH3_PIN, HIGH, 25000);
    ch4 = pulseIn(CH4_PIN, HIGH, 25000);
    ch5 = pulseIn(CH5_PIN, HIGH, 25000);
    ch6 = pulseIn(CH6_PIN, HIGH, 25000);

    // Normalize throttle and steering
    int throttle = map(ch1, 900, 2100, 100, -100);
    int steering = map(ch3, 990, 1990, 100, -100);

    // Steering deadband
    if (steering < STEERING_DEADBAND && steering > -STEERING_DEADBAND)
    {
        steering = 0;
    }

    /*
     * ADAPTED FROM:
     * 
     * Calvin Hass
     * https://www.impulseadventure.com/elec/
     **/

    // Mix throttle and steering
    int dutyCycleLeft;
    int dutyCycleRight;

    float dutyCycleLeftUnmixed;
    float dutyCycleRightUnmixed;
    int pivotSpeed;
    float fPivScale;

    // Calculate Drive Turn output due to Joystick X input
    if (throttle >= 0)
    {
        // Forward
        dutyCycleLeftUnmixed = (steering >= 0) ? 100.0 : (100.0 + steering);
        dutyCycleRightUnmixed = (steering >= 0) ? (100.0 - steering) : 100.0;
    }
    else
    {
        // Reverse
        dutyCycleLeftUnmixed = (steering >= 0) ? (100.0 - steering) : 100.0;
        dutyCycleRightUnmixed = (steering >= 0) ? 100.0 : (100.0 + steering);
    }

    // Scale Drive output due to Joystick Y input (throttle)
    dutyCycleLeftUnmixed = dutyCycleLeftUnmixed * throttle / 100.0;
    dutyCycleRightUnmixed = dutyCycleRightUnmixed * throttle / 100.0;

    // Now calculate pivot amount
    // - Strength of pivot (pivotSpeed) based on Joystick X input
    // - Blending of pivot vs drive (fPivScale) based on Joystick Y input
    pivotSpeed = steering;
    fPivScale = (abs(throttle) > PIVOT_THRESHOLD) ? 0.0 : (1.0 - abs(throttle) / PIVOT_THRESHOLD);

    // Calculate final mix of Drive and Pivot
    dutyCycleLeft = (1.0 - fPivScale) * dutyCycleLeftUnmixed + fPivScale * (pivotSpeed);
    dutyCycleRight = (1.0 - fPivScale) * -dutyCycleRightUnmixed + fPivScale * (pivotSpeed);

    // Check if we're in the deadband and turn off the motor
    if (dutyCycleLeft > -DEADBAND_THRESHOLD && dutyCycleLeft < DEADBAND_THRESHOLD)
    {
        dutyCycleLeft = 0;
    }

    if (dutyCycleRight > -DEADBAND_THRESHOLD && dutyCycleRight < DEADBAND_THRESHOLD)
    {
        dutyCycleRight = 0;
    }

    // Motor output before PID
    dutyCycleLeft = min(100, max(-100, dutyCycleLeft));
    dutyCycleRight = min(100, max(-100, dutyCycleRight));

    // PID
    float heading, roll, pitch;
    float x, y, z;

    if (IMU.eulerAnglesAvailable() && IMU.accelerationAvailable())
    {
        IMU.readEulerAngles(heading, roll, pitch);
        IMU.readAcceleration(x, y, z);

        // Complimentary filter
        pitch = pitch * 0.98 + z * 02;

        // Calculate error
        error = pitch - desired_angle;

        pid_p = kp * error;
        pid_i = pid_i + (ki * error);
        pid_d = kd * ((error - previous_error) / elapsed_time);
        pid_output = pid_p + pid_d;
        previous_error = error;

        // TODO: NEED HELP: how to mix the PID_OUTPUT with the duty cycle left and right

        // -100 to 100
        M1.setDuty(dutyCycleLeft);
        M2.setDuty(dutyCycleRight);
    }
}