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