Work for University.
Dependencies: mbed Motor SRF05 ID12RFID Servo
main.cpp
- Committer:
- warpedkevin
- Date:
- 2011-04-28
- Revision:
- 0:8c8c9055046b
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 } }