File content as of revision 9:d9776d8ce47c:

//Balance Bot V4
//Author: Carter Sharer
//Date: 10/13/2016
//ESE519 Lab 6 Part 3 Skeleton Code

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

//Balance Bot Begin
#include "pin_assignments.h"
#include "I2Cdev.h"
#include "JJ_MPU6050_DMP_6Axis.h"
#include "balance_bot.h"
#include "balance_bot_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 point 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 100
#define MAX_TARGET_ANGLE 12
//PID Default control values from constant definitions
float Kp1;
float Kd1;
float Kp2;
float Kd2;

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

//Control Variables
float target_angle;
float throttle = 0; //From joystick
float steering = 0; //From joystick
float max_target_angle = MAX_TARGET_ANGLE;
int robot_pos = 0; //Robots position
bool fallen = true;
float d_angle = 0;
float d_anglemax = 0;
float angle_old2 = 0;
float angle_old3 = 0;
float angle_correction = 0;
float refAngle = 0;

Timer timer;
int timer_value; 
int timer_old; 
int dt;

//Loop Counters 
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"

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


    float integral1 = 0;
    float integral2 = 0;
    int16_t motor1Old = 0;
    int16_t motor2Old = 0;
    float lastError = 0;
    while(1) {
        //Led 4 to indicate if robot it STANDING or FALLEN
        led4 = !fallen;

        //Led 2 to indicate a button press
        led2 = button;

        //If button is pressed reset motor position
        if(button) {
            pos_M1 = 0; //Reset position of Motor 1
            pos_M2 = 0; //Reset position of motor 2
            fallen = false; //Reset fallen flag
        }

        //This is the main while loop, all your code goes here
        while(!mpuInterrupt) {
            //Timer
            timer_value = timer.read_us();

            //Set gainz with knobs
            Kp1 = knob1;
            Kd1 = knob2;
            Kp2 = knob3;
            Kd2 = knob4;

            //Joystick control
            throttle = jstick_v;
            steering = jstick_h;


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

                //wait_ms(1);
                //Enable Motor
                enable = ENABLE;
                
                //controls
                error = angle - 0;//angle_correction; //should be balanced at 0

                float Ki = 0.5;
                
                d_angle = angle-(angle_old+angle_old2+angle_old3)/3.0;
               
                /*
                if (d_angle > d_anglemax) d_anglemax = d_angle;
                if (d_angle > 2.0) d_angle = 2.0;
                if (d_angle < -2.0) d_angle = -2.0;*/
                if (d_angle < 3.0 && d_angle > -3.0) d_angle = 0;
                
                motor1 = error * Kp1/10.0 + Kd1/25.0 * d_angle;
                motor2 = error * Kp2/10.0 + Kd2/25.0 * d_angle;        
                
                /*
                if (d_angle == 0){
                     if (motor1 < -5) angle_correction += 0.002;
                     else if (motor1 > 5) angle_correction -= 0.002;
                }
                */
                /*long positionError = pos_M1 - 0;
                refAngle -= (double)positionError/500000.0; //initially 0
                    
                refAngle -= (double)motor1Old/500000.0;
                    
                if (refAngle < -5) {
                    
                    refAngle = -5;
            
                }
                else if (refAngle > 5) {
                    
                    refAngle = 5;
            
                }
                
                error = (angle - refAngle)/10.0;
                double P = Kp1 * error;
                //I += 0.0001 * error;
                double D = Kd1 * (error - lastError);
                
                lastError = error;
                motor1 = error * P/10.0 + D/25.0 ;
                motor2 = error * P/10.0 + D/25.0 ; 
                */
                
                //Calculate motor inputs
                motor1 += 0.1 * int16_t(throttle/2 + steering/8);
                motor2 += 0.1 * int16_t(throttle/2 - steering/8);
                
                //Cap the max and min values [-100, 100]
                motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
                motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
                
                //Update speed if motor changes are above threshold
                int thresh = 1;
                if ((motor1 - motor1Old >= thresh) || (motor1-motor1Old <= -thresh)) {
                    setMotor1Speed(motor1);
                    motor1Old = motor1;
                }
                //motor1Old = motor1;
                if ((motor2 - motor2Old >= thresh) || (motor2-motor2Old <= -thresh)) {
                    setMotor2Speed(motor2);
                    motor2Old = motor2;
                }
                //motor2Old = motor2;
                //Set Motor Speed here
                //setMotor1Speed(motor1);
                //setMotor2Speed(motor2);

            } else { //FALLEN: Motor Control Disabled
                //Disable Motors
                enable = DISABLE;

                //Set fallen flag
                fallen = true;
                
                integral1 = 0;
                integral2 = 0;
                angle_correction = 0;
                lastError = 0;
                d_anglemax = 0;
            }
    
            /* Here are some loops that trigger at different intervals, this 
            * will allow you to do things at a slower rate, thus saving speed
            * it is important to keep this fast so we dont miss IMU readings */
            
            //Fast Loop: Good for printing to serial monitor
            if(loop_counter >= 5) {
                loop_counter = 0;
                //pc.printf("angle:%0.3f Kp1:%0.3f Kd1:%0.2f Kp2:%0.2f Kd2:%0.3f pos_M1:%d pos_M2:%d dt:%0.2f, %0.2f\r\n", angle, Kp1, Kd1, Kp2, Kd2, pos_M1, pos_M2, robot_pos, throttle, steering);
            }

            //Meduim Loop: Good for sending and receiving 
            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: Good for sending if speed is not an issue 
            if(slow_loop_counter >= 99) {
                slow_loop_counter = 0;
                sprintf(txBuffer, "M1: %d, M2: %d, error: %0.2f, angle: %0.3f, dangle: %0.2f correction: %0.3f, Kp: %0.2f, Kd: %0.2f,  motor1: %d", \
                pos_M1, pos_M2, error, angle, d_angle, d_anglemax, Kp1, Kd1, motor1);
                rf_send(txBuffer, strlen(txBuffer)+1);
            } //End of Slow Loop

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

        /**** Update Values DO NOT MODIFY ********/
        loop_counter++;
        slow_loop_counter++;
        medium_loop_counter++;
        dt = (timer_value - timer_old);
        timer_old = timer_value;
        angle_old3 = angle_old2;
        angle_old2 = angle_old;
        angle_old = angle;
        /*****************************************/
        
        /********************* 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
            } 
        }
        /********************* All IMU Handling DO NOT MODIFY *****************/

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