File content as of revision 0:8c8c9055046b:

/************************************************************************************
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
    }
}