/*! Copyright 2017-2020 Officina Turini\n
This file is part of otStudio project.\n
otStudio is free software: you can redistribute it and/or modify\n
it under the terms of the GNU General Public License as published by\n
the Free Software Foundation, either version 3 of the License.\n
\n
otStudio is distributed in the hope that it will be useful,\n
but WITHOUT ANY WARRANTY; without even the implied warranty of\n
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n
GNU General Public License for more details.\n
\n
You should have received a copy of the GNU General Public License\n
along with otStudio. If not, see <http://www.gnu.org/licenses/>.
*/
#include "_lib.h"
#include "otBotEngine.h"
#include "otTimer.h"
#include "otPort.h"
#include "otFPGA.h"
#include "otPrintf.h"
otBotEngine::otBotEngine()
{
}
void otBotEngine::action(U8 com, U32 trip)
{
_engineCom = com;
FpgaEncoderClear(_leftEncoderIndex);
FpgaEncoderClear(_rightEncoderIndex);
_speedControl = true;
_tripLeft = 0;
_tripRight = 0;
_lastLeftEncoder = 0;
_lastRightEncoder = 0;
_leftEncoder = 0;
_rightEncoder = 0;
_tripLeftTarget = trip;
_tripRightTarget = trip;
if(trip)
{
_brakeSlopePtr = 0;
_brakeSlope[0] = trip / 2;
_brakeSlope[1] = _brakeSlope[0] + _brakeSlope[0] / 2;
_brakeSlope[2] = _brakeSlope[1] + _brakeSlope[1] / 8;
_brakeSlope[3] = _brakeSlope[2] + _brakeSlope[2] / 16;
}
else _brakeSlopePtr = 3;
ElapsedTimeStart(_timerId);
minimumPWM();
process(true);
}
U8 otBotEngine::action()
{
return _engineLast;
}
void otBotEngine::setSpeed(U16 lspeed, U16 rspeed)
{
_speedSetLeft = lspeed;
if(rspeed == 0xFFFF)
_speedSetRight = lspeed;
else
_speedSetRight = rspeed;
}
S16 otBotEngine::encoder(bool left)
{
return left ? _leftEncoder : _rightEncoder;
}
S32 otBotEngine::route(bool left)
{
if(left)
return _travelLeft;
return _travelRight;
}
void otBotEngine::setMinimumPwm(U16 pwmValue)
{
_pwmMin = pwmValue;
}
bool otBotEngine::ready()
{
if(_engineCom == MA_RUN ||
_engineCom == MA_BRAKE ||
_engineCom == MA_HARD_STOP)
return false;
return true;
}
void otBotEngine::setBrakeTime(U32 dly)
{
_brakeTime = dly;
}
void otBotEngine::setSlope(U16 val)
{
_slope = val;
}
void otBotEngine::writePort(U8 val)
{
if(_outPort) _outPort(val);
else FpgaPxPut(val);
}
void otBotEngine::init(U8 timerId, otPWM * pwmClass, U8 leftPwmPin, U8 rightPwmPin, U8 leftEncoderIndex, U8 rightEncoderIndex, portOutputCallBack op)
{
_timerId = timerId;
pwm = pwmClass;
_travelLeft = 0;
_travelRight = 0;
_tripLeft = 0;
_tripRight = 0;
_engine = 0;
_engineCom = MA_NOP;
_leftPWM = 256;
_rightPWM = 256;
_pwmMin = 256;
_brakeTime = 10;
_slope = 5;
_leftPwmPin = leftPwmPin;
_rightPwmPin = rightPwmPin;
_speedSetLeft = 10;
_speedSetRight = 10;
_leftEncoderIndex = leftEncoderIndex;
_rightEncoderIndex = rightEncoderIndex;
_lastLeftEncoder = 0;
_lastRightEncoder = 0;
_outPort = op;
_mc[MC_FORWARD] = 0x60;
_mc[MA_BACKWARD] = 0x90;
_mc[MC_TURNLEFT] = 0xA0;
_mc[MC_TURNRIGHT] = 0x50;
_mc[MC_COMPASS_LEFT_CW] = 0x40;
_mc[MC_COMPASS_LEFT_CCW] = 0x80;
_mc[MC_COMPASS_RIGHT_CW] = 0x10;
_mc[MC_COMPASS_RIGHT_CCW] = 0x20;
_mc[MC_MASK_LEFT] = 0x3F;
_mc[MC_MASK_RIGHT] = 0xCF;
_mc[MC_COM_INVERT] = 0xF0;
minimumPWM();
FpgaInit();
// Configure the port for the control of the motor driver (L298N)
if(op)
FpgaPortPxSet( PD_INPUT, PD_INPUT, PD_INPUT, PD_INPUT,
PD_INPUT, PD_INPUT, PD_INPUT, PD_INPUT);
else
FpgaPortPxSet( PD_INPUT, PD_INPUT, PD_INPUT, PD_INPUT,
PD_OUTPUT, PD_OUTPUT, PD_OUTPUT, PD_OUTPUT);
// Encoder port setup
FpgaEncoderSetup(E_0_PORTX | E_1_PORTX, E_2_5MHZ, 0);
FpgaEncoderClear(_leftEncoderIndex);
FpgaEncoderClear(_rightEncoderIndex);
writePort(_engine);
_timerCount = TimerGetCounts(_timerId);
}
void otBotEngine::setMotorControls(U8 index, U8 val)
{
if(index <= MC_COM_INVERT)
_mc[index] = val;
}
U16 otBotEngine::speed(bool left)
{
if(left)
return _speedLeft;
return _speedRight;
}
void otBotEngine::process(bool force)
{
if(!force)
{
U32 t = ElapsedTimeUs(_timerCount, TimerGetCounts(_timerId));
// Minimum interval must be 50ms
if(t < 50000)
return;
if(_engineCom == MA_NOP) return;
}
_lastLeftEncoder = _leftEncoder;
_lastRightEncoder = _rightEncoder;
_leftEncoder = FpgaEncoderRead(_leftEncoderIndex);
_rightEncoder = FpgaEncoderRead(_rightEncoderIndex);
switch(_engineCom)
{
case MA_RUN:
break;
case MA_FORWARD:
_engine = _mc[MC_FORWARD];
writePort(_engine);
_engineLast = _engineCom;
_engineCom = MA_RUN;
break;
case MA_BACKWARD:
_engine = _mc[MC_BACKWARD];
writePort(_engine);
_engineLast = _engineCom;
_engineCom = MA_RUN;
break;
case MA_TURNLEFT:
_engine = _mc[MC_TURNLEFT];
writePort(_engine);
_engineLast = _engineCom;
_engineCom = MA_RUN;
break;
case MA_TURNRIGHT:
_engine = _mc[MC_TURNRIGHT];
writePort(_engine);
_engineLast = _engineCom;
_engineCom = MA_RUN;
break;
case MA_COMPASS_LEFT_CW:
_engine = _mc[MC_COMPASS_LEFT_CW];
writePort(_engine);
_engineLast = _engineCom;
_engineCom = MA_RUN;
break;
case MA_COMPASS_LEFT_CCW:
_engine = _mc[MC_COMPASS_LEFT_CCW];
writePort(_engine);
_engineLast = _engineCom;
_engineCom = MA_RUN;
break;
case MA_COMPASS_RIGHT_CW:
_engine = _mc[MC_COMPASS_RIGHT_CW];
writePort(_engine);
_engineLast = _engineCom;
_engineCom = MA_RUN;
break;
case MA_COMPASS_RIGHT_CCW:
_engine = _mc[MC_COMPASS_RIGHT_CCW];
writePort(_engine);
_engineLast = _engineCom;
_engineCom = MA_RUN;
break;
case MA_BRAKE:
if(_speedSetLeft || _speedSetRight)
{
if(_speedSetLeft > 0)
_speedSetLeft /= 2;
if(_speedSetRight > 0)
_speedSetRight /= 2;
}
else _engineCom = MA_HARD_STOP;
break;
case MA_HARD_STOP:
minimumPWM();
writePort(_engine ^ _mc[MC_COM_INVERT]);
DelayMs(10);
_engine = 0;
writePort(_engine);
_engineCom = MA_NOP;
_engineLast = 0;
break;
}
if(force) return;
// Speed measure
U32 la = (U32) abs(abs(_leftEncoder) - abs(_lastLeftEncoder));
_tripLeft += la;
U32 ra = (U32) abs(abs(_rightEncoder) - abs(_lastRightEncoder));
_tripRight += ra;
//U32 t = ElapsedTimeEndUs(_timerId);
//ElapsedTimeStart(_timerId);
U32 t = ElapsedTimeUs(_timerCount, TimerGetCounts(_timerId));
_timerCount = TimerGetCounts(_timerId);
_speedLeft = la * 1000000 / t;
_speedRight = ra * 1000000 / t;
// Differential action
if(_tripLeft > _tripRight)
_speedLeft += (_tripLeft - _tripRight) * 2;
else
_speedRight += (_tripRight - _tripLeft) * 2;
// Check the speed
if(_engineLast && _speedControl)
{
if(_speedLeft != _speedSetLeft)
{
if(_speedLeft < _speedSetLeft)
{
if(_leftPWM < (4095 - _slope))
{
_leftPWM += _slope;
pwm->setPin(_leftPwmPin, _leftPWM);
}
}
else
{
if(_leftPWM > (_pwmMin + _slope))
{
_leftPWM -= _slope;
pwm->setPin(_leftPwmPin, _leftPWM);
}
}
}
if(_speedRight != _speedSetRight)
{
if(_speedRight < _speedSetRight)
{
if(_rightPWM < (4095 - _slope))
{
_rightPWM += _slope;
pwm->setPin(_rightPwmPin, _rightPWM);
}
}
else
{
if(_rightPWM > (_pwmMin + _slope))
{
_rightPWM -= _slope;
pwm->setPin(_rightPwmPin, _rightPWM);
}
}
}
// path amount
if(_engineLast == MA_FORWARD || _engineLast == MA_BACKWARD)
{
_travelLeft += _leftEncoder - _lastLeftEncoder;
_travelRight += _rightEncoder - _lastRightEncoder;
}
}
// Check the trip
if((_tripLeftTarget || _tripRightTarget) && _engine)
{
// Deceleration control
if(_brakeSlopePtr < 3)
{
if(_tripLeft >= _brakeSlope[_brakeSlopePtr])
{
_brakeSlopePtr++;
_speedSetLeft *= 0.8f;
_speedSetRight *= 0.8f;
}
}
if(_tripRight >= _tripRightTarget)
{
_tripRightTarget = 0;
_rightPWM = _pwmMin;
pwm->setPin(_rightPwmPin, _rightPWM);
_speedControl = false;
}
if(_tripLeft >= _tripLeftTarget)
{
//printf("_tripLeftTarget = %u, _tripLeft = %u\n", _tripLeftTarget, _tripLeft);
_tripLeftTarget = 0;
_leftPWM = _pwmMin;
pwm->setPin(_leftPwmPin, _leftPWM);
if(!_speedControl)
{
writePort(_engine ^ _mc[MC_COM_INVERT]);
DelayMs(_brakeTime);
_engine = 0;
writePort(_engine);
_engineCom = MA_NOP;
}
}
}
}
void otBotEngine::minimumPWM()
{
_leftPWM = _pwmMin;
pwm->setPin(_leftPwmPin, _leftPWM);
_rightPWM = _pwmMin;
pwm->setPin(_rightPwmPin, _rightPWM);
}
void otBotEngine::waitEnded(U32 timeout)
{
do
{
process();
DelayMs(50);
}while(!ready() && timeout--);
}