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
diff -r 000000000000 -r 48e6560d23a5 HC_SR04_Ultrasonic_Library.lib --- /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
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
diff -r 000000000000 -r 48e6560d23a5 mbed.bld --- /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
diff -r 000000000000 -r 48e6560d23a5 nRF24L01P.lib --- /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