Diff: main.cpp

Revision:

0:48e6560d23a5

diff -r 000000000000 -r 48e6560d23a5 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Aug 16 12:36:27 2017 +0000
@@ -0,0 +1,261 @@
+/* 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); 
+        }
+    }   
+}
\ No newline at end of file