File content as of revision 9:67f2110fce8e:

/*
 * The code is released under the GNU General Public License.
 * This is the algorithm for my balancing robot/segway.
 * It is controlled by Bluetooth
 */

#include "mbed.h"
#include "BalancingRobot.h"
#include "MPU6050.h"
#include "ledControl.h"
#include "HALLFX_ENCODER.h"

/* Setup serial communication */
Serial blue(p28,p27); // For remote control ps3
Serial pc(USBTX, USBRX); // USB

/* IMU */
MPU6050 mpu6050;  

/* Encoders*/
HALLFX_ENCODER leftEncoder(p13);
HALLFX_ENCODER rightEncoder(p14);

/* Setup timer instance */
Timer t;

/* Ticker for led toggling */
Ticker toggler1;               

int main() {
    /* Set baudrate */
    blue.baud(9600);
    pc.baud(9600);
    
    /* Set PWM frequency */
    leftPWM.period(0.00005); // The motor driver can handle a pwm frequency up to 20kHz (1/20000=0.00005)
    rightPWM.period(0.00005);
    
    /* Calibrate the gyro and accelerometer relative to ground */
    mpu6050.whoAmI();                            // Communication test: WHO_AM_I register reading 
    wait(0.5);
    mpu6050.calibrate(accelBias,gyroBias);       // Calibrate MPU6050 and load biases into bias registers
    pc.printf("Calibration is completed. \r\n"); 
    mpu6050.init();                              // Initialize the sensor
    pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
    
    /* Setup timing */
    t.start();
    loopStartTime = t.read_us();
    timer = loopStartTime;
    
    while (1) {
        /* Calculate pitch angle using a Complementary filter */
        mpu6050.complementaryFilter(&pitchAngle, &rollAngle);
        wait_us(5);
        timer = t.read_us();
        
        /* Drive motors */
        if (abs(pitchAngle)> 20) // Stop if falling or laying down
            stopAndReset();
        else
            PID(targetAngle,targetOffset);
        
        /* Update wheel velocity every 100ms */
        loopCounter++;
        if (loopCounter%10 == 0) 
        { // If remainder is equal 0, it must be 10,20,30 etc.
            LeftWheelPosition = leftEncoder.read();
            RightWheelPosition = rightEncoder.read();
            wheelPosition = LeftWheelPosition + RightWheelPosition;
            wheelVelocity = wheelPosition - lastWheelPosition;
            lastWheelPosition = wheelPosition;
            
            if (abs(wheelVelocity) <= 20 && !stopped) 
            { // Set new targetPosition if braking
                targetPosition = wheelPosition;
                stopped = true;
            }
        }
        
        pc.printf("Pitch: %f, error: %f, PIDValue: %f \n",pitchAngle,error,PIDValue);
        pc.printf("Left Wheel Encoder: %f, Right Wheel Encoder: %f \n", LeftWheelPosition, RightWheelPosition);
        pc.printf("Wheel Position: %f, Last wheel position:%f, Velocity: %f \n\n", wheelPosition, lastWheelPosition, wheelVelocity);
                
        /* Check for incoming serial data */
        if (blue.readable()) // Check if there's any incoming data
            receiveBluetooth();
          
        /* Use a time fixed loop */
        lastLoopUsefulTime = t.read_us() - loopStartTime;
        if (lastLoopUsefulTime < STD_LOOP_TIME)
            wait_us(STD_LOOP_TIME - lastLoopUsefulTime);
        lastLoopTime = t.read_us() - loopStartTime; // only used for debugging
        loopStartTime = t.read_us();
    }
}

void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}

void toggle_led1() {ledToggle(1);}
void toggle_led2() {ledToggle(2);}

void PID(double restAngle, double offset) 
{
    /* Steer robot */
    if (steerForward) 
    {
        offset += (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing
        restAngle -= offset;
    } 
    else if (steerBackward) 
    {
        offset -= (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing
        restAngle += offset;
    }
    /* Brake */
    else if (steerStop) 
    {
        //long positionError = wheelPosition - targetPosition;
        
    /*  if (abs(positionError) > zoneA) // Inside zone A
            restAngle -= (double)positionError/positionScaleA;
        else if (abs(positionError) > zoneB) // Inside zone B
            restAngle -= (double)positionError/positionScaleB;
        else // Inside zone C
            restAngle -= (double)positionError/positionScaleC; */
        
        restAngle -= (double)wheelVelocity/velocityScaleStop;
        
        if (restAngle < -5.00) // Limit rest Angle
            restAngle = -5.00;
        else if (restAngle > 5.00)
            restAngle = 5.00;
    }
    
    /* Update PID values */
    error = restAngle - pitchAngle;
    double pTerm = Kp * error;
    iTerm += Ki * error;
    double dTerm = Kd * (error - lastError);
    lastError = error;
    
    PIDValue = pTerm + iTerm + dTerm;
    
    //pc.printf("Pitch: %5.2f\tPIDValue: %5.2f\tKp: %5.2f\tKi: %5.2f\tKd: %5.2f\n",pitch,PIDValue,Kp,Ki,Kd);
    
    /* Steer robot sideways */
    double PIDLeft;
    double PIDRight;
    if (steerLeft) {
        PIDLeft = PIDValue-turnSpeed;
        PIDRight = PIDValue+turnSpeed;
    } else if (steerRotateLeft) {
        PIDLeft = PIDValue-rotateSpeed;
        PIDRight = PIDValue+rotateSpeed;
    } else if (steerRight) {
        PIDLeft = PIDValue+turnSpeed;
        PIDRight = PIDValue-turnSpeed;
    } else if (steerRotateRight) {
        PIDLeft = PIDValue+rotateSpeed;
        PIDRight = PIDValue-rotateSpeed;
    } else {
        PIDLeft = PIDValue;
        PIDRight = PIDValue;
    }
    //PIDLeft *= 0.95; // compensate for difference in the motors
    
    /* Set PWM Values */
    if (PIDLeft >= 0)
        move(left, forward, PIDLeft);
    else
        move(left, backward, PIDLeft * -1);
    if (PIDRight >= 0)
        move(right, forward, PIDRight);
    else
        move(right, backward, PIDRight * -1);
}
void receiveBluetooth() {
    //char input[16]; // The serial buffer is only 16 characters
    char phone_char;
    //int i = 0;
    //while (1) {
        //input[i] = blue.getc();
        //if (input[i] == ';' || i == 15) // keep reading until it reads a semicolon or it exceeds the serial buffer
            //break;
        //i++;
        
        //pc.printf("Im here..");
    //}
    phone_char = blue.getc();
    pc.putc(phone_char);
    //pc.printf("Input: %c\n",i);
    
    // Set all false
    steerForward = false;
    steerBackward = false;
    steerStop = false;
    steerLeft = false;
    steerRotateLeft = false;
    steerRight = false;
    steerRotateRight = false;
    
    /* For remote control */
    if (/*input[0]*/phone_char == 'F') { // Forward
        //strtok(/*input*/phone_char, ","); // Ignore 'F'
        //targetOffset = atof((strtok(NULL, ";")); // read until the end and then convert from string to double
        //pc.printf("%f\n",targetOffset); // Print targetOffset for debugging
        steerForward = true;
    } else if (/*input[0]*/phone_char == 'B') { // Backward
        //strtok(/*input*/phone_char, ","); // Ignore 'B'
        targetOffset = atof(strtok(NULL, ";")); // read until the end and then convert from string to double
        //pc.printf("%f\n",targetOffset); // Print targetOffset for debugging
        steerBackward = true;
    } else if (/*input[0]*/phone_char == 'L') { // Left
        if (/*input[0]*/phone_char == 'L') // Left Rotate
            steerRotateLeft = true;
        else
            steerLeft = true;
    } else if (/*input[0]*/phone_char == 'R') { // Right
        if (/*input[0]*/phone_char == 'R') // Right Rotate
            steerRotateRight = true;
        else
            steerRight = true;
    } else if (/*input[0]*/phone_char == 'S') { // Stop
        steerStop = true;
        stopped = false;
        targetPosition = wheelPosition;
    } else if (/*input[0]*/phone_char == 'A') { // Abort
        stopAndReset();
        while (blue.getc() != 'C'); // Wait until continue is send
    }
}

void stopAndReset() {
    stop(both);
    lastError = 0;
    iTerm = 0;
    targetPosition = wheelPosition;
}

void move(Motor motor, Direction direction, float speed) { // speed is a value in percentage (0.0f to 1.0f)
    if (motor == left) {
        leftPWM = speed;
        if (direction == forward) {
            leftA = 0;
            leftB = 1;
        } else if (direction == backward) {
            leftA = 1;
            leftB = 0;
        }
    } else if (motor == right) {
        rightPWM = speed;
        if (direction == forward) {
            rightA = 0;
            rightB = 1;
        } else if (direction == backward) {
            rightA = 1;
            rightB = 0;
        }
    } else if (motor == both) {
        leftPWM = speed;
        rightPWM = speed;
        if (direction == forward) {
            leftA = 0;
            leftB = 1;
            
            rightA = 0;
            rightB = 1;
        } else if (direction == backward) {
            leftA = 1;
            leftB = 0;
            
            rightA = 1;
            rightB = 0;
        }
    }
}
void stop(Motor motor) {
    if (motor == left) {
        leftPWM = 1;
        leftA = 1;
        leftB = 1;
    } else if (motor == right) {
        rightPWM = 1;
        rightA = 1;
        rightB = 1;
    } else if (motor == both) {
        leftPWM = 1;
        leftA = 1;
        leftB = 1;
        
        rightPWM = 1;
        rightA = 1;
        rightB = 1;
    }
}