File content as of revision 9:fb671fa0c0be:

//BroBot V3 this is the most up to date, velocity is now working
//Author: Carter Sharer
//Date: 10/13/2016
//Added communication protocol v1 (no type selection)
//Knob1 39.898 Knob2 44.381 Knob3 10.55 Knob4 18.67
//Knob1 13.418 Knob2 28.848 Knob3 9.45  Knob4 42.29 Good
//Knob1 15.373 Knob2 28.261 Knob3 10.42 Knob4 40.97 Smoother

/******************************* README USAGE *******************************
* This robot must be powered on while it is laying down flat on a still table
* This allows the robot to calibrate the IMU (~5 seconds)
* The motors are DISABLED when the robot tilts more then +-45 degrees from
* vertical.  To ENABLE the motors you must lift the robot to < +- 45 degres and
* press the joystick button.
* To reset the motor positions you must press the josystick button anytime.
******************************************************************************/

//BroBot Begin
#include "cmsis_os.h"
#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"

//Angle Offset is used to set the natural balance point of your robot.
//You should adjust this offset so that your robots balance points is near 0
#define ANGLE_OFFSET 107

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

//Controller Values
float knob1, knob2, knob3, knob4;
float jstick_h, jstick_v;

//PID Default control values from constant definitions
float Kp1 = KP;
float Kd1 = KD;
float Kp2 = KP_THROTTLE;
float Ki2 = KI_THROTTLE;
float Kd2; //Added for CS Pos contorl
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;
float velocity_error;

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;
#include "communication.h"

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

// ================================================================
// ===                      Threads                             ===
// ================================================================
void led2_thread(void const *args)
{
    while (true) {
        //if(mpuInterrupt) {
            led3 = !led3;
            /********************* All IMU Handling DO NOT MODIFY *****************/
            //Disable IRQ
            //checkpin.rise(NULL);

            //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");
                }
            }
            /********************* All IMU Handling DO NOT MODIFY *****************/
        //}


        osDelay(1);
        //pc.printf(" thread2 ");
    }
}
osThreadDef(led2_thread, osPriorityNormal, DEFAULT_STACK_SIZE);

// ================================================================
// ===                      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_old1, robot_pos_old2;
float velocity;
bool fallen = true;

int main()
{
    //Set the Channel. 0 is default, 15 is max
    uint8_t channel = 2;
    mrf.SetChannel(channel);

    pc.baud(115200);
    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 = DISABLE; //Disable Motors

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

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

    //Enable Motors
    enable = ENABLE;
    
    //Create Threads
    osThreadCreate(osThread(led2_thread), NULL);

    while(1) {
        //led1 = led1^1;
        led4 = !fallen;
        led2 = button;

        if(jstick_v > 80)
            led3 = 1;
        else
            led3 = 0;

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

       // while(!mpuInterrupt) {
            timer_value = timer.read_us();

            //Preset Knob Values for Green Balance Bot
            knob1 = 42.157; 
            knob2 = 41.135;
            knob3 = 8.82; 
            knob4 = 20.68;
                
            //Set Gainz with knobs
            Kp1 = ((float)knob1) / 1000.0;
            Kd1 = ((float)knob2) / 1.0;
            Kp2 = ((float)knob3) / 1000.0;
            Kd2 = ((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;

            // STANDING: Motor Control Enabled
            if(((angle < 45) && (angle > -45)) && (fallen == false)) {

                //CS Pd Target Angle Contoller Goes Here

                //Robot Position
                robot_pos = (pos_M1 + pos_M2) / 2;
                target_pos += throttle/2;

                //Robot Current Velocity
                velocity = motor1 + motor2 / 2;

                //CS ***************** Velocity Controller *********************
                //float target_velocity = 0;
                //velocity_error = target_velocity - velocity;
                //target_angle = -velocity_error * Kp2 * 100;
                //CS ***************** Position Controller *********************
                pos_error = robot_pos - target_pos; //robot_pos - target_pos;

                //KP Term
                kp_pos_term = -Kp2 * pos_error;

                //KD Term
                change_in_target_pos = target_pos - target_pos_old;
                change_in_pos = robot_pos - robot_pos_old2;
                //kd_pos_term = ((-Kd2 * change_in_target_pos) + (-Kd2*change_in_pos)) /dt;
                kd_pos_term = ((Kd2 * change_in_target_pos) + (Kd2*velocity));
                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;

                //CS ************ PD Stability CONTROLLER HERE ****************
                error = target_angle - angle;
                kp_term = Kp1 * error;

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

                //pc.printf("dAngle:%f\r\n", angle-angle_old1);

                //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));
                motor2 = (int16_t)(control_output - (steering));
                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 { //[FALLEN}
                //Disable Motors
                enable = DISABLE;

                //Set fallen flag
                fallen = true;
            }

            //Fast Loop
            if(loop_counter >= 5) {
                loop_counter = 0;
                pc.printf("angle:%d Kp1: %0.3f Kd1: %0.2f  Kp2: %0.2f Kd2: %0.3f  tang: %0.2f dt:%d rob_pos: %d VE: %f\r\n", (int)angle, Kp1, Kd1, Kp2, Ki2, target_angle, dt, robot_pos, velocity_error);
                //pc.printf("Jstick_h: %d Jstick_v: %d Knob1 %d Knob2 %d Knob3 %d Knob4 %d Button: %d\r\n", jstick_h, jstick_v, knob1, knob2, knob3, knob4, button);
            }

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

                //Recieve Data
                rxLen = rf_receive(rxBuffer, 128);
                if(rxLen > 0) {
                    led1 = led1^1;
                    //Process data with our protocal
                    communication_protocal(rxLen);
                }

            }  // End of medium loop

            //Slow Loop
            if(slow_loop_counter >= 99) {
                slow_loop_counter = 0;

                /* Send Data To Controller goes here *
                 *                                    */

            } //End of Slow Loop

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


        /********************* All IMU Handling DO NOT MODIFY *****************/

        /********************* All IMU Handling DO NOT MODIFY *****************/

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