Robotics - ARM / Mbed 2 deprecated ARM_Robotics

Robot code for searching an object and charging at it.

Dependencies:   HCSR04 Motor mbed

functions.cpp@1:bd88d4062c97, 2015-05-23 (annotated)

Committer:
abdsha01
Date:
Sat May 23 14:30:32 2015 +0000
Revision:
1:bd88d4062c97
Parent:
0:15664f71b21f
Child:
4:0507835a3dce
Updated functions

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 0:15664f71b21f 8
abdsha01 0:15664f71b21f 9 // Returns value from the QRE1113
abdsha01 0:15664f71b21f 10 // lower numbers mean more refleacive
abdsha01 0:15664f71b21f 11 // more than 3000 means nothing was reflected.
abdsha01 0:15664f71b21f 12 int read_line1()
abdsha01 0:15664f71b21f 13 {
abdsha01 0:15664f71b21f 14 // Time to record how long input is active
abdsha01 1:bd88d4062c97 15 Timer temp_t;
abdsha01 0:15664f71b21f 16
abdsha01 0:15664f71b21f 17 // Activate the line sensor and then read
abdsha01 0:15664f71b21f 18 line1.output();
abdsha01 0:15664f71b21f 19 line1 = 1;
abdsha01 0:15664f71b21f 20 wait_us(15);
abdsha01 0:15664f71b21f 21 line1.input();
abdsha01 0:15664f71b21f 22
abdsha01 0:15664f71b21f 23 // Start timer
abdsha01 1:bd88d4062c97 24 temp_t.start();
abdsha01 0:15664f71b21f 25
abdsha01 0:15664f71b21f 26 // Time how long the input is HIGH, but quit
abdsha01 0:15664f71b21f 27 // after 1ms as nothing happens after that
abdsha01 1:bd88d4062c97 28 while (line1 == 1 && temp_t.read_us() < 1000);
abdsha01 1:bd88d4062c97 29 return temp_t.read_us();
abdsha01 0:15664f71b21f 30 }
abdsha01 0:15664f71b21f 31
abdsha01 0:15664f71b21f 32 int read_line2()
abdsha01 0:15664f71b21f 33 {
abdsha01 0:15664f71b21f 34 // Time to record how long input is active
abdsha01 1:bd88d4062c97 35 Timer temp_t;
abdsha01 0:15664f71b21f 36
abdsha01 0:15664f71b21f 37 // Activate the line sensor and then read
abdsha01 0:15664f71b21f 38 line2.output();
abdsha01 0:15664f71b21f 39 line2 = 1;
abdsha01 0:15664f71b21f 40 wait_us(15);
abdsha01 0:15664f71b21f 41 line2.input();
abdsha01 0:15664f71b21f 42
abdsha01 0:15664f71b21f 43 // Start timer
abdsha01 0:15664f71b21f 44 t.start();
abdsha01 0:15664f71b21f 45
abdsha01 0:15664f71b21f 46 // Time how long the input is HIGH, but quit
abdsha01 0:15664f71b21f 47 // after 1ms as nothing happens after that
abdsha01 1:bd88d4062c97 48 while (line2 == 1 && temp_t.read_us() < 1000);
abdsha01 0:15664f71b21f 49 return t.read_us();
abdsha01 0:15664f71b21f 50 }
abdsha01 0:15664f71b21f 51
abdsha01 0:15664f71b21f 52 int detect_line()
abdsha01 0:15664f71b21f 53 {
abdsha01 0:15664f71b21f 54 int line1val = read_line1();
abdsha01 0:15664f71b21f 55 int line2val = read_line2();
abdsha01 0:15664f71b21f 56
abdsha01 0:15664f71b21f 57 if ((line1val-line2val) > 50) {
abdsha01 0:15664f71b21f 58 printf("Line detected from front");
abdsha01 0:15664f71b21f 59 return 1;
abdsha01 0:15664f71b21f 60 } else if ((line1val-line2val) < -50) {
abdsha01 0:15664f71b21f 61 printf("Line detected from back");
abdsha01 0:15664f71b21f 62 return -1;
abdsha01 0:15664f71b21f 63 } else {
abdsha01 0:15664f71b21f 64 printf("Line not detected");
abdsha01 0:15664f71b21f 65 return 0;
abdsha01 0:15664f71b21f 66 }
abdsha01 0:15664f71b21f 67 }
abdsha01 0:15664f71b21f 68
abdsha01 0:15664f71b21f 69 void reverse()
abdsha01 0:15664f71b21f 70 {
abdsha01 0:15664f71b21f 71 printf("Reverse\n");
abdsha01 0:15664f71b21f 72 MotorLeft.speed((chargespeed));
abdsha01 0:15664f71b21f 73 MotorRight.speed(-(chargespeed));
abdsha01 0:15664f71b21f 74 wait_ms(1000);
abdsha01 0:15664f71b21f 75 return;
abdsha01 0:15664f71b21f 76 }
abdsha01 0:15664f71b21f 77
abdsha01 0:15664f71b21f 78 void reverseandturn()
abdsha01 0:15664f71b21f 79 {
abdsha01 0:15664f71b21f 80 printf("Reverse and turn\n");
abdsha01 1:bd88d4062c97 81 MotorLeft.speed((chargespeed-0.3));
abdsha01 0:15664f71b21f 82 MotorRight.speed(-(chargespeed-0.1));
abdsha01 0:15664f71b21f 83 wait_ms(2000);
abdsha01 0:15664f71b21f 84 return;
abdsha01 0:15664f71b21f 85 }
abdsha01 0:15664f71b21f 86
abdsha01 0:15664f71b21f 87 void charge()
abdsha01 0:15664f71b21f 88 {
abdsha01 0:15664f71b21f 89 MotorLeft.speed(chargespeed);
abdsha01 0:15664f71b21f 90 MotorRight.speed(chargespeed);
abdsha01 0:15664f71b21f 91 return;
abdsha01 1:bd88d4062c97 92 }
abdsha01 1:bd88d4062c97 93
abdsha01 1:bd88d4062c97 94 int detect_object(int smooth)
abdsha01 1:bd88d4062c97 95 {
abdsha01 1:bd88d4062c97 96 // Start a timer - finds an object for 10 seconds
abdsha01 1:bd88d4062c97 97 // if it doesn't find anything returns 0
abdsha01 1:bd88d4062c97 98 Timer usensor_t;
abdsha01 1:bd88d4062c97 99 usensor_t.start();
abdsha01 1:bd88d4062c97 100
abdsha01 1:bd88d4062c97 101 // Variable to store sensed value
abdsha01 1:bd88d4062c97 102 int sense;
abdsha01 1:bd88d4062c97 103
abdsha01 1:bd88d4062c97 104 while (usensor_t.readms < 10000)
abdsha01 1:bd88d4062c97 105 {
abdsha01 1:bd88d4062c97 106 // Start the ultrasonic sensor
abdsha01 1:bd88d4062c97 107 usensor.start();
abdsha01 1:bd88d4062c97 108 dist = usensor.get_dist_cm();
abdsha01 1:bd88d4062c97 109
abdsha01 1:bd88d4062c97 110 // If an object is detected based on out set range return 1
abdsha01 1:bd88d4062c97 111 if (dist <= range && dist >= 1) {
abdsha01 1:bd88d4062c97 112 sense = 1;
abdsha01 1:bd88d4062c97 113 break;
abdsha01 1:bd88d4062c97 114 } else {
abdsha01 1:bd88d4062c97 115 sense = 0;
abdsha01 1:bd88d4062c97 116 MotorLeft.speed(searchspeed);
abdsha01 1:bd88d4062c97 117 MotorRight.speed(-(searchspeed));
abdsha01 1:bd88d4062c97 118 }
abdsha01 1:bd88d4062c97 119 }
abdsha01 1:bd88d4062c97 120 return sense;
abdsha01 0:15664f71b21f 121 }