File content as of revision 3:2f76ffbc5cef:

//BroBot V2
//Author: Carter Sharer 
//Date: 10/11/2016

#include "pin_assignments.h"
#include "I2Cdev.h"
#include "JJ_MPU6050_DMP_6Axis.h"
#include "BroBot.h"
#include "BroBot_IMU.h"
#include "stepper_motors.h"

//PID
#define MAX_THROTTLE 580
#define MAX_STEERING 150
#define MAX_TARGET_ANGLE 12
#define KP 0.19
#define KD 28
#define KP_THROTTLE 0.01 //0.07    
#define KI_THROTTLE 0//0.04
#define ITERM_MAX_ERROR 25   // Iterm windup constants for PI control //40
#define ITERM_MAX 8000       // 5000

//PID Default control values from constant definitions
float Kp = KP;
float Kd = KD;
float Kp_thr = KP_THROTTLE;
float Ki_thr = KI_THROTTLE;
float Kp_user = KP;
float Kd_user = KD;
float Kp_thr_user = KP_THROTTLE;
float Ki_thr_user = KI_THROTTLE;
bool newControlParameters = false;
bool modifing_control_parameters = false;
float PID_errorSum;
float PID_errorOld = 0;
float PID_errorOld2 = 0;
float setPointOld = 0;
float target_angle;
float throttle = 0;
float steering = 0;
float max_throttle = MAX_THROTTLE;
float max_steering = MAX_STEERING;
float max_target_angle = MAX_TARGET_ANGLE;
float control_output;
int16_t actual_robot_speed;        // overall robot speed (measured from steppers speed)
int16_t actual_robot_speed_old;
float estimated_speed_filtered;    // Estimated robot speed

Timer timer;
int timer_value; //maybe make this a long
int timer_old; //maybe make this a long
float dt;

uint8_t slow_loop_counter;
uint8_t loop_counter;

Serial pc(USBTX, USBRX);

// =============================================================================
// ===      PD controller implementation(Proportional, derivative)           ===
// =============================================================================
// PD controller implementation(Proportional, derivative). DT is in miliseconds
//    stabilityPDControl(      dt,       angle,   target_angle,        Kp,       Kd);
float stabilityPDControl(float DT, float input, float setPoint,  float Kp, float Kd)
{
    float error;
    float output;

    error = setPoint - input;

    // Kd is implemented in two parts
    //    The biggest one using only the input (sensor) part not the SetPoint input-input(t-2)
    //    And the second using the setpoint to make it a bit more agressive   setPoint-setPoint(t-1)
    output = Kp * error + (Kd * (setPoint - setPointOld) - Kd * (input - PID_errorOld2)) / DT;
    //Serial.print(Kd*(error-PID_errorOld));Serial.print("\t");
    //PID_errorOld2 = PID_errorOld;
    //PID_errorOld = input;  // error for Kd is only the input component
    //setPointOld = setPoint;
    return output;
}

// PI controller implementation (Proportional, integral). DT is in miliseconds
float speedPIControl(float DT, float input, float setPoint,  float Kp, float Ki)
{
    float error;
    float output;

    error = setPoint - input;
    PID_errorSum += CAP(error, ITERM_MAX_ERROR);
    PID_errorSum = CAP(PID_errorSum, ITERM_MAX);

    //Serial.println(PID_errorSum);

    output = Kp * error + Ki * PID_errorSum * DT * 0.001; // DT is in miliseconds...
    return (output);
}
// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================
void init_imu()
{
    pc.printf("\r\r\n\n Start \r\n");

    // Manual MPU initialization... accel=2G, gyro=2000º/s, filter=20Hz BW, output=200Hz
    mpu.setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
    mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
    mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
    mpu.setDLPFMode(MPU6050_DLPF_BW_10);  //10,20,42,98,188  // Default factor for BROBOT:10
    mpu.setRate(4);   // 0=1khz 1=500hz, 2=333hz, 3=250hz 4=200hz
    mpu.setSleepEnabled(false);
    wait_ms(500);

    // load and configure the DMP
    devStatus = mpu.dmpInitialize();
    if(devStatus == 0) {
        mpu.setDMPEnabled(true);
        mpuIntStatus = mpu.getIntStatus();
        dmpReady = true;
    } else { 
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        pc.printf("DMP INIT error \r\n");
    }

    //Gyro Calibration
    wait_ms(500);
    pc.printf("Gyro calibration!!  Dont move the robot in 10 seconds... \r\n");
    wait_ms(500);
    
    // verify connection
    pc.printf(mpu.testConnection() ? "Connection Good \r\n" : "Connection Failed\r\n");

    //Adjust Sensor Fusion Gain
    dmpSetSensorFusionAccelGain(0x20);

    wait_ms(200);
    mpu.resetFIFO();
}

// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================
int main()
{
    pc.baud(115200);
    pc.printf("Start\r\n");
    init_imu();
    timer.start();
    //timer
    timer_value = timer.read_ms();

    //Init Stepper Motors
    //Attach Timer Interupts (Tiker)
    timer_M1.attach_us(&ISR1, ZERO_SPEED);
    timer_M2.attach_us(&ISR2, ZERO_SPEED);
    step_M1 = 1;
    dir_M1 = 1;
    enable = ENABLE; //Enable Motors

    //Set Gains
    Kp_thr = 0; //0.15;
    Ki_thr = 0; //0.15;
    
    while(1) {
        // New DMP Orientation solution?
        fifoCount = mpu.getFIFOCount();
        if (fifoCount >= 18) {
            if (fifoCount > 18) { // If we have more than one packet we take the easy path: discard the buffer and wait for the next one
                pc.printf("FIFO RESET!!\r\n");
                mpu.resetFIFO();
                return;
            }
            loop_counter++;
            slow_loop_counter++;
            dt = (timer_value - timer_old);
            timer_old = timer_value;

            angle_old = angle;
            // Get new orientation angle from IMU (MPU6050)
            angle = dmpGetPhi();

            mpu.resetFIFO();  // We always reset FIFO
        } // End of new IMU data

        if(loop_counter >= 5) {
            loop_counter = 0;
            int16_t offset = 
            pc.printf("angle: %d \r\n", int16_t(angle-ANGLE_OFFSET));
            setMotor1Speed(int16_t(angle-ANGLE_OFFSET));
            setMotor2Speed(int16_t(angle-ANGLE_OFFSET));
        }
        if (slow_loop_counter >= 99) { // 2Hz
            slow_loop_counter = 0; // Read  status
        }  // End of slow loop


        /*
        //Set Gains
        Kp = 0.02;
        Kd = 0.01;

        timer_value = timer.read_ms();

        // if programming failed, don't try to do anything
        if (!dmpReady) return;

        // reset interrupt flag and get INT_STATUS byte
        mpuInterrupt = false;
        mpuIntStatus = mpu.getIntStatus();

        // get current FIFO count
        fifoCount = mpu.getFIFOCount();

        // check for overflow (this should never happen unless our code is too inefficient)
        if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
            // reset so we can continue cleanly
            mpu.resetFIFO();
            // otherwise, check for DMP data ready interrupt (this should happen frequently)
        } else if (mpuIntStatus & 0x02) {
            // wait for correct available data length, should be a VERY short wait
            //CS while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

            // read a packet from FIFO
            mpu.getFIFOBytes(fifoBuffer, packetSize);

            angle_old = angle; //Update old angle before reading new angle

            // track FIFO count here in case there is > 1 packet available
            // (this lets us immediately read more without waiting for an interrupt)
            fifoCount -= packetSize;
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            angle = atan2(2 * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z) * RAD2GRAD;
            angle = angle - ANGLE_OFFSET;
            //pc.printf("angle: %f \r\n", angle);


            //Update timer
            dt = (timer_value - timer_old);
            timer_old = timer_value;

            //PID CONTROL MAGIC GOES HERE
            // We calculate the estimated robot speed:
            // Estimated_Speed = angular_velocity_of_stepper_motors(combined) - angular_velocity_of_robot(angle measured by IMU)
            //CS actual_robot_speed_old = actual_robot_speed;
            //CS actual_robot_speed = (speed_M1 + speed_M2) / 2; // Positive: forward
            //CS int16_t angular_velocity = (angle - angle_old) * 90.0; // 90 is an empirical extracted factor to adjust for real units
            //CS int16_t estimated_speed = -actual_robot_speed_old - angular_velocity;     // We use robot_speed(t-1) or (t-2) to compensate the delay
            //CS estimated_speed_filtered = estimated_speed_filtered * 0.95 + (float)estimated_speed * 0.05;  // low pass filter on estimated speed
            // SPEED CONTROL: This is a PI controller.
            //    input:user throttle, variable: estimated robot speed, output: target robot angle to get the desired speed
            //CS target_angle = speedPIControl(dt, estimated_speed_filtered, throttle, Kp_thr, Ki_thr);
            //CD target_angle = constrain(target_angle, -max_target_angle, max_target_angle); // limited output
            // Stability control: This is a PD controller.
            //    input: robot target angle(from SPEED CONTROL), variable: robot angle, output: Motor speed
            //    We integrate the output (sumatory), so the output is really the motor acceleration, not motor speed.

            //pc.printf("dt: %f, angle: %f, target_angle: %f, Kp: %f, Kd: %f \r\n", dt, angle, target_angle, Kp, Kd);
            control_output += stabilityPDControl(dt, angle, target_angle, Kp, Kd);
            control_output = constrain(control_output, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT); // Limit max output from control
            motor1 = control_output + steering;
            motor2 = control_output - steering;


            //TEST P CONTROL
            float gain = 1;
            motor1 = angle * gain;
            motor2 = angle * gain;
            pc.printf("motor: %d control output: %f \r\n", motor1, control_output);

            // Limit max speed (control output)
            motor1 = constrain(motor1, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT);
            motor2 = constrain(motor2, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT);


        }


        // Put all the pid loop stuff here
        if((angle < 45) && (angle > -45)) {
            //Enable Motors
            if(motor1 == 0) {
                enable = DISABLE;
                setMotor1Speed(0);
                setMotor2Speed(0);
            } else {
                enable = ENABLE;
                setMotor1Speed(motor1);
                setMotor2Speed(motor2);
            }
            //pc.printf("m1: %d m2: %d angle: %0.1f, controlout: %f tAngle: %f dt: %f timer: %d \r\n", motor1, motor2, angle, control_output, target_angle, dt, timer_value);
        } else {
            //Disable Motors
            enable = DISABLE;
            //Set Motor Speed 0
            PID_errorSum = 0;  // Reset PID I term
            Kp = 0;
            Kd = 0;
            Kp_thr = 0;
            Ki_thr = 0;
        }

        *////////////
    } //end main loop
} //End Main()