Robotics - ARM / Mbed 2 deprecated ARM_Robotics

Robot code for searching an object and charging at it.

Dependencies:   HCSR04 Motor mbed

main.cpp@0:15664f71b21f, 2015-05-23 (annotated)

Committer:
abdsha01
Date:
Sat May 23 14:13:09 2015 +0000
Revision:
0:15664f71b21f
Child:
1:bd88d4062c97
Draft version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
abdsha01 0:15664f71b21f 1 // Code written by:
abdsha01 0:15664f71b21f 2 // Jon Baker
abdsha01 0:15664f71b21f 3 // Alessandro Grande
abdsha01 0:15664f71b21f 4 // Abdul-Rehman Sharif
abdsha01 0:15664f71b21f 5 // Lucinda Hartfield
abdsha01 0:15664f71b21f 6
abdsha01 0:15664f71b21f 7 // Circuitry made by:
abdsha01 0:15664f71b21f 8 // Yucando Navarrete
abdsha01 0:15664f71b21f 9 // Vivekanand Gupta
abdsha01 0:15664f71b21f 10
abdsha01 0:15664f71b21f 11 // The following code will control a robotic car
abdsha01 0:15664f71b21f 12 // by detetecting an object and charging towards it
abdsha01 0:15664f71b21f 13 // it uses basic functions as:
abdsha01 0:15664f71b21f 14 // charge() - used to charge on an object detected
abdsha01 0:15664f71b21f 15 // the robot will move in a straight line
abdsha01 0:15664f71b21f 16 // until it detects the arena line where
abdsha01 0:15664f71b21f 17 // it will use reverse() to move back
abdsha01 0:15664f71b21f 18 // detect_object() - used to detect an object, the robot will
abdsha01 0:15664f71b21f 19 // move in a circle to find an object and
abdsha01 0:15664f71b21f 20 // return 1 if it finds something and return
abdsha01 0:15664f71b21f 21 // 0 if it does not find anything - the search
abdsha01 0:15664f71b21f 22 // will be carried out for 10 seconds.
abdsha01 0:15664f71b21f 23 // detect_line () - used to detect a line, it returns the following
abdsha01 0:15664f71b21f 24 // an int value as follows:
abdsha01 0:15664f71b21f 25 // 0 - if no line is detected
abdsha01 0:15664f71b21f 26 // 1 - if line detected from the front
abdsha01 0:15664f71b21f 27 // -1 - if line detected from the back
abdsha01 0:15664f71b21f 28
abdsha01 0:15664f71b21f 29 // Libraries for using the above functions and more ...
abdsha01 0:15664f71b21f 30 #include "mbed.h"
abdsha01 0:15664f71b21f 31 #include "functions.h"
abdsha01 0:15664f71b21f 32
abdsha01 0:15664f71b21f 33 // Two sensors are used to detect a line, line_sens1 and line_sens2
abdsha01 0:15664f71b21f 34 // if there is a difference between the readings of these sensors
abdsha01 0:15664f71b21f 35 // the robot has detected a line.
abdsha01 0:15664f71b21f 36 // Setting pins for line sensor
abdsha01 0:15664f71b21f 37 DigitalInOut line1(p20);
abdsha01 0:15664f71b21f 38 DigitalInOut line2(p19);
abdsha01 0:15664f71b21f 39
abdsha01 0:15664f71b21f 40 // Setting pins for motor, as follows:
abdsha01 0:15664f71b21f 41 // Example: Motor____(PWM, Forward, Reverse)
abdsha01 0:15664f71b21f 42 Motor MotorLeft(p23, p28, p27);
abdsha01 0:15664f71b21f 43 Motor MotorRight(p22, p29, p30);
abdsha01 0:15664f71b21f 44
abdsha01 0:15664f71b21f 45 // Setting pins for ultrasonic sensor, as follows:
abdsha01 0:15664f71b21f 46 // Example: usensor(Trigger, Echo)
abdsha01 0:15664f71b21f 47 HCSR04 usensor(p25,p26);
abdsha01 0:15664f71b21f 48
abdsha01 0:15664f71b21f 49 // Set for debugging purpose
abdsha01 0:15664f71b21f 50 // Example: pc(TX, RX)
abdsha01 0:15664f71b21f 51 //Serial pc(USBTX, USBRX);
abdsha01 0:15664f71b21f 52
abdsha01 0:15664f71b21f 53
abdsha01 0:15664f71b21f 54 // Global parameters
abdsha01 0:15664f71b21f 55 // Speed at which it charged an object
abdsha01 0:15664f71b21f 56 // optimum value: 0.4 to 0.8
abdsha01 0:15664f71b21f 57 float chargespeed;
abdsha01 0:15664f71b21f 58 // Speed at which it rotates to find an object
abdsha01 0:15664f71b21f 59 // optimum value: 0.3 to 0.5
abdsha01 0:15664f71b21f 60 float searchspeed;
abdsha01 0:15664f71b21f 61 // Range of detection
abdsha01 0:15664f71b21f 62 // optimum value: 30 to 50
abdsha01 0:15664f71b21f 63 int range;
abdsha01 0:15664f71b21f 64
abdsha01 0:15664f71b21f 65 void initialise()
abdsha01 0:15664f71b21f 66 {
abdsha01 0:15664f71b21f 67 chargespeed = 0.5;
abdsha01 0:15664f71b21f 68 searchspeed = 0.3;
abdsha01 0:15664f71b21f 69 range = 40;
abdsha01 0:15664f71b21f 70
abdsha01 0:15664f71b21f 71 // Wait for 5 seconds to move away from robot
abdsha01 0:15664f71b21f 72 wait(5);
abdsha01 0:15664f71b21f 73 }
abdsha01 0:15664f71b21f 74
abdsha01 0:15664f71b21f 75 // The main loop - please write your code here
abdsha01 0:15664f71b21f 76 int main()
abdsha01 0:15664f71b21f 77 {
abdsha01 0:15664f71b21f 78 // Initialise the code
abdsha01 0:15664f71b21f 79 initialise();
abdsha01 0:15664f71b21f 80
abdsha01 0:15664f71b21f 81 printf("Starting the robot...");
abdsha01 0:15664f71b21f 82
abdsha01 0:15664f71b21f 83 Timer t;
abdsha01 0:15664f71b21f 84 t.start();
abdsha01 0:15664f71b21f 85
abdsha01 0:15664f71b21f 86 while(1) { // main loop
abdsha01 0:15664f71b21f 87 if (dist <= 35 && dist >= 1) {
abdsha01 0:15664f71b21f 88 MotorLeft.speed(-0.4);
abdsha01 0:15664f71b21f 89 MotorRight.speed(-0.4);
abdsha01 0:15664f71b21f 90 Timer t3;
abdsha01 0:15664f71b21f 91
abdsha01 0:15664f71b21f 92 t3.start();
abdsha01 0:15664f71b21f 93
abdsha01 0:15664f71b21f 94 //printf("%d,%d,%d\r\n",line1val,line2val,line1val-line2val);
abdsha01 0:15664f71b21f 95
abdsha01 0:15664f71b21f 96 while (t3.read_ms() < 100) {
abdsha01 0:15664f71b21f 97
abdsha01 0:15664f71b21f 98 if (((line1val-line2val) > 50) || ((line1val-line2val) < -50)) {
abdsha01 0:15664f71b21f 99 MotorLeft.speed(0.4);
abdsha01 0:15664f71b21f 100 MotorRight.speed(0.4);
abdsha01 0:15664f71b21f 101
abdsha01 0:15664f71b21f 102 wait_ms(500);
abdsha01 0:15664f71b21f 103
abdsha01 0:15664f71b21f 104 //return 0;
abdsha01 0:15664f71b21f 105 printf("STOP\r\n");
abdsha01 0:15664f71b21f 106 }
abdsha01 0:15664f71b21f 107 //t.reset();
abdsha01 0:15664f71b21f 108 }
abdsha01 0:15664f71b21f 109
abdsha01 0:15664f71b21f 110 } else {
abdsha01 0:15664f71b21f 111 MotorLeft.speed(motorspeed);
abdsha01 0:15664f71b21f 112 MotorRight.speed(-(motorspeed));
abdsha01 0:15664f71b21f 113 }
abdsha01 0:15664f71b21f 114
abdsha01 0:15664f71b21f 115 usensor.start();
abdsha01 0:15664f71b21f 116 if (t.read_ms() >=100) {
abdsha01 0:15664f71b21f 117 dist=usensor.get_dist_cm();
abdsha01 0:15664f71b21f 118 motorspeed = -0.8;
abdsha01 0:15664f71b21f 119 if (reverse==2) {
abdsha01 0:15664f71b21f 120 motorspeed = -0.7;
abdsha01 0:15664f71b21f 121 reverse = 0;
abdsha01 0:15664f71b21f 122 //motorspeed = -(motorspeed);
abdsha01 0:15664f71b21f 123 //reverse=0;
abdsha01 0:15664f71b21f 124 } else {
abdsha01 0:15664f71b21f 125 motorspeed = 0.0;
abdsha01 0:15664f71b21f 126
abdsha01 0:15664f71b21f 127 }
abdsha01 0:15664f71b21f 128 reverse++;
abdsha01 0:15664f71b21f 129 t.reset();
abdsha01 0:15664f71b21f 130 }
abdsha01 0:15664f71b21f 131
abdsha01 0:15664f71b21f 132 }
abdsha01 0:15664f71b21f 133
abdsha01 0:15664f71b21f 134 }
abdsha01 0:15664f71b21f 135