Jon Fuge / MotorController

A motor control class for driving two DC motors as part of RenBuggy

Dependents:   RenBuggy

MotorController.cpp@3:2c9ebe06b127, 2014-01-21 (annotated)

Committer:
jf1452
Date:
Tue Jan 21 13:31:11 2014 +0000
Revision:
3:2c9ebe06b127
Parent:
1:548ee2d0bb89 
Update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jf1452 0:4aa0a6b134ea 1 /*******************************************************************************
jf1452 0:4aa0a6b134ea 2 * RenBED DC Motor Drive for RenBuggy *
jf1452 0:4aa0a6b134ea 3 * Copyright (c) 2014 Jon Fuge *
jf1452 0:4aa0a6b134ea 4 * *
jf1452 0:4aa0a6b134ea 5 * Permission is hereby granted, free of charge, to any person obtaining a copy *
jf1452 0:4aa0a6b134ea 6 * of this software and associated documentation files (the "Software"), to deal*
jf1452 0:4aa0a6b134ea 7 * in the Software without restriction, including without limitation the rights *
jf1452 0:4aa0a6b134ea 8 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
jf1452 0:4aa0a6b134ea 9 * copies of the Software, and to permit persons to whom the Software is *
jf1452 0:4aa0a6b134ea 10 * furnished to do so, subject to the following conditions: *
jf1452 0:4aa0a6b134ea 11 * *
jf1452 0:4aa0a6b134ea 12 * The above copyright notice and this permission notice shall be included in *
jf1452 0:4aa0a6b134ea 13 * all copies or substantial portions of the Software. *
jf1452 0:4aa0a6b134ea 14 * *
jf1452 0:4aa0a6b134ea 15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
jf1452 0:4aa0a6b134ea 16 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
jf1452 0:4aa0a6b134ea 17 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
jf1452 0:4aa0a6b134ea 18 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
jf1452 0:4aa0a6b134ea 19 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,*
jf1452 0:4aa0a6b134ea 20 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
jf1452 0:4aa0a6b134ea 21 * THE SOFTWARE. *
jf1452 0:4aa0a6b134ea 22 * *
jf1452 0:4aa0a6b134ea 23 * DCMotorDrive.h *
jf1452 0:4aa0a6b134ea 24 * *
jf1452 0:4aa0a6b134ea 25 * V1.0 06/01/2014 First issue of code Jon Fuge *
jf1452 0:4aa0a6b134ea 26 *******************************************************************************/
jf1452 0:4aa0a6b134ea 27
jf1452 0:4aa0a6b134ea 28 #ifndef _DCMOTORCONTROLLER_C
jf1452 0:4aa0a6b134ea 29 #define _DCMOTORCONTROLLER_C
jf1452 0:4aa0a6b134ea 30
jf1452 0:4aa0a6b134ea 31 #include "mbed.h"
jf1452 0:4aa0a6b134ea 32 #include "MotorController.h"
jf1452 0:4aa0a6b134ea 33
jf1452 1:548ee2d0bb89 34 MotorController::MotorController(PinName LMotorOut, PinName LBrakeOut, PinName LSensorIn, PinName RMotorOut, PinName RBrakeOut, PinName RSensorIn):
jf1452 1:548ee2d0bb89 35 _LeftMotor(LMotorOut, LBrakeOut, LSensorIn), _RightMotor(RMotorOut, RBrakeOut, RSensorIn)
jf1452 0:4aa0a6b134ea 36 {
jf1452 1:548ee2d0bb89 37 trackomatic.attach_us(this, &MotorController::MatchSpeeds, PWM_PERIOD * 2000);
jf1452 1:548ee2d0bb89 38 LeftMotorSpeed = 0;
jf1452 1:548ee2d0bb89 39 RightMotorSpeed = 0;
jf1452 1:548ee2d0bb89 40 LeftToRightRatio = 1;
jf1452 1:548ee2d0bb89 41 ResetOdometer();
jf1452 1:548ee2d0bb89 42 _LeftMotor.SetMotorPwmAndRevolutions(LeftMotorSpeed,0);
jf1452 1:548ee2d0bb89 43 _RightMotor.SetMotorPwmAndRevolutions(RightMotorSpeed,0);
jf1452 1:548ee2d0bb89 44
jf1452 1:548ee2d0bb89 45 /* _LeftMotor.SetMotorPwmAndRevolutions(LeftMotorFullSpeed,20);
jf1452 1:548ee2d0bb89 46 _RightMotor.SetMotorPwmAndRevolutions(RightMotorFullSpeed,20);
jf1452 1:548ee2d0bb89 47 while (_LeftMotor.GetRevolutionsLeft() != 0) {};
jf1452 1:548ee2d0bb89 48 while (_RightMotor.GetRevolutionsLeft() != 0) {};
jf1452 1:548ee2d0bb89 49
jf1452 1:548ee2d0bb89 50 while (_LeftMotor.ReadCaptureTime() < _RightMotor.ReadCaptureTime()) {
jf1452 1:548ee2d0bb89 51 // Left motor is faster, adjust to match right motor
jf1452 1:548ee2d0bb89 52 LeftMotorFullSpeed -= 1;
jf1452 1:548ee2d0bb89 53 _LeftMotor.SetMotorPwmAndRevolutions(LeftMotorFullSpeed,20);
jf1452 1:548ee2d0bb89 54 _RightMotor.SetMotorPwmAndRevolutions(RightMotorFullSpeed,20);
jf1452 1:548ee2d0bb89 55 _LeftMotor.ResetOdometer();
jf1452 1:548ee2d0bb89 56 while (_LeftMotor.GetRevolutionsLeft() != 0) {};
jf1452 1:548ee2d0bb89 57 while (_RightMotor.GetRevolutionsLeft() != 0) {};
jf1452 1:548ee2d0bb89 58 }
jf1452 1:548ee2d0bb89 59 while (_LeftMotor.ReadCaptureTime() > _RightMotor.ReadCaptureTime()) {
jf1452 1:548ee2d0bb89 60 // Right motor is faster, adjust to match left motor
jf1452 1:548ee2d0bb89 61 RightMotorFullSpeed -= 1;
jf1452 1:548ee2d0bb89 62 _LeftMotor.SetMotorPwmAndRevolutions(LeftMotorFullSpeed,20);
jf1452 1:548ee2d0bb89 63 _RightMotor.SetMotorPwmAndRevolutions(RightMotorFullSpeed,20);
jf1452 1:548ee2d0bb89 64 _RightMotor.ResetOdometer();
jf1452 1:548ee2d0bb89 65 while (_LeftMotor.GetRevolutionsLeft() != 0) {};
jf1452 1:548ee2d0bb89 66 while (_RightMotor.GetRevolutionsLeft() != 0) {};
jf1452 1:548ee2d0bb89 67 }*/
jf1452 0:4aa0a6b134ea 68 }
jf1452 0:4aa0a6b134ea 69
jf1452 1:548ee2d0bb89 70 void MotorController::Forwards(int ForwardCount)
jf1452 1:548ee2d0bb89 71 {
jf1452 1:548ee2d0bb89 72 LeftMotorSpeed = 18000;
jf1452 1:548ee2d0bb89 73 RightMotorSpeed = 18000;
jf1452 1:548ee2d0bb89 74 LeftToRightRatio = 1;
jf1452 1:548ee2d0bb89 75 DistanceToTravel = ForwardCount;
jf1452 1:548ee2d0bb89 76 ResetOdometer();
jf1452 1:548ee2d0bb89 77 _LeftMotor.SetMotorPwmAndRevolutions(LeftMotorSpeed,ForwardCount);
jf1452 1:548ee2d0bb89 78 _RightMotor.SetMotorPwmAndRevolutions(RightMotorSpeed,ForwardCount);
jf1452 1:548ee2d0bb89 79 }
jf1452 1:548ee2d0bb89 80
jf1452 1:548ee2d0bb89 81 void MotorController::UpdatePwms(void)
jf1452 1:548ee2d0bb89 82 {
jf1452 1:548ee2d0bb89 83 _LeftMotor.SetMotorPwm(LeftMotorSpeed);
jf1452 1:548ee2d0bb89 84 _RightMotor.SetMotorPwm(RightMotorSpeed);
jf1452 1:548ee2d0bb89 85 }
jf1452 1:548ee2d0bb89 86
jf1452 1:548ee2d0bb89 87 void MotorController::ResetOdometer(void)
jf1452 0:4aa0a6b134ea 88 {
jf1452 0:4aa0a6b134ea 89 _LeftMotor.ResetOdometer();
jf1452 1:548ee2d0bb89 90 _RightMotor.ResetOdometer();
jf1452 0:4aa0a6b134ea 91 }
jf1452 0:4aa0a6b134ea 92
jf1452 1:548ee2d0bb89 93 int MotorController::ReadOdometer(void)
jf1452 1:548ee2d0bb89 94 {
jf1452 1:548ee2d0bb89 95 return((_LeftMotor.ReadOdometer() + _RightMotor.ReadOdometer())/2);
jf1452 1:548ee2d0bb89 96 }
jf1452 1:548ee2d0bb89 97
jf1452 1:548ee2d0bb89 98 int MotorController::ReadLeftOdometer(void)
jf1452 0:4aa0a6b134ea 99 {
jf1452 0:4aa0a6b134ea 100 return(_LeftMotor.ReadOdometer());
jf1452 0:4aa0a6b134ea 101 }
jf1452 0:4aa0a6b134ea 102
jf1452 1:548ee2d0bb89 103 int MotorController::ReadRightOdometer(void)
jf1452 0:4aa0a6b134ea 104 {
jf1452 0:4aa0a6b134ea 105 return(_RightMotor.ReadOdometer());
jf1452 0:4aa0a6b134ea 106 }
jf1452 0:4aa0a6b134ea 107
jf1452 1:548ee2d0bb89 108 void MotorController::MatchSpeeds(void)
jf1452 0:4aa0a6b134ea 109 {
jf1452 1:548ee2d0bb89 110 if (ReadOdometer() >= DistanceToTravel)
jf1452 1:548ee2d0bb89 111 {
jf1452 1:548ee2d0bb89 112 LeftMotorSpeed = 0;
jf1452 1:548ee2d0bb89 113 RightMotorSpeed = 0;
jf1452 1:548ee2d0bb89 114 _LeftMotor.SetMotorPwmAndRevolutions(LeftMotorSpeed,0);
jf1452 1:548ee2d0bb89 115 _RightMotor.SetMotorPwmAndRevolutions(RightMotorSpeed,0);
jf1452 1:548ee2d0bb89 116 }
jf1452 1:548ee2d0bb89 117 else
jf1452 1:548ee2d0bb89 118 {
jf1452 1:548ee2d0bb89 119 if (_LeftMotor.ReadOdometer() < _RightMotor.ReadOdometer())
jf1452 1:548ee2d0bb89 120 {
jf1452 1:548ee2d0bb89 121 LeftMotorSpeed+=10;
jf1452 1:548ee2d0bb89 122 RightMotorSpeed-=10;
jf1452 1:548ee2d0bb89 123 UpdatePwms();
jf1452 1:548ee2d0bb89 124 } else
jf1452 1:548ee2d0bb89 125 {
jf1452 1:548ee2d0bb89 126 LeftMotorSpeed-=10;
jf1452 1:548ee2d0bb89 127 RightMotorSpeed+=10;
jf1452 1:548ee2d0bb89 128 UpdatePwms();
jf1452 1:548ee2d0bb89 129 }
jf1452 1:548ee2d0bb89 130 }
jf1452 0:4aa0a6b134ea 131 }
jf1452 0:4aa0a6b134ea 132
jf1452 0:4aa0a6b134ea 133 #endif