Gabriel Kim
PSI3422_Aula3
Dependencies: HC_SR04_Ultrasonic_Library mbed nRF24L01P
Revision 0:48e6560d23a5, committed 2017-08-16
- Comitter:
- gabrielkim13
- Date:
- Wed Aug 16 12:36:27 2017 +0000
- Commit message:
- PSI3422_Aula3
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HC_SR04_Ultrasonic_Library.lib Wed Aug 16 12:36:27 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/ejteb/code/HC_SR04_Ultrasonic_Library/#e0f9c9fb4cf3
--- /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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed Aug 16 12:36:27 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/fd96258d940d \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/nRF24L01P.lib Wed Aug 16 12:36:27 2017 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/Owen/code/nRF24L01P/#8ae48233b4e4