PCB0031_Grip.h

Go to the documentation of this file.

#pragma once
/*
Copyright 2025 Broadwell Consulting Inc.
 
"Serial Wombat" is a registered trademark of Broadwell Consulting Inc. in
the United States.  See SerialWombat.com for usage guidance.
 
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.
*/
#include "SerialWombat.h"

 
 
class GripACS712 : public SerialWombatAnalogInput_18AB
{
public:
    GripACS712(SerialWombatChip& serialWombatChip) :SerialWombatAnalogInput_18AB(serialWombatChip) {}

 
    int16_t begin(uint8_t pin)
    {
        if (pin < 4 || pin > 7)
        {
            return ((int16_t)(-1 * SW_ERROR_PIN_NOT_CAPABLE));
        }
        return SerialWombatAnalogInput_18AB::begin(pin);
    }

    uint16_t zeroCurrentCalibration = 32768;

    uint16_t calibrateIdleCurrent()
    {
        
        zeroCurrentCalibration = readAveragedCounts();
        return zeroCurrentCalibration;
    }

    int16_t readCurrent_mA()
    {
        volatile uint16_t initialReading = readAveragedCounts();
        //ACS712 moves 185mV / A.
        // 65535 / 5000mV = 13.107 counts / mV  
        // 2425 counts / A
        int32_t reading = initialReading;
        reading -= zeroCurrentCalibration;
        reading *= 1000;  // Go to mA
        reading /= 2425;
        int16_t result = reading;
        return (result);
 
    }
};

 
class GripServo : public SerialWombatServo_18AB
{
public:
    GripServo(SerialWombatChip & serialWombatChip):SerialWombatServo_18AB(serialWombatChip), sensor(serialWombatChip)
    {
    }
    GripACS712 sensor;
 

    int16_t begin(byte pin, bool reverse = false)
    {
        attach(pin, reverse);
        return sensor.begin(pin + 4);
    }
  
    uint16_t calibratedMinPosition = 0;  
    uint16_t calibratedMaxPosition = 65535;  
    uint16_t calibratedMinCurrent = 0;  
    uint16_t calibratedMaxCurrent = 65535;
 

    void calibrateServoRange(uint16_t expectedCurrentRise = 400, uint16_t calibrationStartPosition = 0x8000)
    {
        // Attach again to remove any scaled output settings
        attach(_pin,//Pin  
            _min, //Minimum Pulse Time
            _max, //MaximumPulse time
            _reverse);  //Reverse//put this line in setup.
        writePublicData(calibrationStartPosition);
        delay(500);
        uint16_t position = calibrationStartPosition;
        while (sensor.readAveragedCounts() < sensor.zeroCurrentCalibration + expectedCurrentRise && position >= 2000)
        {
            position -= 2000;
            writePublicData(position);
            delay(150);
        }
 
        calibratedMinPosition = position + 2000;
 
        position = calibrationStartPosition;
        writePublicData(position);
        delay(500);
        while (sensor.readAveragedCounts() < sensor.zeroCurrentCalibration + expectedCurrentRise && position <= 63535)
        {
            position += 2000;
            writePublicData(position);
            delay(150);
        }
        calibratedMaxPosition = position - 2000;
 
    }

 
    void grip(uint16_t gripStrength = 0, uint16_t slowIncrement = 100, uint16_t fastSlowThreshold = 350, uint16_t fastIncrement = 3500)
    {
        attach(_pin,//Pin  
            _min, //Minimum Pulse Time
            _max, //MaximumPulse time
            _reverse);  //Reverse//put this line in setup.
        
        if (gripStrength == 0)  // Zero default is 3/4 current.   Allows neat call with no parameters.
        {
            gripStrength = 49152; // 3/4
        }
        uint32_t gripCurrent = (calibratedMaxCurrent - calibratedMinCurrent);
        gripCurrent *= gripStrength;
        gripCurrent >>= 16;
        writeScalingTargetValue(((uint16_t) gripCurrent) + sensor.zeroCurrentCalibration);
        writeRamp(slowIncrement, //Slow Increment  How many counts the gripper moves if the current is above or below the target.
            fastSlowThreshold, //Fast/slow threshold  //  If the difference between measured and target current is more than this, move fast instead
            fastIncrement, //Fast Increment rate
            SerialWombatAbstractScaledOutput::Period::PERIOD_32mS,//How often the control loop updates.  
            SerialWombatAbstractScaledOutput::RampMode::RAMP_MODE_BOTH); // Ramp mode
 
        writeOutputScaling(calibratedMinPosition, calibratedMaxPosition);
 
        writeScalingEnabled(true, //Enabled
            sensor.pin()); //Pin providing input to control loop.  
    }

    void release()
    {
        attach(_pin,//Pin
            500, //Minimum Pulse Time
            2500, //MaximumPulse time
            false);  //Reverse//put this line in setup.
 
        // Set output scaling.  This will remap the meaning of the 0-65535 value from full range of the servo to full range of the gripper.
        writeOutputScaling(calibratedMinPosition, calibratedMaxPosition);
 
        writeScalingEnabled(true, //Enabled
            _pin); //DataSource
        writePublicData(0);
    }

    bool objectPresent(uint16_t divisor = 50)
    {
        return(readPublicData() < (calibratedMaxPosition - ((calibratedMaxPosition - calibratedMinPosition) / divisor)));
    }

    void calibrateGripper(bool reverse = false, uint16_t rangeConstant = 49152)
    {
        uint32_t position = 0;
 
        begin(_pin, reverse);
        calibratedMinCurrent = 65535;
        calibratedMaxCurrent = 0;
        uint16_t minCurrentPosition = 0;
        for (position = 0; position <= 65000; position += 1000)
        {
            writePublicData((uint16_t)position);
            delay(250);
            uint16_t current =sensor.readAveragedCounts();
            if (current < calibratedMinCurrent)
            {
                calibratedMinCurrent = current;
                minCurrentPosition = position;
            }       
            if (current > calibratedMaxCurrent)
            {
                calibratedMaxCurrent = current;
            }
        }
 
        writePublicData(minCurrentPosition);
        delay(1000);
        sensor.calibrateIdleCurrent();
        uint32_t range = (calibratedMaxCurrent - calibratedMinCurrent);
        range *= rangeConstant;
        range >>= 16;
        calibrateServoRange((uint16_t) range, minCurrentPosition);
 
    }
 
};

 
class PCB0031_Grip : public SerialWombatChip
{
public:
    PCB0031_Grip() : SerialWombatChip(), gs0(*this), gs1(*this), gs2(*this), gs3(*this), powerVoltage(*this)
    {}
    
    GripServo gs0,gs1,gs2,gs3;
    SerialWombatAnalogInput_18AB powerVoltage;
 
    GripServo* gsArray[4] = { &gs0,&gs1,&gs2,&gs3 };
 

    int16_t begin(uint8_t i2cAddress,bool pin3IsVoltage = false)
    {
        SerialWombatChip::begin(Wire, i2cAddress, true);
        for (int i = 0; i < 3; ++i)
        {
            gsArray[i]->begin(i);
        }
        if (!pin3IsVoltage)
        {
            gs3.begin(3);
        }
        else
        {
            powerVoltage.begin(3);
            pin3VoltageEnable = true;
 
        }
        return (0);
    }
 

    uint16_t readPowerVoltage_mv()
    {
        if (pin3VoltageEnable)
        {
            uint32_t v = powerVoltage.readAveraged_mV();
            v *= (8200 + 2000);  // Board has an 8200 / 2000 ohm voltage divider
            v /= 2000;
            return ((uint16_t)v);
        }
        return (0);
            
    }
 
private:
    bool pin3VoltageEnable = false;
};