main.cpp

00001 /* Libraries */
00002 #include "mbed.h"
00003 #include "ultrasonic.h"
00004 #include "nRF24L01P.h"
00005 
00006 #define DEBUG
00007 
00008 /* Encoder Defines */
00009 #define LEFT_ENC        PTD4
00010 #define RIGHT_ENC       PTA12
00011 
00012 #define HOLES_ENC       20
00013 
00014 #define DEBOUNCE_MS     5
00015 
00016 /* HC-SR04 Defines */
00017 #define TRIG_PIN        PTD7
00018 #define ECHO_PIN        PTD6
00019 
00020 #define REFRESH         0.050
00021 #define TIMEOUT         0.100
00022 
00023 #define N_AVERAGES      10    
00024 
00025 /* L298 Defines */
00026 #define LEFTA_PIN       PTB1
00027 #define LEFTB_PIN       PTB0
00028 #define RIGHTA_PIN      PTB2
00029 #define RIGHTB_PIN      PTB3
00030 
00031 #define LEFT            0
00032 #define RIGHT           1
00033 
00034 #define PWM_FREQUENCY   1000.0
00035 #define PWM_STEP        0.1
00036 #define PWM_DELAY_US    50   
00037 
00038 /* nRF24L01 Defines */
00039 #define MOSI_PIN        PTD2
00040 #define MISO_PIN        PTD3
00041 #define SCK_PIN         PTD1
00042 #define CSN_PIN         PTD5
00043 #define CE_PIN          PTD0
00044 #define IRQ_PIN         PTA13
00045 
00046 #define TRANSFER_SIZE   3
00047 
00048 /* Encoder Functions */
00049 void leftInterrupt();
00050 void rightInterrupt();
00051 
00052 /* HC-SR04 Functions */
00053 void distanceUpdate(int);
00054 
00055 /* L298 Functions */
00056 void setSpeed(unsigned char, float);
00057 
00058 #ifdef DEBUG
00059 Serial pc(USBTX, USBRX);
00060 #endif
00061 
00062 /* Encoder Variables */
00063 InterruptIn leftEncoder(LEFT_ENC);
00064 InterruptIn rightEncoder(RIGHT_ENC);
00065 
00066 Timer debounce;
00067 
00068 unsigned char leftCounts = 0;
00069 unsigned char rightCounts = 0;
00070 
00071 /* HC-SR04 Variables */
00072 ultrasonic hcsr04(TRIG_PIN, ECHO_PIN, REFRESH, TIMEOUT, &distanceUpdate);
00073 
00074 unsigned long int distanceSum = 0;
00075 unsigned int currentDistance = 0;
00076 unsigned int previousDistance = 0;
00077 unsigned int averagedDistance = 0;
00078 
00079 /* L298 Variables */
00080 PwmOut leftOutA(LEFTA_PIN);
00081 PwmOut leftOutB(LEFTB_PIN);
00082 PwmOut rightOutA(RIGHTA_PIN);
00083 PwmOut rightOutB(RIGHTB_PIN);
00084 
00085 float leftSpeed = 0.0;
00086 float rightSpeed = 0.0;
00087 
00088 /* nRF24L01 Variables */
00089 nRF24L01P nRF24(MOSI_PIN, MISO_PIN, SCK_PIN, CSN_PIN, CE_PIN, IRQ_PIN);
00090 
00091 /* Main */
00092 int main() {
00093     signed char txData[TRANSFER_SIZE], rxData[TRANSFER_SIZE];
00094     
00095     // Encoder initialization
00096     // leftEncoder.rise(&leftInterrupt);
00097     // rightEncoder.rise(&rightInterrupt);
00098     
00099     // HCSR04 initialization
00100     hcsr04.startUpdates();
00101     
00102     // L298 initialization
00103     leftOutA.period_us(1000000.0 / PWM_FREQUENCY);
00104     leftOutB.period_us(1000000.0 / PWM_FREQUENCY);
00105     rightOutA.period_us(1000000.0 / PWM_FREQUENCY);
00106     rightOutB.period_us(1000000.0 / PWM_FREQUENCY);
00107     
00108     // nRF24L01 initialization
00109     nRF24.powerUp();
00110     nRF24.setRfFrequency(2450);
00111  
00112     #ifdef DEBUG
00113     pc.printf("nRF24L01+ Frequency    : %d MHz\r\n", nRF24.getRfFrequency());
00114     pc.printf("nRF24L01+ Output power : %d dBm\r\n", nRF24.getRfOutputPower());
00115     pc.printf("nRF24L01+ Data Rate    : %d kbps\r\n", nRF24.getAirDataRate());
00116     pc.printf("nRF24L01+ TX Address   : 0x%010llX\r\n", nRF24.getTxAddress());
00117     pc.printf("nRF24L01+ RX Address   : 0x%010llX\r\n", nRF24.getRxAddress());
00118     #endif
00119  
00120     nRF24.setTransferSize(TRANSFER_SIZE);
00121  
00122     nRF24.setReceiveMode();
00123     nRF24.enable();
00124     
00125     // Main loop
00126     while(1) {
00127         if (averagedDistance < 10) {
00128             setSpeed(LEFT, 0.0);
00129             setSpeed(RIGHT, 0.0);       
00130         } else {
00131             setSpeed(LEFT, 1.0);
00132             setSpeed(RIGHT, 1.0);    
00133         }
00134                 
00135         #ifdef DEBUG
00136         pc.printf("Distance: %dcm\r\n", averagedDistance);
00137         #endif
00138         
00139         hcsr04.checkDistance();
00140         
00141         wait_us(PWM_DELAY_US);
00142     }
00143 }
00144 
00145 /* Left Encoder Interrupt */
00146 void leftInterrupt() {
00147     debounce.reset();
00148     debounce.start();
00149     while (leftEncoder.read());
00150     debounce.stop();
00151     
00152     if (debounce.read_ms() >= DEBOUNCE_MS) {
00153         leftCounts++;
00154         
00155         if (leftCounts >= HOLES_ENC) {
00156             leftCounts = 0;
00157             
00158             #ifdef DEBUG
00159             pc.printf("Left spin!\r\n");   
00160             #endif
00161         }   
00162     }
00163 }
00164 
00165 /* Right Encoder Interrupt */
00166 void rightInterrupt() {
00167     debounce.reset();
00168     debounce.start();
00169     while (rightEncoder.read());
00170     debounce.stop();
00171     
00172     if (debounce.read_ms() >= DEBOUNCE_MS) {
00173         rightCounts++;
00174         
00175         if (rightCounts >= HOLES_ENC) {
00176             rightCounts = 0;
00177             
00178             #ifdef DEBUG
00179             pc.printf("Right spin!\r\n");   
00180             #endif
00181         }   
00182     }
00183 }
00184 
00185 /* HC-SR04 Interrupt */
00186 void distanceUpdate(int distance) {
00187     currentDistance = distance;
00188     
00189     distanceSum -= previousDistance;
00190     distanceSum += currentDistance;
00191     
00192     averagedDistance = distanceSum / N_AVERAGES;
00193     
00194     previousDistance = currentDistance;
00195 }
00196 
00197 /* L298 Set Speed */
00198 void setSpeed(unsigned char motor, float setSpeed) {
00199     float speed = 0.0;
00200     
00201     // Check for invalid PWM values
00202     if (setSpeed > 1.0) {
00203         speed = 1.0;
00204     } else if (setSpeed < -1.0) {
00205         speed = -1.0;   
00206     } else {
00207         speed = setSpeed;   
00208     }
00209     
00210     // Left Motor
00211     if (motor == LEFT) {
00212         // Limit PWM variation to fixed step
00213         if (speed > leftSpeed) {
00214             if (speed - leftSpeed > PWM_STEP) {
00215                 leftSpeed += PWM_STEP;
00216             } else {
00217                 leftSpeed = speed;
00218             }
00219         } else if (speed < leftSpeed) {
00220             if (leftSpeed - speed > PWM_STEP) {
00221                 leftSpeed -= PWM_STEP;
00222             } else {
00223                 leftSpeed = speed;
00224             }
00225         }
00226         
00227         // Drive L298 inputs according to leftSpeed
00228         if (leftSpeed >= 0.0) {
00229             leftOutA.write(leftSpeed);
00230             leftOutB.write(0.0);
00231         } else {
00232             leftOutA.write(0.0);
00233             leftOutB.write(-leftSpeed); 
00234         }
00235     // Right Motor
00236     } else if (motor == RIGHT) {
00237         // Limit PWM variation to fixed step
00238         if (speed > rightSpeed) {
00239             if (speed - rightSpeed > PWM_STEP) {
00240                 rightSpeed += PWM_STEP;
00241             } else {
00242                 rightSpeed = speed;
00243             }
00244         } else if (speed < rightSpeed) {
00245             if (rightSpeed - speed > PWM_STEP) {
00246                 rightSpeed -= PWM_STEP;
00247             } else {
00248                 rightSpeed = speed;
00249             }
00250         }
00251         
00252         // Drive L298 inputs according to rightSpeed
00253         if (rightSpeed >= 0.0) {
00254             rightOutA.write(rightSpeed);
00255             rightOutB.write(0.0);
00256         } else {
00257             rightOutA.write(0.0);
00258             rightOutB.write(-rightSpeed); 
00259         }
00260     }   
00261 }