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?
User | Revision | Line number | New 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 | } |