File content as of revision 20:82836745332e:

#include "mbed.h"

#include "irpair.h"
#include "main.h"
#include "motor.h"

#include <stdlib.h>
#include "ITG3200.h"
#include "stm32f4xx.h"
#include "QEI.h"

/* Constants for when HIGH_PWM_VOLTAGE = 0.2
#define IP_CONSTANT 6
#define II_CONSTANT 0
#define ID_CONSTANT 1
*/

// Constants for when HIGH_PWM_VOLTAGE = 0.1
#define IP_CONSTANT 8.85
#define II_CONSTANT 0.005
#define ID_CONSTANT 3.15

const int desiredCountR = 1400;
const int desiredCountL = 1475;

const int oneCellCount = 5400;
const int oneCellCountMomentum = 4700;      // one cell count is actually approximately 5400, but this value is considering momentum!

float receiverOneReading = 0.0;
float receiverTwoReading = 0.0;
float receiverThreeReading = 0.0;
float receiverFourReading = 0.0;

void pidOnEncoders();

void turnLeft()
{
    double speed0 = 0.15;
    double speed1 = -0.15;

    int counter = 0;
    int initial0 = encoder0.getPulses();
    int initial1 = encoder1.getPulses();

    int desiredCount0 = initial0 - desiredCountL;
    int desiredCount1 = initial1 + desiredCountL;

    int count0 = initial0;
    int count1 = initial1;

    double error0 = count0 - desiredCount0;
    double error1 = count1 - desiredCount1;


    while(1) {

        if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
            count0 = encoder0.getPulses();
            count1 = encoder1.getPulses();

            error0 = count0 - desiredCount0;
            error1 = count1 - desiredCount1;

            right_motor.move(speed0);
            left_motor.move(speed1);
            counter = 0;
        } else {
            counter++;
            right_motor.brake();
            left_motor.brake();
        }

        if (counter > 60) {
            break;
        }
    }

    right_motor.brake();
    left_motor.brake();
    turnFlag = 0;           // zeroing out the flags!
    currDir -= 1;
}

void turnRight()
{
    double speed0 = -0.15;
    double speed1 = 0.15;

    int counter = 0;
    int initial0 = encoder0.getPulses();
    int initial1 = encoder1.getPulses();

    int desiredCount0 = initial0 + desiredCountR;
    int desiredCount1 = initial1 - desiredCountR;

    int count0 = initial0;
    int count1 = initial1;

    double error0 = count0 - desiredCount0;
    double error1 = count1 - desiredCount1;

    while(1) {

        if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
            count0 = encoder0.getPulses();
            count1 = encoder1.getPulses();

            error0 = count0 - desiredCount0;
            error1 = count1 - desiredCount1;

            right_motor.move(speed0);
            left_motor.move(speed1);
            counter = 0;
        } else {
            counter++;
            right_motor.brake();
            left_motor.brake();
        }

        if (counter > 60) {
            break;
        }
    }

    right_motor.brake();
    left_motor.brake();
    turnFlag = 0;
    currDir += 1;
}

void turnLeft180()
{
    double speed0 = 0.15;
    double speed1 = -0.15;

    int counter = 0;
    int initial0 = encoder0.getPulses();
    int initial1 = encoder1.getPulses();

    int desiredCount0 = initial0 - desiredCountL*2;
    int desiredCount1 = initial1 + desiredCountL*2;

    int count0 = initial0;
    int count1 = initial1;

    double error0 = count0 - desiredCount0;
    double error1 = count1 - desiredCount1;


    while(1) {
    
        if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
            count0 = encoder0.getPulses();
            count1 = encoder1.getPulses();

            error0 = count0 - desiredCount0;
            error1 = count1 - desiredCount1;

            right_motor.move(speed0);
            left_motor.move(speed1);
            counter = 0;
        } else {
            counter++;
            right_motor.brake();
            left_motor.brake();
        }

        if (counter > 60) {
            break;
        }
    }

    right_motor.brake();
    left_motor.brake();
    currDir -= 2;
}

void turnRight180()
{
    double speed0 = -0.15;
    double speed1 = 0.15;

    int counter = 0;
    int initial0 = encoder0.getPulses();
    int initial1 = encoder1.getPulses();

    int desiredCount0 = initial0 + desiredCountR*2;
    int desiredCount1 = initial1 - desiredCountR*2;

    int count0 = initial0;
    int count1 = initial1;

    double error0 = count0 - desiredCount0;
    double error1 = count1 - desiredCount1;


    while(1) {

        if(!(abs(error0) < 1) && !(abs(error1) < 1)) {
            count0 = encoder0.getPulses();
            count1 = encoder1.getPulses();

            error0 = count0 - desiredCount0;
            error1 = count1 - desiredCount1;

            right_motor.move(speed0);
            left_motor.move(speed1);
            counter = 0;
        } else {
            counter++;
            right_motor.brake();
            left_motor.brake();
        }

        if (counter > 60) {
            break;
        }
    }
    right_motor.brake();
    left_motor.brake();
    currDir += 2;
}

void moveForwardCellEncoder(double cellNum){
    int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellNum;
    int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellNum;

    left_motor.forward(0.125);
    right_motor.forward(0.095);
    wait_ms(1);
    while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
        receiverTwoReading = IRP_2.getSamples(100);
        receiverThreeReading = IRP_3.getSamples(100);
        // serial.printf("Average 2: %f Average 3: %f Sensor 2: %f Sensor 3: %f\n", IRP_2.sensorAvg, IRP_3.sensorAvg, receiverTwoReading, receiverThreeReading);
        if (receiverThreeReading < IRP_3.sensorAvg/4){
            // redLed.write(1);
            // blueLed.write(0);
            turnFlag |= RIGHT_FLAG;
        }
        else if (receiverTwoReading < IRP_2.sensorAvg/4){
            // redLed.write(0);
            // blueLed.write(1);
            turnFlag |= LEFT_FLAG;
        }
        pidOnEncoders();
    }

    left_motor.brake();
    right_motor.brake();
}

void handleTurns(){
    if (turnFlag == 0x1){
        // moveForwardCellEncoder(0.3);
        turnLeft();
    }
    else if (turnFlag == 0x2){
        // moveForwardCellEncoder(0.3);
        turnRight();
    }
    else if (turnFlag == 0x3){
        // moveForwardCellEncoder(0.3);
        turnLeft();
        turnRight();
    }
}

void moveForwardUntilWallIr()
{
    double currentError = 0;
    double previousError = 0;
    double derivError = 0;
    double sumError = 0;

    double HIGH_PWM_VOLTAGE = 0.1;

    double rightSpeed = 0.1;
    double leftSpeed = 0.12;

    float ir2 = IRP_2.getSamples( SAMPLE_NUM );
    float ir3 = IRP_3.getSamples( SAMPLE_NUM );

    int count = encoder0.getPulses();
    while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) {
        int pulseCount = (encoder0.getPulses()-count) % 5600;
        if (pulseCount > 5400 && pulseCount < 5800) {
            blueLed.write(0);
        } else {
            blueLed.write(1);
        }
        sumError += currentError;
        currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
        derivError = currentError - previousError;
        double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
        if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
            rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
            leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
        } else { // r is faster than L. speed up l, slow down r
            rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
            leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
        }

        if (leftSpeed > 0.30) leftSpeed = 0.30;
        if (leftSpeed < 0) leftSpeed = 0;
        if (rightSpeed > 0.30) rightSpeed = 0.30;
        if (rightSpeed < 0) rightSpeed = 0;

        right_motor.forward(rightSpeed);
        left_motor.forward(leftSpeed);

        previousError = currentError;

        ir2 = IRP_2.getSamples( SAMPLE_NUM );
        ir3 = IRP_3.getSamples( SAMPLE_NUM );

    }

    //backward();
    wait_ms(40);
    //brake();

    left_motor.brake();
    right_motor.brake();
}

void printDipFlag()
{
    if (DEBUGGING) serial.printf("Flag value is %d", dipFlags);
}

void enableButton1()
{
    dipFlags |= BUTTON1_FLAG;
    printDipFlag();
}
void enableButton2()
{
    dipFlags |= BUTTON2_FLAG;
    printDipFlag();
}
void enableButton3()
{
    dipFlags |= BUTTON3_FLAG;
    printDipFlag();
}
void enableButton4()
{
    dipFlags |= BUTTON4_FLAG;
    printDipFlag();
}
void disableButton1()
{
    dipFlags &= ~BUTTON1_FLAG;
    printDipFlag();
}
void disableButton2()
{
    dipFlags &= ~BUTTON2_FLAG;
    printDipFlag();
}
void disableButton3()
{
    dipFlags &= ~BUTTON3_FLAG;
    printDipFlag();
}
void disableButton4()
{
    dipFlags &= ~BUTTON4_FLAG;
    printDipFlag();
}

void pidOnEncoders()
{
    int count0;
    int count1;
    count0 = encoder0.getPulses();
    count1 = encoder1.getPulses();
    int diff = count0 - count1;
    double kp = 0.00011;
    double kd = 0.00014;
    int prev = 0;

    int counter = 0;
    while(1)
    {
        count0 = encoder0.getPulses();
        count1 = encoder1.getPulses();
        int x = count0 - count1;
        //double d = kp * x + kd * ( x - prev );
        double kppart = kp * x;
        double kdpart = kd * (x-prev);
        double d = kppart + kdpart;
        
        //serial.printf( "x: %d,\t prev: %d,\t d: %f,\t kppart: %f,\t kdpart: %f\n", x, prev, d, kppart, kdpart );
        if( x < diff - 40 ) // count1 is bigger, right wheel pushed forward
        {
            left_motor.move( -d );
            right_motor.move( d );
        }
        else if( x > diff + 40 )
        {
            left_motor.move( -d );
            right_motor.move( d );
        }
        // else
        // {
        //     left_motor.brake();
        //     right_motor.brake();   
        // }
        prev = x;
        counter++;
        if (counter == 5)
            break;
    }
}

void oneCellEncoderAndIR(double cellCount){
    double currentError = 0;
    double previousError = 0;
    double derivError = 0;
    double sumError = 0;

    double HIGH_PWM_VOLTAGE = 0.1;
    double rightSpeed = 0.10;
    double leftSpeed = 0.10;

    int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount;
    int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount;

    left_motor.forward(0.125);
    right_motor.forward(0.10);

    float receiverTwoReading = 0.0;
    float receiverThreeReading = 0.0;

    float ir2 = IRP_2.getSamples( SAMPLE_NUM );
    float ir3 = IRP_3.getSamples( SAMPLE_NUM );
        
    while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
        receiverTwoReading = IRP_2.getSamples(100);
        receiverThreeReading = IRP_3.getSamples(100);
        if (receiverTwoReading <= IRP_2.sensorAvg/4)
            currentError = receiverThreeReading - IRP_3.sensorAvg;
        else if (receiverThreeReading <= IRP_3.sensorAvg/4)
            currentError = receiverTwoReading - IRP_2.sensorAvg;
        // else if (receiverTwoReading <= IRP_2.sensorAvg/2 && receiverThreeReading <= IRP_3.sensorAvg/2)       // scenario when both left and right wall missing, use encoders only
        //     moveForwardCellEncoder(2);
        else
            currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg));
        
        sumError += currentError;
        derivError = currentError - previousError;
        double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
        if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
            rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
            leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
        } else { // r is faster than L. speed up l, slow down r
            rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE);
            leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE);
        }   
        if (leftSpeed > 0.30) leftSpeed = 0.30;
        if (leftSpeed < 0) leftSpeed = 0;
        if (rightSpeed > 0.30) rightSpeed = 0.30;
        if (rightSpeed < 0) rightSpeed = 0;

        right_motor.forward(rightSpeed);
        left_motor.forward(leftSpeed);
        pidOnEncoders();

        previousError = currentError;
        ir2 = IRP_2.getSamples( SAMPLE_NUM );
        ir3 = IRP_3.getSamples( SAMPLE_NUM );
    }

    left_motor.brake();
    right_motor.brake();
}

int main()
{
    //Set highest bandwidth.
    gyro.setLpBandwidth(LPFBW_42HZ);
    serial.baud(9600);
    serial.printf("The gyro's address is %s", gyro.getWhoAmI());

    wait (0.1);


    redLed.write(1);
    greenLed.write(0);
    blueLed.write(1);

    //left_motor.forward(0.1);
    //right_motor.forward(0.1);

    // PA_1 is A of right
    // PA_0 is B of right
    // PA_5 is A of left
    // PB_3 is B of left
    //QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
//    QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );

    // TODO: Setting all the registers and what not for Quadrature Encoders
    /*    RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3)
        RCC->AHB1ENR |= 0x11; // Enable GPIO port clock enables for Tim2(A) and Tim5(B)
        GPIOA->AFR[0] |= 0x10; // Set GPIO alternate function modes for Tim2
        GPIOB->AFR[0] |= 0x100; // Set GPIO alternate function modes for Tim5
        */

    // set GPIO pins to alternate for the pins corresponding to A/B for eacah encoder, and 2 alternate function registers need to be selected for each type
    // of alternate function specified
    // 4 modes sets AHB1ENR
    // Now TMR: enable clock with timer, APB1ENR
    // set period, autoreload value, ARR value 2^32-1, CR1 - TMR resets itself, ARPE and EN
    //
    // Encoder mode: disable timer before changing timer to encoder
    // CCMR1/2 1/2 depends on channel 1/2 or 3/4, depends on upper bits, depending which channels you use
    // CCMR sets sample rate and set the channel to input
    // CCER, which edge to trigger on, cannot be 11(not allowed for encoder mode), CCER for both channels
    // SMCR - encoder mode
    // CR1 reenabales
    // then read CNT reg of timer


    dipButton1.rise(&enableButton1);
    dipButton2.rise(&enableButton2);
    dipButton3.rise(&enableButton3);
    dipButton4.rise(&enableButton4);

    dipButton1.fall(&disableButton1);
    dipButton2.fall(&disableButton2);
    dipButton3.fall(&disableButton3);
    dipButton4.fall(&disableButton4);

    //right_motor.forward( 0.2 );
    //left_motor.forward( 0.2 );

    while (1) {
        // oneCellEncoderAndIR(5);
        // break;
        moveForwardCellEncoder(1);
        wait(0.5);
        handleTurns();
        wait(0.5);
        moveForwardCellEncoder(1);
        wait(0.5);
        handleTurns();
        break;
        //pidOnEncoders();
       // moveForwardUntilWallIr();
        //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ());
        //serial.printf("Pulse Count=> e0:%d, e1:%d      \r\n", encoder0.getPulses(),encoder1.getPulses());
        // double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
        // serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n",
        // IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError );

        //reading = Rec_4.read();
//        serial.printf("reading: %f\n", reading);
    }
}