A motor control class for driving two DC motors as part of RenBuggy
Embed:
(wiki syntax)
Show/hide line numbers
MotorController.cpp
00001 /******************************************************************************* 00002 * RenBED DC Motor Drive for RenBuggy * 00003 * Copyright (c) 2014 Jon Fuge * 00004 * * 00005 * Permission is hereby granted, free of charge, to any person obtaining a copy * 00006 * of this software and associated documentation files (the "Software"), to deal* 00007 * in the Software without restriction, including without limitation the rights * 00008 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * 00009 * copies of the Software, and to permit persons to whom the Software is * 00010 * furnished to do so, subject to the following conditions: * 00011 * * 00012 * The above copyright notice and this permission notice shall be included in * 00013 * all copies or substantial portions of the Software. * 00014 * * 00015 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * 00016 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * 00017 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * 00018 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * 00019 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,* 00020 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * 00021 * THE SOFTWARE. * 00022 * * 00023 * DCMotorDrive.h * 00024 * * 00025 * V1.0 06/01/2014 First issue of code Jon Fuge * 00026 *******************************************************************************/ 00027 00028 #ifndef _DCMOTORCONTROLLER_C 00029 #define _DCMOTORCONTROLLER_C 00030 00031 #include "mbed.h" 00032 #include "MotorController.h" 00033 00034 MotorController::MotorController(PinName LMotorOut, PinName LBrakeOut, PinName LSensorIn, PinName RMotorOut, PinName RBrakeOut, PinName RSensorIn): 00035 _LeftMotor(LMotorOut, LBrakeOut, LSensorIn), _RightMotor(RMotorOut, RBrakeOut, RSensorIn) 00036 { 00037 trackomatic.attach_us(this, &MotorController::MatchSpeeds, PWM_PERIOD * 2000); 00038 LeftMotorSpeed = 0; 00039 RightMotorSpeed = 0; 00040 LeftToRightRatio = 1; 00041 ResetOdometer(); 00042 _LeftMotor.SetMotorPwmAndRevolutions(LeftMotorSpeed,0); 00043 _RightMotor.SetMotorPwmAndRevolutions(RightMotorSpeed,0); 00044 00045 /* _LeftMotor.SetMotorPwmAndRevolutions(LeftMotorFullSpeed,20); 00046 _RightMotor.SetMotorPwmAndRevolutions(RightMotorFullSpeed,20); 00047 while (_LeftMotor.GetRevolutionsLeft() != 0) {}; 00048 while (_RightMotor.GetRevolutionsLeft() != 0) {}; 00049 00050 while (_LeftMotor.ReadCaptureTime() < _RightMotor.ReadCaptureTime()) { 00051 // Left motor is faster, adjust to match right motor 00052 LeftMotorFullSpeed -= 1; 00053 _LeftMotor.SetMotorPwmAndRevolutions(LeftMotorFullSpeed,20); 00054 _RightMotor.SetMotorPwmAndRevolutions(RightMotorFullSpeed,20); 00055 _LeftMotor.ResetOdometer(); 00056 while (_LeftMotor.GetRevolutionsLeft() != 0) {}; 00057 while (_RightMotor.GetRevolutionsLeft() != 0) {}; 00058 } 00059 while (_LeftMotor.ReadCaptureTime() > _RightMotor.ReadCaptureTime()) { 00060 // Right motor is faster, adjust to match left motor 00061 RightMotorFullSpeed -= 1; 00062 _LeftMotor.SetMotorPwmAndRevolutions(LeftMotorFullSpeed,20); 00063 _RightMotor.SetMotorPwmAndRevolutions(RightMotorFullSpeed,20); 00064 _RightMotor.ResetOdometer(); 00065 while (_LeftMotor.GetRevolutionsLeft() != 0) {}; 00066 while (_RightMotor.GetRevolutionsLeft() != 0) {}; 00067 }*/ 00068 } 00069 00070 void MotorController::Forwards(int ForwardCount) 00071 { 00072 LeftMotorSpeed = 18000; 00073 RightMotorSpeed = 18000; 00074 LeftToRightRatio = 1; 00075 DistanceToTravel = ForwardCount; 00076 ResetOdometer(); 00077 _LeftMotor.SetMotorPwmAndRevolutions(LeftMotorSpeed,ForwardCount); 00078 _RightMotor.SetMotorPwmAndRevolutions(RightMotorSpeed,ForwardCount); 00079 } 00080 00081 void MotorController::UpdatePwms(void) 00082 { 00083 _LeftMotor.SetMotorPwm(LeftMotorSpeed); 00084 _RightMotor.SetMotorPwm(RightMotorSpeed); 00085 } 00086 00087 void MotorController::ResetOdometer(void) 00088 { 00089 _LeftMotor.ResetOdometer(); 00090 _RightMotor.ResetOdometer(); 00091 } 00092 00093 int MotorController::ReadOdometer(void) 00094 { 00095 return((_LeftMotor.ReadOdometer() + _RightMotor.ReadOdometer())/2); 00096 } 00097 00098 int MotorController::ReadLeftOdometer(void) 00099 { 00100 return(_LeftMotor.ReadOdometer()); 00101 } 00102 00103 int MotorController::ReadRightOdometer(void) 00104 { 00105 return(_RightMotor.ReadOdometer()); 00106 } 00107 00108 void MotorController::MatchSpeeds(void) 00109 { 00110 if (ReadOdometer() >= DistanceToTravel) 00111 { 00112 LeftMotorSpeed = 0; 00113 RightMotorSpeed = 0; 00114 _LeftMotor.SetMotorPwmAndRevolutions(LeftMotorSpeed,0); 00115 _RightMotor.SetMotorPwmAndRevolutions(RightMotorSpeed,0); 00116 } 00117 else 00118 { 00119 if (_LeftMotor.ReadOdometer() < _RightMotor.ReadOdometer()) 00120 { 00121 LeftMotorSpeed+=10; 00122 RightMotorSpeed-=10; 00123 UpdatePwms(); 00124 } else 00125 { 00126 LeftMotorSpeed-=10; 00127 RightMotorSpeed+=10; 00128 UpdatePwms(); 00129 } 00130 } 00131 } 00132 00133 #endif
Generated on Sat Jul 16 2022 18:57:57 by 1.7.2