Kevin Catherines
Work for University.
Dependencies: mbed Motor SRF05 ID12RFID Servo
Revision 0:8c8c9055046b, committed 2011-04-28
- Comitter:
- warpedkevin
- Date:
- Thu Apr 28 10:02:39 2011 +0000
- Commit message:
- Dissertation work.
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ID12RFID.lib Thu Apr 28 10:02:39 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/ID12RFID/#f04afa911cf5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motor.lib Thu Apr 28 10:02:39 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/Motor/#f265e441bcd9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SRF05.lib Thu Apr 28 10:02:39 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/SRF05/#e758665e072c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Thu Apr 28 10:02:39 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/Servo/#36b69a7ced07
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Apr 28 10:02:39 2011 +0000
@@ -0,0 +1,196 @@
+/************************************************************************************
+DERBOT CONTROLLED BY MBED - Kevin Catheines - 100081329
+Program to sweep a block of area, and look for an ID tag. Obstacle detection enabled.
+Last updated - 28/04/11
+************************************************************************************/
+//*************
+//LIBRARY LIST:
+//*************
+#include "mbed.h"
+#include "SRF05.h"
+#include "Motor.h"
+#include "Servo.h"
+#include "ID12RFID.h"
+//**********************************
+//DECLARING FUNCTIONS AND VARIABLES:
+//**********************************
+//LED's
+DigitalOut Led1 = (LED1);
+DigitalOut Led2 = (LED2);
+DigitalOut Led3 = (LED3);
+DigitalOut Led4 = (LED4);
+//Opto Sensors
+InterruptIn LeftOpto(p6);
+InterruptIn RightOpto(p5);
+//Motors (PWM stream, fwd, rev)
+Motor LeftMotor(p22, p19, p19);
+Motor RightMotor(p21, p20, p20);
+//Servo
+Servo MyServo(p23);
+//Ultrasound (Trigger, echo)
+SRF05 Srf(p25, p24);
+//ID-20 RFID Tag
+ID12RFID Rfid(p14);
+//Variables
+int Counter;
+float MyServoPWM = 0.2;
+float GlobalMotor = -0.225;
+//float GlobalSpeed = -0.15; //Global for Shaft Encoding; 0 = stationary, 1 = fully rev, -1 = fully fwd: removed do to hardware failure
+//**********
+//FUNCTIONS:
+//**********
+//Dual Motor Speed
+void MotorSpeed(float left, float right)
+{
+ LeftMotor.speed(left);
+ RightMotor.speed(right);
+}
+//Servo Rotation
+void ServoRotate(float max, float min)
+{
+ MyServo.calibrate(0.00099,90); //Servo Calibration, Range and angle
+ MyServo = MyServo + MyServoPWM; //Here onwards we are telling the servo to increment slightly
+ if (MyServo >= max || MyServo <= min) //If it reaches fully left or right facing, invert and go back
+ {
+ MyServoPWM = -MyServoPWM;
+ }
+}
+//RFID check, Wait, Servo rotation and Ultrasound Detection combined
+void RFIDCheck(float waitTime)
+ {
+ Counter = 0;
+ float Range = 15; //Servo range, in cm
+ float time = waitTime / 15; //Here is dividing the wait time in 10, so that the RFID reader
+ while (Counter < 15) //has chance to detect a tag and interrupt in time
+ {
+ ServoRotate(1,0); //In this loop we put the servo rotate, so detection and adjustment
+ wait(time); //can take place in real time
+ Counter += 1;
+ if (Rfid.readable()) //If an RFID is detected, Stop the motors and light all LED's,
+ { //until the Mbed is reset
+ while(1)
+ {
+ MotorSpeed(0,0);
+ Led1 = 1;
+ Led2 = 1;
+ Led3 = 1;
+ Led4 = 1;
+ }
+ }
+ else if (Srf.read() <= Range && MyServo < 0.2) //If object is detected, these codes will change the motor speed
+ { //depening on where the servo has the ultrasound pointing.
+ MotorSpeed(GlobalMotor,GlobalMotor*1.2);
+ wait(0.2); //For example here we can see, if something is within the range
+ MotorSpeed(GlobalMotor*1.1,GlobalMotor); //specified earlier, and the servo is before 0.2 position,
+ break; //turn right slightly
+
+ }
+ else if (Srf.read() <= Range && MyServo < 0.4 && MyServo > 0.2) //This else if is for between different parameteres,
+ { //as the RFID readeris one the right, all obstacle
+ MotorSpeed(GlobalMotor,GlobalMotor*1.4);
+ wait(0.4); //detection leads to the Derbot moving to the right
+ MotorSpeed(GlobalMotor*1.2,GlobalMotor);
+ break;
+ }
+ else if (Srf.read() <= Range && MyServo < 0.6 && MyServo > 0.4)
+ {
+ MotorSpeed(GlobalMotor*-1.2,GlobalMotor*1.6);
+ wait (0.6);
+ MotorSpeed(GlobalMotor*1.4,GlobalMotor*-1.2);
+ break; //Using Break jumps out and back into moving normally for fast obstacle avoidance
+ }
+ else if (Srf.read() <= Range && MyServo < 0.8 && MyServo > 0.6) //As the obstacle moves more to the right, the harsher
+ { //the turning to the left will be to compensate
+ MotorSpeed(GlobalMotor*-1.6,GlobalMotor*1.6);
+ wait(0.8);
+ MotorSpeed(GlobalMotor*1.4,GlobalMotor*-1.2);
+ break;
+ }
+ else if (Srf.read() <= Range && MyServo > 0.8) //This is the most interesting else if for the obstacle detection,
+ { //as at this point and obstacle is detected almost at a less than
+ MotorSpeed(0,0); //30 degree angle, the obstacle must be right next to it's right
+ MotorSpeed(0.225,-0.255); //side. This means, in case the RFID is on the other side, it must
+ wait(1.2); //scan around by goin backwards and lining up with the other
+ MotorSpeed(-0.225,-0.255); //side. This is only for small objects, and moves a fixed value
+ wait (1.0);
+ MotorSpeed(0.225,-0.255);
+ wait(1.2);
+ break;
+ }
+ }
+}
+//The code for moving in a stright line then turning right 180 degress, to start or continue the sweep.
+void StraightRight()
+{
+ MotorSpeed(GlobalMotor,GlobalMotor);
+ RFIDCheck (3.5);
+ MotorSpeed(0,0);
+ RFIDCheck (0.5);
+ MotorSpeed(-0.23,0);
+ RFIDCheck (2.2);
+ MotorSpeed(0,0);
+ RFIDCheck (0.5);
+}
+//The code for moving in a stright line then turningleft 180 degress, to continue the sweep.
+void StraightLeft()
+{
+ MotorSpeed(GlobalMotor,GlobalMotor);
+ RFIDCheck (3.5);
+ MotorSpeed(0,0);
+ RFIDCheck (0.5);
+ MotorSpeed(0,-0.255);
+ RFIDCheck (2.1);
+ MotorSpeed(0,0);
+ RFIDCheck (0.5);
+}
+/*Shaft encoder functions: Commented OUT. Due to OPTO's failing.
+//Left shaft encoder counter
+void LeftEncoder()
+{
+ Counter1 += 1; //Counter for left opto
+}
+//Right shaft encoder counter
+void RightEncoder()
+{
+ Counter2 += 1; //Counter for right opto
+}
+//Shaft encoder equaliser
+void ShaftEncoder()
+{
+ LeftOpto.rise(&LeftEncoder); //Each time a rising or falling edge is detected, increment
+ RightOpto.rise(&RightEncoder); //the relvant counters.
+ LeftOpto.fall(&LeftEncoder);
+ RightOpto.fall(&RightEncoder);
+ if (Counter1 > Counter2) //This last section determines if one of the wheels is moving
+ { //faster than the other
+ RightMotor.speed(GlobalSpeed - 0.03);
+ LeftMotor.speed(GlobalSpeed);
+ }
+ else if (Counter2 > Counter1) //If one is moving faster, it will slow its speed marginally
+ {
+ LeftMotor.speed(GlobalSpeed - 0.03);
+ RightMotor.speed(GlobalSpeed);
+ }
+ else if (Counter1 == Counter2) //Quickly the statement is reached and the wheels will be
+ { //going at equal speeds to ensure straight directional movement
+ RightMotor.speed(GlobalSpeed);
+ LeftMotor.speed(GlobalSpeed);
+ }
+}
+*/
+//******************
+//THE MAIN FUNCTION
+//******************
+int main()
+{
+ while (1) //This is simple, a loop that work continously through the movement routine, in these
+ { //routines contains all of the tasks outlined in the report.
+ wait (0.5);
+ StraightRight();
+ StraightLeft();
+ StraightRight();
+ StraightLeft();
+ StraightRight();
+ //ShaftEncoder(); //This is commented out, due to it not being used, due to component failure
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Apr 28 10:02:39 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/63bcd7ba4912