CytronSmartDriveDuoDriver.h

Go to the documentation of this file.

/*
Arduino Library for Cytron SmartDriveDuo
 
Permission to use, copy, modify, and/or distribute this software for any
purpose with or without fee is hereby granted, provided that the above
copyright notice and this permission notice appear in all copies.
 
THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER
RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE
USE OR PERFORMANCE OF THIS SOFTWARE.
*/
 
class CytronSmartDriveDuoDriver
{
public:
    CytronSmartDriveDuoDriver(byte address, Stream& port, uint8_t initialByte = 0x80) :
        fPort(&port),
        fAddress(address & 0x7),
        fInitialByte(initialByte)
    {
    }
 
    inline byte address() const
    {
        return fAddress;
    }
 
    inline void setAddress(byte addr)
    {
        fAddress = (addr & 0x7);
    }
 
    void autobaud(boolean dontWait = false) const
    {
        if (fPort != nullptr)
        {
            if (!dontWait)
            {
                delay(1000);
            }
            fPort->write(&fInitialByte, 1);
            fPort->flush();
            if (!dontWait)
            {
                delay(500);
            }
        }
    }
 
    void turn(int power)
    {
        fTurnPower = power;
    }
 
    void drive(int power)
    {
        int turn = map(fTurnPower, -127, 127, -255, 255);
        int throttle = map(power, -127, 127, -255, 255);
        int left = throttle + turn;
        int right = throttle - turn;
        float scaleLeft = abs(left/255.0);
        float scaleRight = abs(right/255.0);
        float scaleMax = max(scaleLeft, scaleRight);
        scaleMax = max(1.0f, scaleMax);
        left = int(constrain(left/scaleMax, -255, 255));
        right = int(constrain(right/scaleMax, -255, 255));
        motor(map(left, -255, 255, -127, 127),
                map(right, -255, 255, -127, 127));
    }
 
    void motor(int power)
    {
        const uint8_t headerByte = 0x85;
 
        // Left motor
        uint8_t addressByte = fAddress;
        uint8_t commandByte = map(power, -127, 127, 0, 255);
        uint8_t checksum = headerByte + addressByte + commandByte;
        fPort->write(&headerByte, 1);
        fPort->write(&addressByte, 1);
        fPort->write(&commandByte, 1);
        fPort->write(&checksum, 1);
    }
 
    void motor(int powerLeft, int powerRight)
    {
        printf("MOTOR{%d}:%d:%d\n", address(), powerLeft, powerRight);
        const uint8_t headerByte = 0x85;
 
        // Left motor
        uint8_t addressByte = fAddress;
        uint8_t commandByte = map(powerLeft, -127, 127, 0, 255);
        uint8_t checksum = headerByte + addressByte + commandByte;
        printf("  LEFT: address=%02X cmd=%02X checksum=%02X\n", addressByte, commandByte, checksum);
        fPort->write(&headerByte, 1);
        fPort->write(&addressByte, 1);
        fPort->write(&commandByte, 1);
        fPort->write(&checksum, 1);
 
        // Right motor
        addressByte = (fAddress | 0x40);
        commandByte = map(powerRight, -127, 127, 0, 255);
        checksum = headerByte + addressByte + commandByte;
        printf("  RIGHT: address=%02X cmd=%02X checksum=%02X\n", addressByte, commandByte, checksum);
        fPort->write(&headerByte, 1);
        fPort->write(&addressByte, 1);
        fPort->write(&commandByte, 1);
        fPort->write(&checksum, 1);
 
        fTurnPower = 0;
    }
 
    void stop()
    {
        motor(0, 0);
    }
 
    // Not supported
    void setDeadband(byte value)
    {
    }
 
    // Not supported
    void setTimeout(int milliseconds)
    {
    }
 
private:
    Stream* fPort;
    uint8_t fAddress;
    uint8_t fInitialByte;
    uint32_t fStartTime;
    int fTurnPower = 0;
};
 
class CytronSmartDriveDuoMDDS10Driver : public CytronSmartDriveDuoDriver
{
public:
    CytronSmartDriveDuoMDDS10Driver(byte address, Stream& port) :
        CytronSmartDriveDuoDriver(address, port, 0x55) {}
};
 
class CytronSmartDriveDuoMDDS30Driver : public CytronSmartDriveDuoDriver
{
public:
    CytronSmartDriveDuoMDDS30Driver(byte address, Stream& port) :
        CytronSmartDriveDuoDriver(address, port, 0x80) {}
};
 
class CytronSmartDriveDuoMDDS60Driver : public CytronSmartDriveDuoDriver
{
public:
    CytronSmartDriveDuoMDDS60Driver(byte address, Stream& port) :
        CytronSmartDriveDuoDriver(address, port, 0x55) {}
};