Kevin Catherines / Mbed 2 deprecated MyFinalDerbot

Work for University.

Dependencies:   mbed Motor SRF05 ID12RFID Servo

main.cpp@0:8c8c9055046b, 2011-04-28 (annotated)

Committer:
warpedkevin
Date:
Thu Apr 28 10:02:39 2011 +0000
Revision:
0:8c8c9055046b
Dissertation work.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
warpedkevin 0:8c8c9055046b 1 /************************************************************************************
warpedkevin 0:8c8c9055046b 2 DERBOT CONTROLLED BY MBED - Kevin Catheines - 100081329
warpedkevin 0:8c8c9055046b 3 Program to sweep a block of area, and look for an ID tag. Obstacle detection enabled.
warpedkevin 0:8c8c9055046b 4 Last updated - 28/04/11
warpedkevin 0:8c8c9055046b 5 ************************************************************************************/
warpedkevin 0:8c8c9055046b 6 //*************
warpedkevin 0:8c8c9055046b 7 //LIBRARY LIST:
warpedkevin 0:8c8c9055046b 8 //*************
warpedkevin 0:8c8c9055046b 9 #include "mbed.h"
warpedkevin 0:8c8c9055046b 10 #include "SRF05.h"
warpedkevin 0:8c8c9055046b 11 #include "Motor.h"
warpedkevin 0:8c8c9055046b 12 #include "Servo.h"
warpedkevin 0:8c8c9055046b 13 #include "ID12RFID.h"
warpedkevin 0:8c8c9055046b 14 //**********************************
warpedkevin 0:8c8c9055046b 15 //DECLARING FUNCTIONS AND VARIABLES:
warpedkevin 0:8c8c9055046b 16 //**********************************
warpedkevin 0:8c8c9055046b 17 //LED's
warpedkevin 0:8c8c9055046b 18 DigitalOut Led1 = (LED1);
warpedkevin 0:8c8c9055046b 19 DigitalOut Led2 = (LED2);
warpedkevin 0:8c8c9055046b 20 DigitalOut Led3 = (LED3);
warpedkevin 0:8c8c9055046b 21 DigitalOut Led4 = (LED4);
warpedkevin 0:8c8c9055046b 22 //Opto Sensors
warpedkevin 0:8c8c9055046b 23 InterruptIn LeftOpto(p6);
warpedkevin 0:8c8c9055046b 24 InterruptIn RightOpto(p5);
warpedkevin 0:8c8c9055046b 25 //Motors (PWM stream, fwd, rev)
warpedkevin 0:8c8c9055046b 26 Motor LeftMotor(p22, p19, p19);
warpedkevin 0:8c8c9055046b 27 Motor RightMotor(p21, p20, p20);
warpedkevin 0:8c8c9055046b 28 //Servo
warpedkevin 0:8c8c9055046b 29 Servo MyServo(p23);
warpedkevin 0:8c8c9055046b 30 //Ultrasound (Trigger, echo)
warpedkevin 0:8c8c9055046b 31 SRF05 Srf(p25, p24);
warpedkevin 0:8c8c9055046b 32 //ID-20 RFID Tag
warpedkevin 0:8c8c9055046b 33 ID12RFID Rfid(p14);
warpedkevin 0:8c8c9055046b 34 //Variables
warpedkevin 0:8c8c9055046b 35 int Counter;
warpedkevin 0:8c8c9055046b 36 float MyServoPWM = 0.2;
warpedkevin 0:8c8c9055046b 37 float GlobalMotor = -0.225;
warpedkevin 0:8c8c9055046b 38 //float GlobalSpeed = -0.15; //Global for Shaft Encoding; 0 = stationary, 1 = fully rev, -1 = fully fwd: removed do to hardware failure
warpedkevin 0:8c8c9055046b 39 //**********
warpedkevin 0:8c8c9055046b 40 //FUNCTIONS:
warpedkevin 0:8c8c9055046b 41 //**********
warpedkevin 0:8c8c9055046b 42 //Dual Motor Speed
warpedkevin 0:8c8c9055046b 43 void MotorSpeed(float left, float right)
warpedkevin 0:8c8c9055046b 44 {
warpedkevin 0:8c8c9055046b 45 LeftMotor.speed(left);
warpedkevin 0:8c8c9055046b 46 RightMotor.speed(right);
warpedkevin 0:8c8c9055046b 47 }
warpedkevin 0:8c8c9055046b 48 //Servo Rotation
warpedkevin 0:8c8c9055046b 49 void ServoRotate(float max, float min)
warpedkevin 0:8c8c9055046b 50 {
warpedkevin 0:8c8c9055046b 51 MyServo.calibrate(0.00099,90); //Servo Calibration, Range and angle
warpedkevin 0:8c8c9055046b 52 MyServo = MyServo + MyServoPWM; //Here onwards we are telling the servo to increment slightly
warpedkevin 0:8c8c9055046b 53 if (MyServo >= max || MyServo <= min) //If it reaches fully left or right facing, invert and go back
warpedkevin 0:8c8c9055046b 54 {
warpedkevin 0:8c8c9055046b 55 MyServoPWM = -MyServoPWM;
warpedkevin 0:8c8c9055046b 56 }
warpedkevin 0:8c8c9055046b 57 }
warpedkevin 0:8c8c9055046b 58 //RFID check, Wait, Servo rotation and Ultrasound Detection combined
warpedkevin 0:8c8c9055046b 59 void RFIDCheck(float waitTime)
warpedkevin 0:8c8c9055046b 60 {
warpedkevin 0:8c8c9055046b 61 Counter = 0;
warpedkevin 0:8c8c9055046b 62 float Range = 15; //Servo range, in cm
warpedkevin 0:8c8c9055046b 63 float time = waitTime / 15; //Here is dividing the wait time in 10, so that the RFID reader
warpedkevin 0:8c8c9055046b 64 while (Counter < 15) //has chance to detect a tag and interrupt in time
warpedkevin 0:8c8c9055046b 65 {
warpedkevin 0:8c8c9055046b 66 ServoRotate(1,0); //In this loop we put the servo rotate, so detection and adjustment
warpedkevin 0:8c8c9055046b 67 wait(time); //can take place in real time
warpedkevin 0:8c8c9055046b 68 Counter += 1;
warpedkevin 0:8c8c9055046b 69 if (Rfid.readable()) //If an RFID is detected, Stop the motors and light all LED's,
warpedkevin 0:8c8c9055046b 70 { //until the Mbed is reset
warpedkevin 0:8c8c9055046b 71 while(1)
warpedkevin 0:8c8c9055046b 72 {
warpedkevin 0:8c8c9055046b 73 MotorSpeed(0,0);
warpedkevin 0:8c8c9055046b 74 Led1 = 1;
warpedkevin 0:8c8c9055046b 75 Led2 = 1;
warpedkevin 0:8c8c9055046b 76 Led3 = 1;
warpedkevin 0:8c8c9055046b 77 Led4 = 1;
warpedkevin 0:8c8c9055046b 78 }
warpedkevin 0:8c8c9055046b 79 }
warpedkevin 0:8c8c9055046b 80 else if (Srf.read() <= Range && MyServo < 0.2) //If object is detected, these codes will change the motor speed
warpedkevin 0:8c8c9055046b 81 { //depening on where the servo has the ultrasound pointing.
warpedkevin 0:8c8c9055046b 82 MotorSpeed(GlobalMotor,GlobalMotor*1.2);
warpedkevin 0:8c8c9055046b 83 wait(0.2); //For example here we can see, if something is within the range
warpedkevin 0:8c8c9055046b 84 MotorSpeed(GlobalMotor*1.1,GlobalMotor); //specified earlier, and the servo is before 0.2 position,
warpedkevin 0:8c8c9055046b 85 break; //turn right slightly
warpedkevin 0:8c8c9055046b 86
warpedkevin 0:8c8c9055046b 87 }
warpedkevin 0:8c8c9055046b 88 else if (Srf.read() <= Range && MyServo < 0.4 && MyServo > 0.2) //This else if is for between different parameteres,
warpedkevin 0:8c8c9055046b 89 { //as the RFID readeris one the right, all obstacle
warpedkevin 0:8c8c9055046b 90 MotorSpeed(GlobalMotor,GlobalMotor*1.4);
warpedkevin 0:8c8c9055046b 91 wait(0.4); //detection leads to the Derbot moving to the right
warpedkevin 0:8c8c9055046b 92 MotorSpeed(GlobalMotor*1.2,GlobalMotor);
warpedkevin 0:8c8c9055046b 93 break;
warpedkevin 0:8c8c9055046b 94 }
warpedkevin 0:8c8c9055046b 95 else if (Srf.read() <= Range && MyServo < 0.6 && MyServo > 0.4)
warpedkevin 0:8c8c9055046b 96 {
warpedkevin 0:8c8c9055046b 97 MotorSpeed(GlobalMotor*-1.2,GlobalMotor*1.6);
warpedkevin 0:8c8c9055046b 98 wait (0.6);
warpedkevin 0:8c8c9055046b 99 MotorSpeed(GlobalMotor*1.4,GlobalMotor*-1.2);
warpedkevin 0:8c8c9055046b 100 break; //Using Break jumps out and back into moving normally for fast obstacle avoidance
warpedkevin 0:8c8c9055046b 101 }
warpedkevin 0:8c8c9055046b 102 else if (Srf.read() <= Range && MyServo < 0.8 && MyServo > 0.6) //As the obstacle moves more to the right, the harsher
warpedkevin 0:8c8c9055046b 103 { //the turning to the left will be to compensate
warpedkevin 0:8c8c9055046b 104 MotorSpeed(GlobalMotor*-1.6,GlobalMotor*1.6);
warpedkevin 0:8c8c9055046b 105 wait(0.8);
warpedkevin 0:8c8c9055046b 106 MotorSpeed(GlobalMotor*1.4,GlobalMotor*-1.2);
warpedkevin 0:8c8c9055046b 107 break;
warpedkevin 0:8c8c9055046b 108 }
warpedkevin 0:8c8c9055046b 109 else if (Srf.read() <= Range && MyServo > 0.8) //This is the most interesting else if for the obstacle detection,
warpedkevin 0:8c8c9055046b 110 { //as at this point and obstacle is detected almost at a less than
warpedkevin 0:8c8c9055046b 111 MotorSpeed(0,0); //30 degree angle, the obstacle must be right next to it's right
warpedkevin 0:8c8c9055046b 112 MotorSpeed(0.225,-0.255); //side. This means, in case the RFID is on the other side, it must
warpedkevin 0:8c8c9055046b 113 wait(1.2); //scan around by goin backwards and lining up with the other
warpedkevin 0:8c8c9055046b 114 MotorSpeed(-0.225,-0.255); //side. This is only for small objects, and moves a fixed value
warpedkevin 0:8c8c9055046b 115 wait (1.0);
warpedkevin 0:8c8c9055046b 116 MotorSpeed(0.225,-0.255);
warpedkevin 0:8c8c9055046b 117 wait(1.2);
warpedkevin 0:8c8c9055046b 118 break;
warpedkevin 0:8c8c9055046b 119 }
warpedkevin 0:8c8c9055046b 120 }
warpedkevin 0:8c8c9055046b 121 }
warpedkevin 0:8c8c9055046b 122 //The code for moving in a stright line then turning right 180 degress, to start or continue the sweep.
warpedkevin 0:8c8c9055046b 123 void StraightRight()
warpedkevin 0:8c8c9055046b 124 {
warpedkevin 0:8c8c9055046b 125 MotorSpeed(GlobalMotor,GlobalMotor);
warpedkevin 0:8c8c9055046b 126 RFIDCheck (3.5);
warpedkevin 0:8c8c9055046b 127 MotorSpeed(0,0);
warpedkevin 0:8c8c9055046b 128 RFIDCheck (0.5);
warpedkevin 0:8c8c9055046b 129 MotorSpeed(-0.23,0);
warpedkevin 0:8c8c9055046b 130 RFIDCheck (2.2);
warpedkevin 0:8c8c9055046b 131 MotorSpeed(0,0);
warpedkevin 0:8c8c9055046b 132 RFIDCheck (0.5);
warpedkevin 0:8c8c9055046b 133 }
warpedkevin 0:8c8c9055046b 134 //The code for moving in a stright line then turningleft 180 degress, to continue the sweep.
warpedkevin 0:8c8c9055046b 135 void StraightLeft()
warpedkevin 0:8c8c9055046b 136 {
warpedkevin 0:8c8c9055046b 137 MotorSpeed(GlobalMotor,GlobalMotor);
warpedkevin 0:8c8c9055046b 138 RFIDCheck (3.5);
warpedkevin 0:8c8c9055046b 139 MotorSpeed(0,0);
warpedkevin 0:8c8c9055046b 140 RFIDCheck (0.5);
warpedkevin 0:8c8c9055046b 141 MotorSpeed(0,-0.255);
warpedkevin 0:8c8c9055046b 142 RFIDCheck (2.1);
warpedkevin 0:8c8c9055046b 143 MotorSpeed(0,0);
warpedkevin 0:8c8c9055046b 144 RFIDCheck (0.5);
warpedkevin 0:8c8c9055046b 145 }
warpedkevin 0:8c8c9055046b 146 /*Shaft encoder functions: Commented OUT. Due to OPTO's failing.
warpedkevin 0:8c8c9055046b 147 //Left shaft encoder counter
warpedkevin 0:8c8c9055046b 148 void LeftEncoder()
warpedkevin 0:8c8c9055046b 149 {
warpedkevin 0:8c8c9055046b 150 Counter1 += 1; //Counter for left opto
warpedkevin 0:8c8c9055046b 151 }
warpedkevin 0:8c8c9055046b 152 //Right shaft encoder counter
warpedkevin 0:8c8c9055046b 153 void RightEncoder()
warpedkevin 0:8c8c9055046b 154 {
warpedkevin 0:8c8c9055046b 155 Counter2 += 1; //Counter for right opto
warpedkevin 0:8c8c9055046b 156 }
warpedkevin 0:8c8c9055046b 157 //Shaft encoder equaliser
warpedkevin 0:8c8c9055046b 158 void ShaftEncoder()
warpedkevin 0:8c8c9055046b 159 {
warpedkevin 0:8c8c9055046b 160 LeftOpto.rise(&LeftEncoder); //Each time a rising or falling edge is detected, increment
warpedkevin 0:8c8c9055046b 161 RightOpto.rise(&RightEncoder); //the relvant counters.
warpedkevin 0:8c8c9055046b 162 LeftOpto.fall(&LeftEncoder);
warpedkevin 0:8c8c9055046b 163 RightOpto.fall(&RightEncoder);
warpedkevin 0:8c8c9055046b 164 if (Counter1 > Counter2) //This last section determines if one of the wheels is moving
warpedkevin 0:8c8c9055046b 165 { //faster than the other
warpedkevin 0:8c8c9055046b 166 RightMotor.speed(GlobalSpeed - 0.03);
warpedkevin 0:8c8c9055046b 167 LeftMotor.speed(GlobalSpeed);
warpedkevin 0:8c8c9055046b 168 }
warpedkevin 0:8c8c9055046b 169 else if (Counter2 > Counter1) //If one is moving faster, it will slow its speed marginally
warpedkevin 0:8c8c9055046b 170 {
warpedkevin 0:8c8c9055046b 171 LeftMotor.speed(GlobalSpeed - 0.03);
warpedkevin 0:8c8c9055046b 172 RightMotor.speed(GlobalSpeed);
warpedkevin 0:8c8c9055046b 173 }
warpedkevin 0:8c8c9055046b 174 else if (Counter1 == Counter2) //Quickly the statement is reached and the wheels will be
warpedkevin 0:8c8c9055046b 175 { //going at equal speeds to ensure straight directional movement
warpedkevin 0:8c8c9055046b 176 RightMotor.speed(GlobalSpeed);
warpedkevin 0:8c8c9055046b 177 LeftMotor.speed(GlobalSpeed);
warpedkevin 0:8c8c9055046b 178 }
warpedkevin 0:8c8c9055046b 179 }
warpedkevin 0:8c8c9055046b 180 */
warpedkevin 0:8c8c9055046b 181 //******************
warpedkevin 0:8c8c9055046b 182 //THE MAIN FUNCTION
warpedkevin 0:8c8c9055046b 183 //******************
warpedkevin 0:8c8c9055046b 184 int main()
warpedkevin 0:8c8c9055046b 185 {
warpedkevin 0:8c8c9055046b 186 while (1) //This is simple, a loop that work continously through the movement routine, in these
warpedkevin 0:8c8c9055046b 187 { //routines contains all of the tasks outlined in the report.
warpedkevin 0:8c8c9055046b 188 wait (0.5);
warpedkevin 0:8c8c9055046b 189 StraightRight();
warpedkevin 0:8c8c9055046b 190 StraightLeft();
warpedkevin 0:8c8c9055046b 191 StraightRight();
warpedkevin 0:8c8c9055046b 192 StraightLeft();
warpedkevin 0:8c8c9055046b 193 StraightRight();
warpedkevin 0:8c8c9055046b 194 //ShaftEncoder(); //This is commented out, due to it not being used, due to component failure
warpedkevin 0:8c8c9055046b 195 }
warpedkevin 0:8c8c9055046b 196 }