Implementation of the fuzzy control algorithm for obstacle avoidance. This algorithm is for a two-wheel differential drive robot. It uses distance information from 3 analogue IR range sensors and controls DC motors using ESCON servo controllers. This C++ library is written for mbed LPC1768. For any questions and/or reporting bugs contact author: vans.edw@gmail.com

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers robot.cpp Source File

robot.cpp

00001 #include "robot.h"
00002 
00003 Robot::Robot(PinName StopPin, const float speed) : stop(StopPin), m_speed(speed)
00004 {
00005     // Constructor
00006     //Sensor::voltage[] = {2.75,2.55,2.00,1.55,1.25,1.15,0.90,0.80,0.75,0.65,0.60,0.55,0.50,0.455,0.45};
00007     //Sensor::meters[] = {0.15,0.20,0.30,0.40,0.50,0.60,0.70,0.80,0.90,1.00,1.10,1.20,1.30,1.40,1.50};
00008     R = 0.0;
00009 
00010     controller = new fuzzylogic(3,3,3,7);
00011     leftWheel = new Wheel(p21, p22, p23, p24);
00012     rightWheel = new Wheel(p25, p26, p27, p28);
00013     left = new Sensor(p20);
00014     left1 = new Sensor(p17);
00015     middle = new Sensor(p19);
00016     right = new Sensor(p18);
00017     right1 = new Sensor(p16);
00018 
00019     InitializeWheels();
00020     InitializeController();
00021 }
00022 
00023 Robot::~Robot()
00024 {
00025     delete controller;
00026     delete leftWheel;
00027     delete rightWheel;
00028     delete left;
00029     delete middle;
00030     delete right;
00031 }
00032 
00033 float Robot::minimum(float a, float b)
00034 {
00035     if (a > b)
00036     {
00037         return b;
00038     }
00039     else
00040     {
00041         return a;
00042     }
00043 }
00044 
00045 void Robot::AvoidObstacles()
00046 {
00047     /* In a continuous loop:
00048      * 1. Read from sensors the distance 
00049      * 2. Pass the distances into the fuzzy controller
00050      * 3. Using output of fuzzy controller control the wheels
00051      *
00052      */
00053     Steering(controller->evalfis((minimum(left->getDist(), left1->getDist()) - 0.15), (middle->getDist() - 0.15), (minimum(right->getDist(), right1->getDist()) - 0.15)));
00054 
00055 }
00056 
00057 void Robot::RandomWandering()
00058 {
00059     // Change randomly the FRB of Fuzzy Controller
00060     controller->setRuleBase();
00061 
00062 }
00063 
00064 
00065 
00066 void Robot::Stop()
00067 {
00068     leftWheel->Stop();
00069     rightWheel->Stop();
00070 }
00071 
00072 void Robot::Start()
00073 {
00074     leftWheel->Start();
00075     rightWheel->Start();
00076 }
00077 
00078 void Robot::EnableWheels()
00079 {
00080     leftWheel->Enable();
00081     rightWheel->Enable();
00082 }
00083 
00084 void Robot::DisableWheels()
00085 {
00086     leftWheel->Disable();
00087     rightWheel->Disable();
00088 }
00089 
00090 void Robot::Reverse()
00091 {
00092     // Coding for reverse
00093     // Put direction reverse and start wheels
00094     leftWheel->Reverse();
00095     rightWheel->Reverse();
00096 }
00097 
00098 void Robot::Steering(float steeringAngle)
00099 {
00100     // Coding to achieve steering. 
00101     if(steeringAngle == 0)           //no steering angle
00102     {
00103         leftWheel->Speed(m_speed);
00104         rightWheel->Speed(m_speed);
00105     }
00106     else if (steeringAngle < 0)
00107     {
00108         rightWheel->Speed(m_speed);
00109         R = fabs(length/(tan(steeringAngle)));
00110         leftWheel->Speed(((R - (wheelbase/2.0))*m_speed)/(R + (wheelbase/2.0)));
00111     }
00112     else if (steeringAngle > 0)
00113     {
00114         leftWheel->Speed(m_speed);
00115         R = fabs(length/(tan(steeringAngle)));
00116         rightWheel->Speed(((R - (wheelbase/2.0))*m_speed)/(R + (wheelbase/2.0)));
00117     }
00118 }
00119 
00120 void Robot::Run()
00121 {
00122 
00123     EnableWheels();
00124     Start();
00125 
00126     while (true)
00127     {
00128         AvoidObstacles();
00129         wait(0.05);
00130 
00131         if (stop)
00132         {
00133             break;
00134         }
00135     }
00136 
00137     Stop();
00138     DisableWheels();
00139 }
00140 
00141 void Robot::InitializeController()
00142 {
00143     double inP[3] = {0.19, 0.34, 1.00};
00144     double outP[7] = {-1.00, -0.90, -0.29, 0.00, 0.29, 0.90, 1.00};
00145     double insf = 1.00/Sensor::range;
00146 
00147     controller->setInParam(inP, inP, inP);
00148     controller->setOutParam(outP);
00149     controller->setRuleBase();
00150     controller->setScalingFactor(insf, insf, insf, 1.44);
00151 }
00152 
00153 void Robot::InitializeWheels()
00154 {
00155     leftWheel->setForwardDirection(1);
00156     rightWheel->setForwardDirection(0);
00157 }