File content as of revision 9:1d8e4da058cd:

#include "mbed.h"

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

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

// PID
#define P_CONSTANT 0
#define I_CONSTANT 0
#define D_CONSTANT 0

#define IP_CONSTANT 6
#define II_CONSTANT 0
#define ID_CONSTANT 1

#include "QEI.h"
#define PULSES 880
#define SAMPLE_NUM 100


void moveForwardUntilWallIr() {    
    double currentError = 0;
    double previousError = 0;
    double derivError = 0;
    double sumError = 0;
    
    double HIGH_PWM_VOLTAGE = 0.2;
    
    double rightSpeed = 0.2;
    double leftSpeed = 0.2;
    
    float ir2 = IRP_2.getSamples( SAMPLE_NUM );
    float ir3 = IRP_3.getSamples( SAMPLE_NUM );
    
    while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) {       
        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 )
    {
        
    }
        
    
}

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

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