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 wheel.cpp Source File

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