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
wheel.cpp
00001 #include <math.h> 00002 #include "wheel.h" 00003 00004 00005 Wheel::Wheel(PinName pwm, PinName enable, PinName direction, PinName stop) : m_pwm(pwm), m_enable(enable), m_direction(direction), m_stop(stop) 00006 { 00007 // Constructor 00008 m_enable = 0; // Disable 00009 m_stop = 1; // Stopped 00010 00011 // Set PWM period and duty cycle 00012 m_pwm.period(period); 00013 m_pwm = 0.1f; // Initial Duty Cycle 10% 00014 } 00015 00016 void Wheel::Enable() 00017 { 00018 m_enable = 1; 00019 } 00020 00021 void Wheel::Disable() 00022 { 00023 m_enable = 0; 00024 } 00025 00026 void Wheel::Stop() 00027 { 00028 m_stop = 1; 00029 } 00030 00031 void Wheel::Start() 00032 { 00033 m_stop = 0; 00034 m_direction = forwardDir; 00035 } 00036 00037 void Wheel::Reverse() 00038 { 00039 Start(); 00040 Speed(-0.1); 00041 } 00042 00043 void Wheel::setForwardDirection(int dir) 00044 { 00045 forwardDir = dir; 00046 reverseDir = !dir; 00047 } 00048 00049 void Wheel::Speed(float speed) // Speed between [-1, 1] 00050 { 00051 /* Clamp speed for safety. If speed is out of range, 00052 * ESCON controller will run into an error. 00053 * 00054 */ 00055 if (speed > 1.0) 00056 { 00057 speed = 1.0f; 00058 } 00059 00060 if (speed < -1.0) 00061 { 00062 speed = -1.0f; 00063 } 00064 00065 // Set appropriate direction 00066 if (speed >= 0) 00067 { 00068 m_direction = forwardDir; 00069 } 00070 else 00071 { 00072 m_direction = reverseDir; 00073 } 00074 00075 // Calculate and set duty 00076 m_pwm = (abs(speed) * (0.9f - 0.1f)) + 0.1f; 00077 } 00078 00079
Generated on Fri Jul 22 2022 11:40:57 by
1.7.2