File content as of revision 11:8fc2b703086b:

#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 15
#define II_CONSTANT 0
#define ID_CONSTANT 0

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.1;
    
    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);
        }
        
        
        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 );       
        
    }
    //sensor1Turn = IR_Sensor1.read();
    //sensor4Turn = IR_Sensor4.read();
    
    //backward();
    wait_ms(40);
    //brake();
    
    left_motor.brake();
    right_motor.brake();
    while( 1 )
    {
        
    }
}

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();
}

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

    wait (0.1);

//    IR_1.write(1);
//    IR_2.write(1);
//    IR_3.write(1);
//    IR_4.write(1);

    redLed.write(1);
    greenLed.write(1);
    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
    
    dipButton1.rise(&enableButton1);
    dipButton2.rise(&enableButton2);
    dipButton3.rise(&enableButton3);
    dipButton4.rise(&enableButton4);
    
    dipButton1.fall(&disableButton1);
    dipButton2.fall(&disableButton2);
    dipButton3.fall(&disableButton3);
    dipButton4.fall(&disableButton4);
    

    while (1) {
        
        moveForwardUntilWallIr();
        wait(0.1);
        //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);
//        redLed.write(0);
//        wait_ms(1000);
//        redLed.write(1);
//        greenLed.write(0);
//        wait_ms(1000);
//        greenLed.write(1);
//        blueLed.write(0);
//        wait_ms(1000);
//        blueLed.write(1);
    }
}