File content as of revision 0:48e6560d23a5:

/* Libraries */
#include "mbed.h"
#include "ultrasonic.h"
#include "nRF24L01P.h"

#define DEBUG

/* Encoder Defines */
#define LEFT_ENC        PTD4
#define RIGHT_ENC       PTA12

#define HOLES_ENC       20

#define DEBOUNCE_MS     5

/* HC-SR04 Defines */
#define TRIG_PIN        PTD7
#define ECHO_PIN        PTD6

#define REFRESH         0.050
#define TIMEOUT         0.100

#define N_AVERAGES      10    

/* L298 Defines */
#define LEFTA_PIN       PTB1
#define LEFTB_PIN       PTB0
#define RIGHTA_PIN      PTB2
#define RIGHTB_PIN      PTB3

#define LEFT            0
#define RIGHT           1

#define PWM_FREQUENCY   1000.0
#define PWM_STEP        0.1
#define PWM_DELAY_US    50   

/* nRF24L01 Defines */
#define MOSI_PIN        PTD2
#define MISO_PIN        PTD3
#define SCK_PIN         PTD1
#define CSN_PIN         PTD5
#define CE_PIN          PTD0
#define IRQ_PIN         PTA13

#define TRANSFER_SIZE   3

/* Encoder Functions */
void leftInterrupt();
void rightInterrupt();

/* HC-SR04 Functions */
void distanceUpdate(int);

/* L298 Functions */
void setSpeed(unsigned char, float);

#ifdef DEBUG
Serial pc(USBTX, USBRX);
#endif

/* Encoder Variables */
InterruptIn leftEncoder(LEFT_ENC);
InterruptIn rightEncoder(RIGHT_ENC);

Timer debounce;

unsigned char leftCounts = 0;
unsigned char rightCounts = 0;

/* HC-SR04 Variables */
ultrasonic hcsr04(TRIG_PIN, ECHO_PIN, REFRESH, TIMEOUT, &distanceUpdate);

unsigned long int distanceSum = 0;
unsigned int currentDistance = 0;
unsigned int previousDistance = 0;
unsigned int averagedDistance = 0;

/* L298 Variables */
PwmOut leftOutA(LEFTA_PIN);
PwmOut leftOutB(LEFTB_PIN);
PwmOut rightOutA(RIGHTA_PIN);
PwmOut rightOutB(RIGHTB_PIN);

float leftSpeed = 0.0;
float rightSpeed = 0.0;

/* nRF24L01 Variables */
nRF24L01P nRF24(MOSI_PIN, MISO_PIN, SCK_PIN, CSN_PIN, CE_PIN, IRQ_PIN);

/* Main */
int main() {
    signed char txData[TRANSFER_SIZE], rxData[TRANSFER_SIZE];
    
    // Encoder initialization
    // leftEncoder.rise(&leftInterrupt);
    // rightEncoder.rise(&rightInterrupt);
    
    // HCSR04 initialization
    hcsr04.startUpdates();
    
    // L298 initialization
    leftOutA.period_us(1000000.0 / PWM_FREQUENCY);
    leftOutB.period_us(1000000.0 / PWM_FREQUENCY);
    rightOutA.period_us(1000000.0 / PWM_FREQUENCY);
    rightOutB.period_us(1000000.0 / PWM_FREQUENCY);
    
    // nRF24L01 initialization
    nRF24.powerUp();
    nRF24.setRfFrequency(2450);
 
    #ifdef DEBUG
    pc.printf("nRF24L01+ Frequency    : %d MHz\r\n", nRF24.getRfFrequency());
    pc.printf("nRF24L01+ Output power : %d dBm\r\n", nRF24.getRfOutputPower());
    pc.printf("nRF24L01+ Data Rate    : %d kbps\r\n", nRF24.getAirDataRate());
    pc.printf("nRF24L01+ TX Address   : 0x%010llX\r\n", nRF24.getTxAddress());
    pc.printf("nRF24L01+ RX Address   : 0x%010llX\r\n", nRF24.getRxAddress());
    #endif
 
    nRF24.setTransferSize(TRANSFER_SIZE);
 
    nRF24.setReceiveMode();
    nRF24.enable();
    
    // Main loop
    while(1) {
        if (averagedDistance < 10) {
            setSpeed(LEFT, 0.0);
            setSpeed(RIGHT, 0.0);       
        } else {
            setSpeed(LEFT, 1.0);
            setSpeed(RIGHT, 1.0);    
        }
                
        #ifdef DEBUG
        pc.printf("Distance: %dcm\r\n", averagedDistance);
        #endif
        
        hcsr04.checkDistance();
        
        wait_us(PWM_DELAY_US);
    }
}

/* Left Encoder Interrupt */
void leftInterrupt() {
    debounce.reset();
    debounce.start();
    while (leftEncoder.read());
    debounce.stop();
    
    if (debounce.read_ms() >= DEBOUNCE_MS) {
        leftCounts++;
        
        if (leftCounts >= HOLES_ENC) {
            leftCounts = 0;
            
            #ifdef DEBUG
            pc.printf("Left spin!\r\n");   
            #endif
        }   
    }
}

/* Right Encoder Interrupt */
void rightInterrupt() {
    debounce.reset();
    debounce.start();
    while (rightEncoder.read());
    debounce.stop();
    
    if (debounce.read_ms() >= DEBOUNCE_MS) {
        rightCounts++;
        
        if (rightCounts >= HOLES_ENC) {
            rightCounts = 0;
            
            #ifdef DEBUG
            pc.printf("Right spin!\r\n");   
            #endif
        }   
    }
}

/* HC-SR04 Interrupt */
void distanceUpdate(int distance) {
    currentDistance = distance;
    
    distanceSum -= previousDistance;
    distanceSum += currentDistance;
    
    averagedDistance = distanceSum / N_AVERAGES;
    
    previousDistance = currentDistance;
}

/* L298 Set Speed */
void setSpeed(unsigned char motor, float setSpeed) {
    float speed = 0.0;
    
    // Check for invalid PWM values
    if (setSpeed > 1.0) {
        speed = 1.0;
    } else if (setSpeed < -1.0) {
        speed = -1.0;   
    } else {
        speed = setSpeed;   
    }
    
    // Left Motor
    if (motor == LEFT) {
        // Limit PWM variation to fixed step
        if (speed > leftSpeed) {
            if (speed - leftSpeed > PWM_STEP) {
                leftSpeed += PWM_STEP;
            } else {
                leftSpeed = speed;
            }
        } else if (speed < leftSpeed) {
            if (leftSpeed - speed > PWM_STEP) {
                leftSpeed -= PWM_STEP;
            } else {
                leftSpeed = speed;
            }
        }
        
        // Drive L298 inputs according to leftSpeed
        if (leftSpeed >= 0.0) {
            leftOutA.write(leftSpeed);
            leftOutB.write(0.0);
        } else {
            leftOutA.write(0.0);
            leftOutB.write(-leftSpeed); 
        }
    // Right Motor
    } else if (motor == RIGHT) {
        // Limit PWM variation to fixed step
        if (speed > rightSpeed) {
            if (speed - rightSpeed > PWM_STEP) {
                rightSpeed += PWM_STEP;
            } else {
                rightSpeed = speed;
            }
        } else if (speed < rightSpeed) {
            if (rightSpeed - speed > PWM_STEP) {
                rightSpeed -= PWM_STEP;
            } else {
                rightSpeed = speed;
            }
        }
        
        // Drive L298 inputs according to rightSpeed
        if (rightSpeed >= 0.0) {
            rightOutA.write(rightSpeed);
            rightOutB.write(0.0);
        } else {
            rightOutA.write(0.0);
            rightOutB.write(-rightSpeed); 
        }
    }   
}