File content as of revision 3:2c9ebe06b127:

/*******************************************************************************
* RenBED DC Motor Drive for RenBuggy                                           *
* Copyright (c) 2014 Jon Fuge                                                  *
*                                                                              *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal*
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell    *
* copies of the Software, and to permit persons to whom the Software is        *
* furnished to do so, subject to the following conditions:                     *
*                                                                              *
* The above copyright notice and this permission notice shall be included in   *
* all copies or substantial portions of the Software.                          *
*                                                                              *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR   *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,     *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE  *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER       *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,*
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN    *
* THE SOFTWARE.                                                                *
*                                                                              *
* DCMotorDrive.h                                                               *
*                                                                              *
* V1.0 06/01/2014 First issue of code                      Jon Fuge            *
*******************************************************************************/

#ifndef _DCMOTORCONTROLLER_C
#define _DCMOTORCONTROLLER_C

#include "mbed.h"
#include "MotorController.h"

MotorController::MotorController(PinName LMotorOut, PinName LBrakeOut, PinName LSensorIn, PinName RMotorOut, PinName RBrakeOut, PinName RSensorIn):
    _LeftMotor(LMotorOut, LBrakeOut, LSensorIn), _RightMotor(RMotorOut, RBrakeOut, RSensorIn)
{
    trackomatic.attach_us(this, &MotorController::MatchSpeeds, PWM_PERIOD * 2000);
    LeftMotorSpeed = 0;
    RightMotorSpeed = 0;
    LeftToRightRatio = 1;
    ResetOdometer();
    _LeftMotor.SetMotorPwmAndRevolutions(LeftMotorSpeed,0);
    _RightMotor.SetMotorPwmAndRevolutions(RightMotorSpeed,0);

/*    _LeftMotor.SetMotorPwmAndRevolutions(LeftMotorFullSpeed,20);
    _RightMotor.SetMotorPwmAndRevolutions(RightMotorFullSpeed,20);
    while (_LeftMotor.GetRevolutionsLeft() != 0) {};
    while (_RightMotor.GetRevolutionsLeft() != 0) {};

    while (_LeftMotor.ReadCaptureTime() < _RightMotor.ReadCaptureTime()) {
        // Left motor is faster, adjust to match right motor
        LeftMotorFullSpeed -= 1;
    _LeftMotor.SetMotorPwmAndRevolutions(LeftMotorFullSpeed,20);
    _RightMotor.SetMotorPwmAndRevolutions(RightMotorFullSpeed,20);
        _LeftMotor.ResetOdometer();
    while (_LeftMotor.GetRevolutionsLeft() != 0) {};
    while (_RightMotor.GetRevolutionsLeft() != 0) {};
    }
    while (_LeftMotor.ReadCaptureTime() > _RightMotor.ReadCaptureTime()) {
        // Right motor is faster, adjust to match left motor
        RightMotorFullSpeed -= 1;
    _LeftMotor.SetMotorPwmAndRevolutions(LeftMotorFullSpeed,20);
    _RightMotor.SetMotorPwmAndRevolutions(RightMotorFullSpeed,20);
        _RightMotor.ResetOdometer();
    while (_LeftMotor.GetRevolutionsLeft() != 0) {};
    while (_RightMotor.GetRevolutionsLeft() != 0) {};
    }*/
}

void MotorController::Forwards(int ForwardCount)
{
    LeftMotorSpeed = 18000;
    RightMotorSpeed = 18000;
    LeftToRightRatio = 1;
    DistanceToTravel = ForwardCount;
    ResetOdometer();
    _LeftMotor.SetMotorPwmAndRevolutions(LeftMotorSpeed,ForwardCount);
    _RightMotor.SetMotorPwmAndRevolutions(RightMotorSpeed,ForwardCount);
}

void MotorController::UpdatePwms(void)
{
    _LeftMotor.SetMotorPwm(LeftMotorSpeed);
    _RightMotor.SetMotorPwm(RightMotorSpeed);
}

void MotorController::ResetOdometer(void)
{
    _LeftMotor.ResetOdometer();
    _RightMotor.ResetOdometer();
}

int MotorController::ReadOdometer(void)
{
    return((_LeftMotor.ReadOdometer() + _RightMotor.ReadOdometer())/2);
}

int MotorController::ReadLeftOdometer(void)
{
    return(_LeftMotor.ReadOdometer());
}

int MotorController::ReadRightOdometer(void)
{
    return(_RightMotor.ReadOdometer());
}

void MotorController::MatchSpeeds(void)
{
    if (ReadOdometer() >= DistanceToTravel)
    {
        LeftMotorSpeed = 0;
        RightMotorSpeed = 0;
        _LeftMotor.SetMotorPwmAndRevolutions(LeftMotorSpeed,0);
        _RightMotor.SetMotorPwmAndRevolutions(RightMotorSpeed,0);
    }
    else
    {
        if (_LeftMotor.ReadOdometer() < _RightMotor.ReadOdometer())
        {
                LeftMotorSpeed+=10;
                RightMotorSpeed-=10;
                UpdatePwms();
        } else
        {
                LeftMotorSpeed-=10;
                RightMotorSpeed+=10;
                UpdatePwms();
        }
    }
}

#endif