File content as of revision 4:2512939c10f0:

//BroBot V3
//Author: Carter Sharer
//Date: 10/13/2016

//BroBot Begin
#include "pin_assignments.h"
#include "I2Cdev.h"
#include "JJ_MPU6050_DMP_6Axis.h"
#include "BroBot.h"
#include "BroBot_IMU.h"
#include "stepper_motors.h"
#include "MRF24J40.h"

//For RF Communication
#define JSTICK_H 8
#define JSTICK_V 9
#define SPACE 10
#define KNOB1 11
#define KNOB2 12
#define KNOB3 13
#define KNOB4 14
#define ANGLE 15
#define BUTTON 16
#define JSTICK_OFFSET 100
#define TX_BUFFER_LEN 18
#define TX_ANGLE_OFFSET 100
//Knobs
#define POT1 p17
#define POT2 p18
#define POT3 p16
#define POT4 p15
//JoyStick
#define POTV p19
#define POTH p20

//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

//MRF24J40
PinName mosi(SDI); //SDI
PinName miso(SDO); //SDO
PinName sck(SCK);  //SCK
PinName cs(CS); //CS
PinName reset(RESET); //RESET
// RF tranceiver to link with handheld.
MRF24J40 mrf(mosi, miso, sck, cs, reset);
uint8_t txBuffer[128]= {1, 8, 0, 0xA1, 0xB2, 0xC3, 0xD4, 0x00};
uint8_t rxBuffer[128];
uint8_t rxLen;

//Controller Values
uint8_t knob1, knob2, knob3, knob4;
int8_t jstick_h, jstick_v;


//PID Default control values from constant definitions
float Kp = KP;
float Kd = KD;
float Kp_thr = KP_THROTTLE;
float Ki_thr = KI_THROTTLE;
float Kd_thr; //Added for CS Pos contorl
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
int robot_pos = 0;

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

uint8_t slow_loop_counter;
uint8_t medium_loop_counter;
uint8_t loop_counter;


Serial pc(USBTX, USBRX);

// LEDs
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

//Button
bool button;

// =============================================================================
// ===      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;

    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]default
    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                     ===
// ================================================================
//CS PID CONTROLLER TEST
float target_angle_old = 0;
float change_in_target_angle = 0;
float change_in_angle = 0;
float angle_old1 = 0;
float angle_old2 = 0;
float kp_term = 0;
float kd_term = 0;
float error;
//For Position controller
float pos_error = 0;
float kp_pos_term = 0;
float kd_pos_term = 0;
float change_in_target_pos;
float target_pos, target_pos_old;
float change_in_pos;
float robot_pos_old, robot_pos_old1, robot_pos_old2;

int main()
{
    pc.baud(230400);
    pc.printf("Start\r\n");
    init_imu();
    timer.start();
    //timer
    timer_value = timer.read_us();

    //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;

    //Attach Interupt for IMU
    checkpin.rise(&dmpDataReady);

    //Used to set angle upon startup, filter
    bool FILTER_DISABLE = true;

    while(1) {

        if(button) {
            pos_M1 = 0;
            pos_M2 = 0;
            target_pos = 0;
        }

        while(!mpuInterrupt) { // && fifoCount < packetSize) {
            //led4 = led4^1;
            //pc.printf("In while comp loop \r\n");
            timer_value = timer.read_us();

            //Set Gainz with knobs
            Kp = ((float)knob1) / 1000.0;
            Kd = ((float)knob2) / 1.0;
            Kp_thr = ((float)knob3) / 1000.0;
            Kd_thr = ((float)knob4) / 100.0;

            //Joystick control
            throttle = (float)jstick_v  /10.0;
            steering = (float)jstick_h / 10.0;

            //Update Values
            loop_counter++;
            slow_loop_counter++;
            medium_loop_counter++;
            dt = (timer_value - timer_old);
            timer_old = timer_value;
            angle_old = angle;

            // Motor contorl
            if((angle < 45) && (angle > -45)) {

                //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)
                actual_robot_speed_old = actual_robot_speed;
                actual_robot_speed = (speed_M1 + speed_M2) / 2; // Positive: forward
                int16_t angular_velocity = (angle - angle_old) * 90.0; // 90 is an empirical extracted factor to adjust for real units
                int16_t estimated_speed = -actual_robot_speed_old - angular_velocity;     // We use robot_speed(t-1) or (t-2) to compensate the delay
                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);
                //CS target_angle = CAP(target_angle, max_target_angle); // limited output
                //target_angle = 0;
                // 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);
                
                //CS Pd Target Angle Contoller Goes Here
                target_pos += throttle;
                robot_pos = (pos_M1 + pos_M2) / 2;
                //KP Term
                pos_error = robot_pos - target_pos; //robot_pos - target_pos;
                kp_pos_term = -Kp_thr * pos_error;
                
                //KD Term
                change_in_target_pos = target_pos - target_pos_old;
                change_in_pos = robot_pos - robot_pos_old2;
                kd_pos_term = ((-Kd_thr * change_in_target_pos) - (-Kd_thr*change_in_pos)) /dt;
                target_angle = kp_pos_term + kd_pos_term;
                target_angle = CAP(target_angle, MAX_TARGET_ANGLE);
                
                //Update values
                target_pos_old = target_pos;
                robot_pos_old2 = robot_pos_old1;
                robot_pos_old1 = robot_pos_old;
                
                //CS PD Stability CONTROLLER HERE
                error = target_angle - angle;
                kp_term = Kp * error;

                change_in_target_angle = target_angle - target_angle_old; //add
                change_in_angle = angle - angle_old2; //add
                kd_term = ((Kd * change_in_target_angle) - Kd*(change_in_angle)) / dt;

                //Control Output
                control_output += kp_term + kd_term;
                control_output = CAP(control_output, MAX_CONTROL_OUTPUT); // Limit max output from control
                motor1 = (int16_t)(control_output + (steering/4));
                motor2 = (int16_t)(control_output - (steering/4));
                motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
                motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);

                //Update variables
                target_angle_old = target_angle;
                angle_old2 = angle_old1;
                angle_old1 = angle;

                //Enable Motors
                enable = ENABLE;
                setMotor1Speed(-motor1);
                setMotor2Speed(-motor2);
                robot_pos += (-motor1 + -motor2) / 2;
                //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
            }

            //Fast Loop
            if(loop_counter >= 5) {
                loop_counter = 0;
                //pc.printf("angle: %d horz: %d verti: %d knob1: %d knob2: %d knob3: %d knob4: %d \r\n", int16_t(angle-ANGLE_OFFSET), jstick_h, jstick_v, knob1, knob2, knob3, knob4);
                //setMotor1Speed(int16_t(angle));
                //setMotor2Speed(int16_t(angle));
                //pc.printf("horz: %d verti: %d knob1: %d angle: %d \r\n", jstick_h, jstick_v, knob1, (int)angle);
                //pc.printf("angle: %d \r\n", (int)angle);
                pc.printf("angle:%d Kp: %0.3f Kd: %0.2f  Kp_thr: %0.2f Kd_thr: %0.3f  tang: %0.2f dt:%d pos_M1:%d pos_M2:%d rob_pos: %d\r\n", (int)angle, Kp, Kd, Kp_thr, Ki_thr, target_angle, dt, pos_M1, pos_M2, robot_pos);
            }


            //Meduim Loop
            if (medium_loop_counter >= 10) {
                medium_loop_counter = 0; // Read  status
                led2 = led2^1;

                //Recieve Data
                rxLen = mrf.Receive(rxBuffer, 128);
                if(rxLen) {
                    if((rxBuffer[0] == (uint8_t)1) && (rxBuffer[1] == (uint8_t)8) && (rxBuffer[2]==(uint8_t)0)) {
                        jstick_h = (int8_t)rxBuffer[JSTICK_H] - JSTICK_OFFSET;
                        jstick_v = (int8_t)rxBuffer[JSTICK_V] - JSTICK_OFFSET;
                        knob1 = rxBuffer[KNOB1];
                        knob2 = rxBuffer[KNOB2];
                        knob3 = rxBuffer[KNOB3];
                        knob4 = rxBuffer[KNOB4];
                        button = rxBuffer[BUTTON];
                        led1= led1^1;  //flash led for debuggin
                        led4 = button;
                    }
                } else {
                    mrf.Reset();
                }
            }  // End of medium loop
            
            //Slow Loop
            if(slow_loop_counter >= 99) {
                slow_loop_counter = 0;

                //Send Data
                txBuffer[ANGLE] = (uint8_t)(angle + TX_ANGLE_OFFSET);
                mrf.Send(txBuffer, TX_BUFFER_LEN);
            } //End of Slow Loop

            //Reattach interupt
            checkpin.rise(&dmpDataReady);
        } //END WHILE

        //Disable IRQ
        checkpin.rise(NULL);
        led3 = led3^1;
        //pc.printf("taking care of imu stuff angle: %f \r\n", angle);
        //All IMU stuff
        // 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();
            pc.printf("FIFO overflow!");

            // 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
            while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

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

            // track FIFO count here in case there is > 1 packet available
            // (this lets us immediately read more without waiting for an interrupt)
            fifoCount -= packetSize;

            //Read new angle from IMU
            new_angle = (float)(dmpGetPhi() - ANGLE_OFFSET);
            dAngle = new_angle - angle;


            //Filter out angle readings larger then MAX_ANGLE_DELTA
            if( ((dAngle < 15) && (dAngle > -15)) || FILTER_DISABLE) {
                angle = new_angle;
                FILTER_DISABLE = false; //turn of filter disabler
            } else {
                pc.printf("\t\t\t filtered angle \r\n");
            }
            //END IMU STUFF

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