Robotics - ARM / Mbed 2 deprecated ARM_Robotics

Robot code for searching an object and charging at it.

Dependencies:   HCSR04 Motor mbed

functions.cpp@4:0507835a3dce, 2015-05-23 (annotated)

Committer:
abdsha01
Date:
Sat May 23 15:05:18 2015 +0000
Revision:
4:0507835a3dce
Parent:
1:bd88d4062c97
Child:
6:af897173cb75
Improved code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
abdsha01 0:15664f71b21f 1 // The following file implements all the functions necessary
abdsha01 0:15664f71b21f 2 // for the robot.
abdsha01 0:15664f71b21f 3 // Please don't modify any of these functions!
abdsha01 0:15664f71b21f 4
abdsha01 0:15664f71b21f 5 #include "mbed.h"
abdsha01 0:15664f71b21f 6 #include "Motor.h"
abdsha01 0:15664f71b21f 7 #include "hcsr04.h"
abdsha01 4:0507835a3dce 8 #include "functions.h"
abdsha01 4:0507835a3dce 9
abdsha01 4:0507835a3dce 10 // Two sensors are used to detect a line, line_sens1 and line_sens2
abdsha01 4:0507835a3dce 11 // if there is a difference between the readings of these sensors
abdsha01 4:0507835a3dce 12 // the robot has detected a line.
abdsha01 4:0507835a3dce 13 // Setting pins for line sensor
abdsha01 4:0507835a3dce 14 DigitalInOut line1(p20);
abdsha01 4:0507835a3dce 15 DigitalInOut line2(p19);
abdsha01 4:0507835a3dce 16
abdsha01 4:0507835a3dce 17 // Setting pins for motor, as follows:
abdsha01 4:0507835a3dce 18 // Example: Motor____(PWM, Forward, Reverse)
abdsha01 4:0507835a3dce 19 Motor MotorLeft(p23, p28, p27);
abdsha01 4:0507835a3dce 20 Motor MotorRight(p22, p29, p30);
abdsha01 4:0507835a3dce 21
abdsha01 4:0507835a3dce 22 // Setting pins for ultrasonic sensor, as follows:
abdsha01 4:0507835a3dce 23 // Example: usensor(Trigger, Echo)
abdsha01 4:0507835a3dce 24 HCSR04 usensor(p25,p26);
abdsha01 0:15664f71b21f 25
abdsha01 0:15664f71b21f 26 // Returns value from the QRE1113
abdsha01 0:15664f71b21f 27 // lower numbers mean more refleacive
abdsha01 0:15664f71b21f 28 // more than 3000 means nothing was reflected.
abdsha01 0:15664f71b21f 29 int read_line1()
abdsha01 0:15664f71b21f 30 {
abdsha01 0:15664f71b21f 31 // Time to record how long input is active
abdsha01 1:bd88d4062c97 32 Timer temp_t;
abdsha01 0:15664f71b21f 33
abdsha01 0:15664f71b21f 34 // Activate the line sensor and then read
abdsha01 0:15664f71b21f 35 line1.output();
abdsha01 0:15664f71b21f 36 line1 = 1;
abdsha01 0:15664f71b21f 37 wait_us(15);
abdsha01 0:15664f71b21f 38 line1.input();
abdsha01 0:15664f71b21f 39
abdsha01 0:15664f71b21f 40 // Start timer
abdsha01 1:bd88d4062c97 41 temp_t.start();
abdsha01 0:15664f71b21f 42
abdsha01 0:15664f71b21f 43 // Time how long the input is HIGH, but quit
abdsha01 0:15664f71b21f 44 // after 1ms as nothing happens after that
abdsha01 1:bd88d4062c97 45 while (line1 == 1 && temp_t.read_us() < 1000);
abdsha01 1:bd88d4062c97 46 return temp_t.read_us();
abdsha01 0:15664f71b21f 47 }
abdsha01 0:15664f71b21f 48
abdsha01 0:15664f71b21f 49 int read_line2()
abdsha01 0:15664f71b21f 50 {
abdsha01 0:15664f71b21f 51 // Time to record how long input is active
abdsha01 1:bd88d4062c97 52 Timer temp_t;
abdsha01 0:15664f71b21f 53
abdsha01 0:15664f71b21f 54 // Activate the line sensor and then read
abdsha01 0:15664f71b21f 55 line2.output();
abdsha01 0:15664f71b21f 56 line2 = 1;
abdsha01 0:15664f71b21f 57 wait_us(15);
abdsha01 0:15664f71b21f 58 line2.input();
abdsha01 0:15664f71b21f 59
abdsha01 0:15664f71b21f 60 // Start timer
abdsha01 4:0507835a3dce 61 temp_t.start();
abdsha01 0:15664f71b21f 62
abdsha01 0:15664f71b21f 63 // Time how long the input is HIGH, but quit
abdsha01 0:15664f71b21f 64 // after 1ms as nothing happens after that
abdsha01 1:bd88d4062c97 65 while (line2 == 1 && temp_t.read_us() < 1000);
abdsha01 4:0507835a3dce 66 return temp_t.read_us();
abdsha01 0:15664f71b21f 67 }
abdsha01 0:15664f71b21f 68
abdsha01 0:15664f71b21f 69 int detect_line()
abdsha01 0:15664f71b21f 70 {
abdsha01 0:15664f71b21f 71 int line1val = read_line1();
abdsha01 0:15664f71b21f 72 int line2val = read_line2();
abdsha01 0:15664f71b21f 73
abdsha01 0:15664f71b21f 74 if ((line1val-line2val) > 50) {
abdsha01 0:15664f71b21f 75 printf("Line detected from front");
abdsha01 0:15664f71b21f 76 return 1;
abdsha01 0:15664f71b21f 77 } else if ((line1val-line2val) < -50) {
abdsha01 0:15664f71b21f 78 printf("Line detected from back");
abdsha01 0:15664f71b21f 79 return -1;
abdsha01 0:15664f71b21f 80 } else {
abdsha01 0:15664f71b21f 81 printf("Line not detected");
abdsha01 0:15664f71b21f 82 return 0;
abdsha01 0:15664f71b21f 83 }
abdsha01 0:15664f71b21f 84 }
abdsha01 0:15664f71b21f 85
abdsha01 4:0507835a3dce 86 void reverse(float speed)
abdsha01 0:15664f71b21f 87 {
abdsha01 0:15664f71b21f 88 printf("Reverse\n");
abdsha01 4:0507835a3dce 89 MotorLeft.speed((speed));
abdsha01 4:0507835a3dce 90 MotorRight.speed(-(speed));
abdsha01 0:15664f71b21f 91 wait_ms(1000);
abdsha01 0:15664f71b21f 92 return;
abdsha01 0:15664f71b21f 93 }
abdsha01 0:15664f71b21f 94
abdsha01 4:0507835a3dce 95 void reverseandturn(float speed)
abdsha01 0:15664f71b21f 96 {
abdsha01 0:15664f71b21f 97 printf("Reverse and turn\n");
abdsha01 4:0507835a3dce 98 MotorLeft.speed((speed-0.3));
abdsha01 4:0507835a3dce 99 MotorRight.speed(-(speed-0.1));
abdsha01 0:15664f71b21f 100 wait_ms(2000);
abdsha01 0:15664f71b21f 101 return;
abdsha01 0:15664f71b21f 102 }
abdsha01 0:15664f71b21f 103
abdsha01 4:0507835a3dce 104 void charge(float speed)
abdsha01 0:15664f71b21f 105 {
abdsha01 4:0507835a3dce 106 MotorLeft.speed(speed);
abdsha01 4:0507835a3dce 107 MotorRight.speed(speed);
abdsha01 0:15664f71b21f 108 return;
abdsha01 1:bd88d4062c97 109 }
abdsha01 1:bd88d4062c97 110
abdsha01 4:0507835a3dce 111 void stop()
abdsha01 4:0507835a3dce 112 {
abdsha01 4:0507835a3dce 113 MotorLeft.speed(0.0);
abdsha01 4:0507835a3dce 114 MotorRight.speed(0.0);
abdsha01 4:0507835a3dce 115 return;
abdsha01 4:0507835a3dce 116 }
abdsha01 4:0507835a3dce 117
abdsha01 4:0507835a3dce 118 int detect_object(int range, float speed)
abdsha01 1:bd88d4062c97 119 {
abdsha01 1:bd88d4062c97 120 // Start a timer - finds an object for 10 seconds
abdsha01 1:bd88d4062c97 121 // if it doesn't find anything returns 0
abdsha01 1:bd88d4062c97 122 Timer usensor_t;
abdsha01 1:bd88d4062c97 123 usensor_t.start();
abdsha01 1:bd88d4062c97 124
abdsha01 1:bd88d4062c97 125 // Variable to store sensed value
abdsha01 4:0507835a3dce 126 int sense, dist;
abdsha01 4:0507835a3dce 127 sense = 0;
abdsha01 4:0507835a3dce 128 dist = 0;
abdsha01 1:bd88d4062c97 129
abdsha01 4:0507835a3dce 130 while (usensor_t.read_ms() < 10000)
abdsha01 1:bd88d4062c97 131 {
abdsha01 1:bd88d4062c97 132 // Start the ultrasonic sensor
abdsha01 1:bd88d4062c97 133 usensor.start();
abdsha01 1:bd88d4062c97 134 dist = usensor.get_dist_cm();
abdsha01 1:bd88d4062c97 135
abdsha01 1:bd88d4062c97 136 // If an object is detected based on out set range return 1
abdsha01 1:bd88d4062c97 137 if (dist <= range && dist >= 1) {
abdsha01 1:bd88d4062c97 138 sense = 1;
abdsha01 1:bd88d4062c97 139 break;
abdsha01 1:bd88d4062c97 140 } else {
abdsha01 1:bd88d4062c97 141 sense = 0;
abdsha01 4:0507835a3dce 142 MotorLeft.speed(speed);
abdsha01 4:0507835a3dce 143 MotorRight.speed(-(speed));
abdsha01 1:bd88d4062c97 144 }
abdsha01 1:bd88d4062c97 145 }
abdsha01 1:bd88d4062c97 146 return sense;
abdsha01 0:15664f71b21f 147 }