Gabriel Kim / Mbed 2 deprecated Test_Robot

PSI3422_Aula3

Dependencies:   HC_SR04_Ultrasonic_Library mbed nRF24L01P

main.cpp@0:48e6560d23a5, 2017-08-16 (annotated)

Committer:
gabrielkim13
Date:
Wed Aug 16 12:36:27 2017 +0000
Revision:
0:48e6560d23a5
PSI3422_Aula3

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gabrielkim13 0:48e6560d23a5 1 /* Libraries */
gabrielkim13 0:48e6560d23a5 2 #include "mbed.h"
gabrielkim13 0:48e6560d23a5 3 #include "ultrasonic.h"
gabrielkim13 0:48e6560d23a5 4 #include "nRF24L01P.h"
gabrielkim13 0:48e6560d23a5 5
gabrielkim13 0:48e6560d23a5 6 #define DEBUG
gabrielkim13 0:48e6560d23a5 7
gabrielkim13 0:48e6560d23a5 8 /* Encoder Defines */
gabrielkim13 0:48e6560d23a5 9 #define LEFT_ENC PTD4
gabrielkim13 0:48e6560d23a5 10 #define RIGHT_ENC PTA12
gabrielkim13 0:48e6560d23a5 11
gabrielkim13 0:48e6560d23a5 12 #define HOLES_ENC 20
gabrielkim13 0:48e6560d23a5 13
gabrielkim13 0:48e6560d23a5 14 #define DEBOUNCE_MS 5
gabrielkim13 0:48e6560d23a5 15
gabrielkim13 0:48e6560d23a5 16 /* HC-SR04 Defines */
gabrielkim13 0:48e6560d23a5 17 #define TRIG_PIN PTD7
gabrielkim13 0:48e6560d23a5 18 #define ECHO_PIN PTD6
gabrielkim13 0:48e6560d23a5 19
gabrielkim13 0:48e6560d23a5 20 #define REFRESH 0.050
gabrielkim13 0:48e6560d23a5 21 #define TIMEOUT 0.100
gabrielkim13 0:48e6560d23a5 22
gabrielkim13 0:48e6560d23a5 23 #define N_AVERAGES 10
gabrielkim13 0:48e6560d23a5 24
gabrielkim13 0:48e6560d23a5 25 /* L298 Defines */
gabrielkim13 0:48e6560d23a5 26 #define LEFTA_PIN PTB1
gabrielkim13 0:48e6560d23a5 27 #define LEFTB_PIN PTB0
gabrielkim13 0:48e6560d23a5 28 #define RIGHTA_PIN PTB2
gabrielkim13 0:48e6560d23a5 29 #define RIGHTB_PIN PTB3
gabrielkim13 0:48e6560d23a5 30
gabrielkim13 0:48e6560d23a5 31 #define LEFT 0
gabrielkim13 0:48e6560d23a5 32 #define RIGHT 1
gabrielkim13 0:48e6560d23a5 33
gabrielkim13 0:48e6560d23a5 34 #define PWM_FREQUENCY 1000.0
gabrielkim13 0:48e6560d23a5 35 #define PWM_STEP 0.1
gabrielkim13 0:48e6560d23a5 36 #define PWM_DELAY_US 50
gabrielkim13 0:48e6560d23a5 37
gabrielkim13 0:48e6560d23a5 38 /* nRF24L01 Defines */
gabrielkim13 0:48e6560d23a5 39 #define MOSI_PIN PTD2
gabrielkim13 0:48e6560d23a5 40 #define MISO_PIN PTD3
gabrielkim13 0:48e6560d23a5 41 #define SCK_PIN PTD1
gabrielkim13 0:48e6560d23a5 42 #define CSN_PIN PTD5
gabrielkim13 0:48e6560d23a5 43 #define CE_PIN PTD0
gabrielkim13 0:48e6560d23a5 44 #define IRQ_PIN PTA13
gabrielkim13 0:48e6560d23a5 45
gabrielkim13 0:48e6560d23a5 46 #define TRANSFER_SIZE 3
gabrielkim13 0:48e6560d23a5 47
gabrielkim13 0:48e6560d23a5 48 /* Encoder Functions */
gabrielkim13 0:48e6560d23a5 49 void leftInterrupt();
gabrielkim13 0:48e6560d23a5 50 void rightInterrupt();
gabrielkim13 0:48e6560d23a5 51
gabrielkim13 0:48e6560d23a5 52 /* HC-SR04 Functions */
gabrielkim13 0:48e6560d23a5 53 void distanceUpdate(int);
gabrielkim13 0:48e6560d23a5 54
gabrielkim13 0:48e6560d23a5 55 /* L298 Functions */
gabrielkim13 0:48e6560d23a5 56 void setSpeed(unsigned char, float);
gabrielkim13 0:48e6560d23a5 57
gabrielkim13 0:48e6560d23a5 58 #ifdef DEBUG
gabrielkim13 0:48e6560d23a5 59 Serial pc(USBTX, USBRX);
gabrielkim13 0:48e6560d23a5 60 #endif
gabrielkim13 0:48e6560d23a5 61
gabrielkim13 0:48e6560d23a5 62 /* Encoder Variables */
gabrielkim13 0:48e6560d23a5 63 InterruptIn leftEncoder(LEFT_ENC);
gabrielkim13 0:48e6560d23a5 64 InterruptIn rightEncoder(RIGHT_ENC);
gabrielkim13 0:48e6560d23a5 65
gabrielkim13 0:48e6560d23a5 66 Timer debounce;
gabrielkim13 0:48e6560d23a5 67
gabrielkim13 0:48e6560d23a5 68 unsigned char leftCounts = 0;
gabrielkim13 0:48e6560d23a5 69 unsigned char rightCounts = 0;
gabrielkim13 0:48e6560d23a5 70
gabrielkim13 0:48e6560d23a5 71 /* HC-SR04 Variables */
gabrielkim13 0:48e6560d23a5 72 ultrasonic hcsr04(TRIG_PIN, ECHO_PIN, REFRESH, TIMEOUT, &distanceUpdate);
gabrielkim13 0:48e6560d23a5 73
gabrielkim13 0:48e6560d23a5 74 unsigned long int distanceSum = 0;
gabrielkim13 0:48e6560d23a5 75 unsigned int currentDistance = 0;
gabrielkim13 0:48e6560d23a5 76 unsigned int previousDistance = 0;
gabrielkim13 0:48e6560d23a5 77 unsigned int averagedDistance = 0;
gabrielkim13 0:48e6560d23a5 78
gabrielkim13 0:48e6560d23a5 79 /* L298 Variables */
gabrielkim13 0:48e6560d23a5 80 PwmOut leftOutA(LEFTA_PIN);
gabrielkim13 0:48e6560d23a5 81 PwmOut leftOutB(LEFTB_PIN);
gabrielkim13 0:48e6560d23a5 82 PwmOut rightOutA(RIGHTA_PIN);
gabrielkim13 0:48e6560d23a5 83 PwmOut rightOutB(RIGHTB_PIN);
gabrielkim13 0:48e6560d23a5 84
gabrielkim13 0:48e6560d23a5 85 float leftSpeed = 0.0;
gabrielkim13 0:48e6560d23a5 86 float rightSpeed = 0.0;
gabrielkim13 0:48e6560d23a5 87
gabrielkim13 0:48e6560d23a5 88 /* nRF24L01 Variables */
gabrielkim13 0:48e6560d23a5 89 nRF24L01P nRF24(MOSI_PIN, MISO_PIN, SCK_PIN, CSN_PIN, CE_PIN, IRQ_PIN);
gabrielkim13 0:48e6560d23a5 90
gabrielkim13 0:48e6560d23a5 91 /* Main */
gabrielkim13 0:48e6560d23a5 92 int main() {
gabrielkim13 0:48e6560d23a5 93 signed char txData[TRANSFER_SIZE], rxData[TRANSFER_SIZE];
gabrielkim13 0:48e6560d23a5 94
gabrielkim13 0:48e6560d23a5 95 // Encoder initialization
gabrielkim13 0:48e6560d23a5 96 // leftEncoder.rise(&leftInterrupt);
gabrielkim13 0:48e6560d23a5 97 // rightEncoder.rise(&rightInterrupt);
gabrielkim13 0:48e6560d23a5 98
gabrielkim13 0:48e6560d23a5 99 // HCSR04 initialization
gabrielkim13 0:48e6560d23a5 100 hcsr04.startUpdates();
gabrielkim13 0:48e6560d23a5 101
gabrielkim13 0:48e6560d23a5 102 // L298 initialization
gabrielkim13 0:48e6560d23a5 103 leftOutA.period_us(1000000.0 / PWM_FREQUENCY);
gabrielkim13 0:48e6560d23a5 104 leftOutB.period_us(1000000.0 / PWM_FREQUENCY);
gabrielkim13 0:48e6560d23a5 105 rightOutA.period_us(1000000.0 / PWM_FREQUENCY);
gabrielkim13 0:48e6560d23a5 106 rightOutB.period_us(1000000.0 / PWM_FREQUENCY);
gabrielkim13 0:48e6560d23a5 107
gabrielkim13 0:48e6560d23a5 108 // nRF24L01 initialization
gabrielkim13 0:48e6560d23a5 109 nRF24.powerUp();
gabrielkim13 0:48e6560d23a5 110 nRF24.setRfFrequency(2450);
gabrielkim13 0:48e6560d23a5 111
gabrielkim13 0:48e6560d23a5 112 #ifdef DEBUG
gabrielkim13 0:48e6560d23a5 113 pc.printf("nRF24L01+ Frequency : %d MHz\r\n", nRF24.getRfFrequency());
gabrielkim13 0:48e6560d23a5 114 pc.printf("nRF24L01+ Output power : %d dBm\r\n", nRF24.getRfOutputPower());
gabrielkim13 0:48e6560d23a5 115 pc.printf("nRF24L01+ Data Rate : %d kbps\r\n", nRF24.getAirDataRate());
gabrielkim13 0:48e6560d23a5 116 pc.printf("nRF24L01+ TX Address : 0x%010llX\r\n", nRF24.getTxAddress());
gabrielkim13 0:48e6560d23a5 117 pc.printf("nRF24L01+ RX Address : 0x%010llX\r\n", nRF24.getRxAddress());
gabrielkim13 0:48e6560d23a5 118 #endif
gabrielkim13 0:48e6560d23a5 119
gabrielkim13 0:48e6560d23a5 120 nRF24.setTransferSize(TRANSFER_SIZE);
gabrielkim13 0:48e6560d23a5 121
gabrielkim13 0:48e6560d23a5 122 nRF24.setReceiveMode();
gabrielkim13 0:48e6560d23a5 123 nRF24.enable();
gabrielkim13 0:48e6560d23a5 124
gabrielkim13 0:48e6560d23a5 125 // Main loop
gabrielkim13 0:48e6560d23a5 126 while(1) {
gabrielkim13 0:48e6560d23a5 127 if (averagedDistance < 10) {
gabrielkim13 0:48e6560d23a5 128 setSpeed(LEFT, 0.0);
gabrielkim13 0:48e6560d23a5 129 setSpeed(RIGHT, 0.0);
gabrielkim13 0:48e6560d23a5 130 } else {
gabrielkim13 0:48e6560d23a5 131 setSpeed(LEFT, 1.0);
gabrielkim13 0:48e6560d23a5 132 setSpeed(RIGHT, 1.0);
gabrielkim13 0:48e6560d23a5 133 }
gabrielkim13 0:48e6560d23a5 134
gabrielkim13 0:48e6560d23a5 135 #ifdef DEBUG
gabrielkim13 0:48e6560d23a5 136 pc.printf("Distance: %dcm\r\n", averagedDistance);
gabrielkim13 0:48e6560d23a5 137 #endif
gabrielkim13 0:48e6560d23a5 138
gabrielkim13 0:48e6560d23a5 139 hcsr04.checkDistance();
gabrielkim13 0:48e6560d23a5 140
gabrielkim13 0:48e6560d23a5 141 wait_us(PWM_DELAY_US);
gabrielkim13 0:48e6560d23a5 142 }
gabrielkim13 0:48e6560d23a5 143 }
gabrielkim13 0:48e6560d23a5 144
gabrielkim13 0:48e6560d23a5 145 /* Left Encoder Interrupt */
gabrielkim13 0:48e6560d23a5 146 void leftInterrupt() {
gabrielkim13 0:48e6560d23a5 147 debounce.reset();
gabrielkim13 0:48e6560d23a5 148 debounce.start();
gabrielkim13 0:48e6560d23a5 149 while (leftEncoder.read());
gabrielkim13 0:48e6560d23a5 150 debounce.stop();
gabrielkim13 0:48e6560d23a5 151
gabrielkim13 0:48e6560d23a5 152 if (debounce.read_ms() >= DEBOUNCE_MS) {
gabrielkim13 0:48e6560d23a5 153 leftCounts++;
gabrielkim13 0:48e6560d23a5 154
gabrielkim13 0:48e6560d23a5 155 if (leftCounts >= HOLES_ENC) {
gabrielkim13 0:48e6560d23a5 156 leftCounts = 0;
gabrielkim13 0:48e6560d23a5 157
gabrielkim13 0:48e6560d23a5 158 #ifdef DEBUG
gabrielkim13 0:48e6560d23a5 159 pc.printf("Left spin!\r\n");
gabrielkim13 0:48e6560d23a5 160 #endif
gabrielkim13 0:48e6560d23a5 161 }
gabrielkim13 0:48e6560d23a5 162 }
gabrielkim13 0:48e6560d23a5 163 }
gabrielkim13 0:48e6560d23a5 164
gabrielkim13 0:48e6560d23a5 165 /* Right Encoder Interrupt */
gabrielkim13 0:48e6560d23a5 166 void rightInterrupt() {
gabrielkim13 0:48e6560d23a5 167 debounce.reset();
gabrielkim13 0:48e6560d23a5 168 debounce.start();
gabrielkim13 0:48e6560d23a5 169 while (rightEncoder.read());
gabrielkim13 0:48e6560d23a5 170 debounce.stop();
gabrielkim13 0:48e6560d23a5 171
gabrielkim13 0:48e6560d23a5 172 if (debounce.read_ms() >= DEBOUNCE_MS) {
gabrielkim13 0:48e6560d23a5 173 rightCounts++;
gabrielkim13 0:48e6560d23a5 174
gabrielkim13 0:48e6560d23a5 175 if (rightCounts >= HOLES_ENC) {
gabrielkim13 0:48e6560d23a5 176 rightCounts = 0;
gabrielkim13 0:48e6560d23a5 177
gabrielkim13 0:48e6560d23a5 178 #ifdef DEBUG
gabrielkim13 0:48e6560d23a5 179 pc.printf("Right spin!\r\n");
gabrielkim13 0:48e6560d23a5 180 #endif
gabrielkim13 0:48e6560d23a5 181 }
gabrielkim13 0:48e6560d23a5 182 }
gabrielkim13 0:48e6560d23a5 183 }
gabrielkim13 0:48e6560d23a5 184
gabrielkim13 0:48e6560d23a5 185 /* HC-SR04 Interrupt */
gabrielkim13 0:48e6560d23a5 186 void distanceUpdate(int distance) {
gabrielkim13 0:48e6560d23a5 187 currentDistance = distance;
gabrielkim13 0:48e6560d23a5 188
gabrielkim13 0:48e6560d23a5 189 distanceSum -= previousDistance;
gabrielkim13 0:48e6560d23a5 190 distanceSum += currentDistance;
gabrielkim13 0:48e6560d23a5 191
gabrielkim13 0:48e6560d23a5 192 averagedDistance = distanceSum / N_AVERAGES;
gabrielkim13 0:48e6560d23a5 193
gabrielkim13 0:48e6560d23a5 194 previousDistance = currentDistance;
gabrielkim13 0:48e6560d23a5 195 }
gabrielkim13 0:48e6560d23a5 196
gabrielkim13 0:48e6560d23a5 197 /* L298 Set Speed */
gabrielkim13 0:48e6560d23a5 198 void setSpeed(unsigned char motor, float setSpeed) {
gabrielkim13 0:48e6560d23a5 199 float speed = 0.0;
gabrielkim13 0:48e6560d23a5 200
gabrielkim13 0:48e6560d23a5 201 // Check for invalid PWM values
gabrielkim13 0:48e6560d23a5 202 if (setSpeed > 1.0) {
gabrielkim13 0:48e6560d23a5 203 speed = 1.0;
gabrielkim13 0:48e6560d23a5 204 } else if (setSpeed < -1.0) {
gabrielkim13 0:48e6560d23a5 205 speed = -1.0;
gabrielkim13 0:48e6560d23a5 206 } else {
gabrielkim13 0:48e6560d23a5 207 speed = setSpeed;
gabrielkim13 0:48e6560d23a5 208 }
gabrielkim13 0:48e6560d23a5 209
gabrielkim13 0:48e6560d23a5 210 // Left Motor
gabrielkim13 0:48e6560d23a5 211 if (motor == LEFT) {
gabrielkim13 0:48e6560d23a5 212 // Limit PWM variation to fixed step
gabrielkim13 0:48e6560d23a5 213 if (speed > leftSpeed) {
gabrielkim13 0:48e6560d23a5 214 if (speed - leftSpeed > PWM_STEP) {
gabrielkim13 0:48e6560d23a5 215 leftSpeed += PWM_STEP;
gabrielkim13 0:48e6560d23a5 216 } else {
gabrielkim13 0:48e6560d23a5 217 leftSpeed = speed;
gabrielkim13 0:48e6560d23a5 218 }
gabrielkim13 0:48e6560d23a5 219 } else if (speed < leftSpeed) {
gabrielkim13 0:48e6560d23a5 220 if (leftSpeed - speed > PWM_STEP) {
gabrielkim13 0:48e6560d23a5 221 leftSpeed -= PWM_STEP;
gabrielkim13 0:48e6560d23a5 222 } else {
gabrielkim13 0:48e6560d23a5 223 leftSpeed = speed;
gabrielkim13 0:48e6560d23a5 224 }
gabrielkim13 0:48e6560d23a5 225 }
gabrielkim13 0:48e6560d23a5 226
gabrielkim13 0:48e6560d23a5 227 // Drive L298 inputs according to leftSpeed
gabrielkim13 0:48e6560d23a5 228 if (leftSpeed >= 0.0) {
gabrielkim13 0:48e6560d23a5 229 leftOutA.write(leftSpeed);
gabrielkim13 0:48e6560d23a5 230 leftOutB.write(0.0);
gabrielkim13 0:48e6560d23a5 231 } else {
gabrielkim13 0:48e6560d23a5 232 leftOutA.write(0.0);
gabrielkim13 0:48e6560d23a5 233 leftOutB.write(-leftSpeed);
gabrielkim13 0:48e6560d23a5 234 }
gabrielkim13 0:48e6560d23a5 235 // Right Motor
gabrielkim13 0:48e6560d23a5 236 } else if (motor == RIGHT) {
gabrielkim13 0:48e6560d23a5 237 // Limit PWM variation to fixed step
gabrielkim13 0:48e6560d23a5 238 if (speed > rightSpeed) {
gabrielkim13 0:48e6560d23a5 239 if (speed - rightSpeed > PWM_STEP) {
gabrielkim13 0:48e6560d23a5 240 rightSpeed += PWM_STEP;
gabrielkim13 0:48e6560d23a5 241 } else {
gabrielkim13 0:48e6560d23a5 242 rightSpeed = speed;
gabrielkim13 0:48e6560d23a5 243 }
gabrielkim13 0:48e6560d23a5 244 } else if (speed < rightSpeed) {
gabrielkim13 0:48e6560d23a5 245 if (rightSpeed - speed > PWM_STEP) {
gabrielkim13 0:48e6560d23a5 246 rightSpeed -= PWM_STEP;
gabrielkim13 0:48e6560d23a5 247 } else {
gabrielkim13 0:48e6560d23a5 248 rightSpeed = speed;
gabrielkim13 0:48e6560d23a5 249 }
gabrielkim13 0:48e6560d23a5 250 }
gabrielkim13 0:48e6560d23a5 251
gabrielkim13 0:48e6560d23a5 252 // Drive L298 inputs according to rightSpeed
gabrielkim13 0:48e6560d23a5 253 if (rightSpeed >= 0.0) {
gabrielkim13 0:48e6560d23a5 254 rightOutA.write(rightSpeed);
gabrielkim13 0:48e6560d23a5 255 rightOutB.write(0.0);
gabrielkim13 0:48e6560d23a5 256 } else {
gabrielkim13 0:48e6560d23a5 257 rightOutA.write(0.0);
gabrielkim13 0:48e6560d23a5 258 rightOutB.write(-rightSpeed);
gabrielkim13 0:48e6560d23a5 259 }
gabrielkim13 0:48e6560d23a5 260 }
gabrielkim13 0:48e6560d23a5 261 }