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
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 }
Generated on Fri Jul 22 2022 11:40:57 by
1.7.2