File content as of revision 2:cd7543fdcb8c:

/*******************************************************************************
* RenBED PID Motor Control for RenBuggy                                        *
* Copyright (c) 2014 Sally Brown & Liz Lloyd                                   *
*                                                                              *
* 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.                                                                *
*                                                                              *
* PID_Controller.cpp                                                           *
*                                                                              *
*******************************************************************************/

#ifndef _PIDCONTROLLER_C
#define _PIDCONTROLLER_C

#include "RenBuggy_PID.h"

PID_Stripes::PID_Stripes(PinName motorL, PinName motorR, PinName brakeL, PinName brakeR, PinName sensorL, PinName sensorR) :
   PID_Controller(motorL, motorR, brakeL, brakeR),
   m_senseL(sensorL),
   m_senseR(sensorR)
{
    m_senseL.setSampleFrequency(1000);
    m_senseR.setSampleFrequency(1000); //If this is playing up, consider changing this to 1001?
    
    //It's 5 samples before it recognises it's held on.
    m_senseL.setSamplesTillHeld(5);
    m_senseR.setSamplesTillHeld(5);
    
    PID_Controller* basePointer = dynamic_cast<PID_Stripes*>(this);
    //Only when it's been held high and then goes low will it increment the number of counts.
    m_senseL.attach_deasserted_held(basePointer, &PID_Controller::countL);
    m_senseR.attach_deasserted_held(basePointer, &PID_Controller::countR);
}

PID_Magnet::PID_Magnet(PinName motorL, PinName motorR, PinName brakeL, PinName brakeR, PinName sensorL, PinName sensorR) :
   PID_Controller(motorL, motorR, brakeL, brakeR),
   m_senseL(sensorL),
   m_senseR(sensorR)
{
    PID_Controller* basePointer = dynamic_cast<PID_Magnet*>(this);
    m_senseL.fall(basePointer, &PID_Controller::countL);
    m_senseR.fall(basePointer, &PID_Controller::countR);
}

PID_Controller::PID_Controller
(
    PinName motorL,
    PinName motorR,
    PinName brakeL,
    PinName brakeR
) :
    m_controllerL(1.0, 0.0, 0.0, RATE),  //Kc, Ti, Td, interval
    m_controllerR(1.0, 0.0, 0.0, RATE),
    m_motorL(motorL),
    m_motorR(motorR),
    m_brakeL(brakeL),
    m_brakeR(brakeR),
    m_countsPerRev(16),                             //Default to 16 stripes
    m_wheelCircumference(16.96),                    //Default to 16.96
    m_axleLength(13),
    m_stripesL(0),                                  //Initialise the number of stripes to 0.
    m_stripesR(0),
    m_turnLeft(false),
    m_turnRight(false),    
    m_fProportionLeft(0.0),
    m_fProportionRight(0.0)
{
    setUpControllers();
}

void PID_Controller::SetUpConstants(int countsPerRev, float wheelCircumference, float axleLength)
{
    m_countsPerRev = countsPerRev;
    m_wheelCircumference = wheelCircumference;
    m_axleLength = axleLength;
}

void PID_Controller::Forwards(int distanceForward)
{
    m_turnRight = false;
    m_turnLeft = false;
    bool moving = true;
    
    int CountsForward = distanceForward * (m_countsPerRev/m_wheelCircumference);
    
    m_rate.attach(this, &PID_Controller::doSomePID, RATE); //Attach the counter if it hasn't gone too far. Then hopefully just sit in a loop.
    
    m_stripesL = m_stripesR = 0;
    m_brakeR = m_brakeL = 0;
    
    m_fProportionLeft = m_fProportionRight = 1.0;
    
    while(moving) 
    {
        if (CountsForward < m_stripesL)
        {
            m_motorL = 0.0;
            m_brakeL = 1;
        }
        if (CountsForward < m_stripesR)
        {
            m_motorR = 0.0;
            m_brakeR = 1;
        }
        if(CountsForward < m_stripesR &&
           CountsForward < m_stripesL)
        {
            m_rate.detach();
            moving = false;
        }
    }
    
    Stop();
    
    return;
}

void PID_Controller::Left(int AngleLeft, int RadiusLeft)
{
    m_turnRight = false; //Turning left, NOT turning right
    m_turnLeft = true;
    bool turning = true;
    
    m_rate.attach(this, &PID_Controller::doSomePID, RATE);
    
    m_brakeR = m_brakeL = 0; //Turning off the brakes is often quite fun.
    m_stripesL = m_stripesR = 0;
    
    int halfAxleLength = (m_axleLength + 1)/2;
    RadiusLeft = RadiusLeft < halfAxleLength ? halfAxleLength : RadiusLeft;
    AngleLeft = AngleLeft > 90 ? 90 : AngleLeft;
    
    float m_fDistanceL = (2*pi*(RadiusLeft - halfAxleLength))*(m_countsPerRev/m_wheelCircumference)/(360/AngleLeft); 
    float m_fDistanceR = (2*pi*(RadiusLeft + halfAxleLength))*(m_countsPerRev/m_wheelCircumference)/(360/AngleLeft); //gives the length of the arc over which the wheel will travel, and translates that into a number of wheel stripes
    
    int LeftWheelDist = (int) m_fDistanceL; //Cast the distance into an int
    int RightWheelDist = (int) m_fDistanceR;
    
    m_fProportionLeft = (float)LeftWheelDist/(float)RightWheelDist;    
    m_fProportionRight = (float)RightWheelDist/(float)LeftWheelDist; //When turning right, you only use the left wheel's proportion

    while (turning)
    {
        if (LeftWheelDist <= m_stripesL) //If the left motor has gone far enough
        {
            m_motorL = 0.0;   //Stop the motor
            m_brakeL = 1;     //Apply the brakes
        }
        if (RightWheelDist <= m_stripesR)
        {
            m_motorR = 0.0;
            m_brakeR = 1;
        }
        if(RightWheelDist <= m_stripesR &&
           LeftWheelDist <= m_stripesL)
        {
            m_rate.detach();
            turning = false;
            break;
        }
    }

    Stop();
} 

void PID_Controller::Right(int AngleRight, int RadiusRight)
{
    m_turnRight = true;
    m_turnLeft = false;
    bool turning = true;
    
    m_rate.attach(this, &PID_Controller::doSomePID, RATE);

    m_brakeR = m_brakeL = 0; //Turning off the brakes is often quite fun.    
    m_stripesL = m_stripesR = 0;
    
    int halfAxleLength = (m_axleLength + 1)/2;
    RadiusRight = RadiusRight < halfAxleLength ? halfAxleLength : RadiusRight;
    AngleRight = AngleRight > 90 ? 90 : AngleRight;
    
    float m_fDistanceL = (2*pi*(RadiusRight + halfAxleLength))*(m_countsPerRev/m_wheelCircumference)/(360/AngleRight); //Forcing it to an int beforehand didn't work. It twitches instead of going argh no, but it still doesn't really work.
    float m_fDistanceR = (2*pi*(RadiusRight - halfAxleLength))*(m_countsPerRev/m_wheelCircumference)/(360/AngleRight);
    
    int LeftWheelDist = (int) m_fDistanceL; 
    int RightWheelDist = (int) m_fDistanceR;
    
    m_fProportionLeft = (float)LeftWheelDist/(float)RightWheelDist;
    m_fProportionRight = (float)RightWheelDist/(float)LeftWheelDist;

    while (turning)
    {
        if (LeftWheelDist <= m_stripesL) //If the left motor has gone far enough
        {
            m_motorL = 0.0;   //Stop the motor
            m_brakeL = 1;     //Apply the brakes
        }
        if (RightWheelDist <= m_stripesR)
        {
            m_motorR = 0.0;
            m_brakeR = 1;
        }
        if(RightWheelDist <= m_stripesR &&
           LeftWheelDist <= m_stripesL)
        {
            m_rate.detach();
            turning = false;
            break;
        }
    }

    Stop();
} 

void PID_Controller::Stop()
{
    m_stripesL = 0;
    m_stripesR = 0;
    m_motorL = 0.0;
    m_motorR = 0.0;
    m_brakeL = 1;
    m_brakeR = 1;
    
    m_rate.detach();
}

void PID_Controller::doSomePID()
{
    PIDLeft();
    PIDRight();
}

void PID_Controller::PIDLeft()
{
    float fSPL = 0.0;
    int SPL = 0;
    
    if(m_turnLeft)
    {
        fSPL = m_stripesR / m_fProportionRight;
        SPL = (int) fSPL;
    }
    else if(m_turnRight)
    {
        fSPL = m_stripesR * m_fProportionLeft;
        SPL = (int) fSPL;
    }
    else
    {
        SPL = m_stripesR;
    }

    m_controllerL.setProcessValue(m_stripesL); 
    
    m_motorL = (m_controllerL.compute()); //PWM output * some speed proportion. May slow it down or speed it up.*/
    
    m_controllerL.setSetPoint(SPL);
}

void PID_Controller::PIDRight()
{
    float fSPR = 0.0;
    int SPR = 0;
    
    if(m_turnRight) //If you're turning right, the left wheel goes further, so use xProportionLeft
    {
        fSPR = m_stripesL / m_fProportionLeft;
        SPR = (int) fSPR;
    }
    else if(m_turnLeft) //If you're turning left, the right wheel goes further, so use xProportionRight
    {
        fSPR = m_stripesL * m_fProportionRight;
        SPR = (int) fSPR;
    }
    else
    {
        SPR = m_stripesL;
    }
    
    m_controllerR.setProcessValue(m_stripesR); //Set the process value (what it IS).
    
    m_motorR = (m_controllerR.compute()); //Calculate the PWM duty cycle
    
    m_controllerR.setSetPoint(SPR); //SPR = set point right. this sets the set point (what it SHOULD BE).
}

void PID_Controller::countL() 
{
    m_stripesL++;
}

void PID_Controller::countR() 
{
    m_stripesR++;
}

void PID_Controller::setUpControllers()
{
    m_controllerL.setInputLimits(0.0, 200);
    m_controllerR.setInputLimits(0.0, 200);
    //Pwm output from 0.0 to 1.0 (PWM duty cycle %)
    m_controllerL.setOutputLimits(0.0, 1.0);
    m_controllerR.setOutputLimits(0.0, 1.0);
    //If there's a bias. FULL SPEED AHEAD. I don't know why this works but it does.
    m_controllerL.setBias(1.0);
    m_controllerR.setBias(1.0);
    //Set it to auto mode.
    m_controllerL.setMode(AUTO_MODE);
    m_controllerR.setMode(AUTO_MODE);  
}

void PID_Controller::ConfigurePID(float p, float i, float d)
{
    m_controllerL.setTunings(p, i, d);
    m_controllerR.setTunings(p, i, d);
}

#endif