File content as of revision 11:0a2944e7be23:

/*
 * 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 "ultrasonic.h"

//#include "HALLFX_ENCODER.h"

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

/* IMU */
MPU6050 mpu6050;



/* HC-SR04 */
void dist(int distance)
{
    //put code here to happen when the distance is changed
    
}

ultrasonic frontUltra(PTA13, PTD5, .1, 1, &dist);
ultrasonic backUltra(PTD0, PTD2, .1, 1, &dist);

/* Setup timer instance */
Timer t;

/* Ticker for led toggling */
Ticker toggler1;      
 

         
int main() {
        

    
    
    /* Set baudrate */
    blue.baud(115200);
    pc.baud(115200);
      
    
    /* 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;
    DesiredAngle = -2.5;
    ConstantAngle = DesiredAngle;

    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(DesiredAngle, 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, error1: %f, PIDValue: %f \n\r",pitchAngle,error1,PIDValue);
                
        //Check for incoming serial data 
        if (blue.readable()) // Check if there's any incoming data
            receiveBluetooth();
        
                 
           
        frontUltra.startUpdates();

        distF = frontUltra.getCurrentDistance();

        backUltra.startUpdates();
       
        distB = backUltra.getCurrentDistance(); 
        
        
        pc.printf("Front distance: %d cm \t Back Distance %d cm \r\n", distF, distB);             
           
        /* 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
        //pc.printf("STD_LOOP_TIME: %d\n\r",STD_LOOP_TIME);
        //pc.printf("anlik: %d\n\r",t.read_us());
        //pc.printf("lastLoopTime: %d\n\r",lastLoopTime);
        //pc.printf("lastLoopUsefulTime: %d\n\r",lastLoopTime);
        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; 
        restAngle -= -1.0;
        
        
    }
    else if (steerBackward) 
    {
        //offset -= (double)wheelVelocity/velocityScaleMove; // Scale down offset at high speed and scale up when reversing
        //restAngle += offset;
        restAngle += 1.0;
    }*/
    /* Brake */
    /*else if (steerStop)
    
        restAngle = 0.0; 
    {
        //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 */
    error1 = restAngle - pitchAngle;
    double pTerm = Kp * error1;
    iTerm += Ki * error1;
    double dTerm = Kd * (error1 - lastError);
    lastError = error1;
    
    PIDValue = pTerm + iTerm + dTerm;
    //pc.printf("DesiredAngle = %lf", DesiredAngle);
    //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 {
    
        if (steerForward && (DesiredAngle > ( ConstantAngle - 1.8 ) ))
            DesiredAngle -= rotateSpeed;
            
        else if (steerBackward && (DesiredAngle < (ConstantAngle + 1.8) ))
        DesiredAngle += rotateSpeed;
        
        else {
        if(!steerForward && DesiredAngle < ConstantAngle)
            DesiredAngle += rotateSpeed;
            
        else if(!steerBackward && DesiredAngle > ConstantAngle)
            DesiredAngle -= rotateSpeed;
        }   
        
        PIDLeft = PIDValue;
        PIDRight = PIDValue;
    
    }
    //PIDLeft *= 0.95; // compensate for difference in the motors
    
    /* Set PWM Values */
    if (PIDLeft >= 0.0)
        move(left, backward, PIDLeft + 0.55);
    else
        move(left, forward, (PIDLeft - 0.55) * -1);
    if (PIDRight >= 0.0)
        move(right, backward, PIDRight + 0.55);
    else
        move(right, forward, (PIDRight - 0.55) * -1);
//pc.printf("RestAngle3 %lf" ,restAngle);
}

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();
    
    /* if(distB < 50 && distB > 4) {
        phone_char = 'S';
        if (distB < 20)
            phone_char = 'F';
    } */
    
    
    if(distB < 20 && distB > 4)
        phone_char = 'F';
    else if ( distB < 30)
        phone_char = 'S';
    else if (distB < 50)
        phone_char = 'B';
    
    
    pc.putc(phone_char);
    pc.printf("Input: %c\n",phone_char);
    
    // 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
        //DesiredAnglee = 0.8;
        pc.printf("F");
        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
        //DesiredAnglee = -5.2;
        pc.printf("B");
        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;
    }
}