Diff: main.cpp

Revision:

0:8c8c9055046b

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