Serial Wombat Arduino Library

Loading...
Searching...
No Matches
PCB0031_Grip.h
Go to the documentation of this file.
1#pragma once
2/*
3 Copyright 2025 Broadwell Consulting Inc.
4
5"Serial Wombat" is a registered trademark of Broadwell Consulting Inc. in
6 the United States. See SerialWombat.com for usage guidance.
7
8 Permission is hereby granted, free of charge, to any person obtaining a
9 copy of this software and associated documentation files (the "Software"),
10 to deal in the Software without restriction, including without limitation
11 the rights to use, copy, modify, merge, publish, distribute, sublicense,
12 and/or sell copies of the Software, and to permit persons to whom the
13 Software is furnished to do so, subject to the following conditions:
14
15 The above copyright notice and this permission notice shall be included in
16 all copies or substantial portions of the Software.
17
18 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
21 THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
22 OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
23 ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
24 OTHER DEALINGS IN THE SOFTWARE.
25*/
29#include "SerialWombat.h"
30
49
50
51 class GripACS712 : public SerialWombatAnalogInput_18AB
52{
53 public:
59 GripACS712(SerialWombatChip& serialWombatChip) :SerialWombatAnalogInput_18AB(serialWombatChip) {}
60
71
72 int16_t begin(uint8_t pin)
73 {
74 if (pin < 4 || pin > 7)
75 {
76 return ((int16_t)(-1 * SW_ERROR_PIN_NOT_CAPABLE));
77 }
78 return SerialWombatAnalogInput_18AB::begin(pin);
79 }
80
84 uint16_t zeroCurrentCalibration = 32768;
85
91 uint16_t calibrateIdleCurrent()
92 {
93
94 zeroCurrentCalibration = readAveragedCounts();
95 return zeroCurrentCalibration;
96 }
97
103 int16_t readCurrent_mA()
104 {
105 volatile uint16_t initialReading = readAveragedCounts();
106 //ACS712 moves 185mV / A.
107 // 65535 / 5000mV = 13.107 counts / mV
108 // 2425 counts / A
109 int32_t reading = initialReading;
110 reading -= zeroCurrentCalibration;
111 reading *= 1000; // Go to mA
112 reading /= 2425;
113 int16_t result = reading;
114 return (result);
115
116 }
117};
118
138
139 class GripServo : public SerialWombatServo_18AB
140{
141 public:
147 GripServo(SerialWombatChip & serialWombatChip):SerialWombatServo_18AB(serialWombatChip), sensor(serialWombatChip)
148 {
149 }
150 GripACS712 sensor;
151
152
163 int16_t begin(byte pin, bool reverse = false)
164 {
165 attach(pin, reverse);
166 sensor.begin(pin + 4);
167 }
168
172 uint16_t calibratedMinPosition = 0;
176 uint16_t calibratedMaxPosition = 65535;
180 uint16_t calibratedMinCurrent = 0;
184 uint16_t calibratedMaxCurrent = 65535;
185
186
195 void calibrateServoRange(uint16_t expectedCurrentRise = 400, uint16_t calibrationStartPosition = 0x8000)
196 {
197 // Attach again to remove any scaled output settings
198 attach(_pin,//Pin
199 _min, //Minimum Pulse Time
200 _max, //MaximumPulse time
201 _reverse); //Reverse//put this line in setup.
202 writePublicData(calibrationStartPosition);
203 delay(500);
204 uint16_t position = calibrationStartPosition;
205 while (sensor.readAveragedCounts() < sensor.zeroCurrentCalibration + expectedCurrentRise && position >= 2000)
206 {
207 position -= 2000;
208 writePublicData(position);
209 delay(150);
210 }
211
212 calibratedMinPosition = position + 2000;
213
214 position = calibrationStartPosition;
215 writePublicData(position);
216 delay(500);
217 while (sensor.readAveragedCounts() < sensor.zeroCurrentCalibration + expectedCurrentRise && position <= 63535)
218 {
219 position += 2000;
220 writePublicData(position);
221 delay(150);
222 }
223 calibratedMaxPosition = position - 2000;
224
225 }
226
240
241 void grip(uint16_t gripStrength = 0, uint16_t slowIncrement = 100, uint16_t fastSlowThreshold = 350, uint16_t fastIncrement = 3500)
242 {
243 attach(_pin,//Pin
244 _min, //Minimum Pulse Time
245 _max, //MaximumPulse time
246 _reverse); //Reverse//put this line in setup.
247
248 if (gripStrength == 0) // Zero default is 3/4 current. Allows neat call with no parameters.
249 {
250 gripStrength = 49152; // 3/4
251 }
252 uint32_t gripCurrent = (calibratedMaxCurrent - calibratedMinCurrent);
253 gripCurrent *= gripStrength;
254 gripCurrent >>= 16;
255 writeScalingTargetValue(((uint16_t) gripCurrent) + sensor.zeroCurrentCalibration);
256 writeRamp(slowIncrement, //Slow Increment How many counts the gripper moves if the current is above or below the target.
257 fastSlowThreshold, //Fast/slow threshold // If the difference between measured and target current is more than this, move fast instead
258 fastIncrement, //Fast Increment rate
259 SerialWombatAbstractScaledOutput::Period::PERIOD_32mS,//How often the control loop updates.
260 SerialWombatAbstractScaledOutput::RampMode::RAMP_MODE_BOTH); // Ramp mode
261
262 writeOutputScaling(calibratedMinPosition, calibratedMaxPosition);
263
264 writeScalingEnabled(true, //Enabled
265 sensor.pin()); //Pin providing input to control loop.
266 }
267
274 void release()
275 {
276 attach(_pin,//Pin
277 500, //Minimum Pulse Time
278 2500, //MaximumPulse time
279 false); //Reverse//put this line in setup.
280
281 // 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.
282 writeOutputScaling(calibratedMinPosition, calibratedMaxPosition);
283
284 writeScalingEnabled(true, //Enabled
285 _pin); //DataSource
286 writePublicData(0);
287 }
288
308 bool objectPresent(uint16_t divisor = 50)
309 {
310 return(readPublicData() < (calibratedMaxPosition - ((calibratedMaxPosition - calibratedMinPosition) / divisor)));
311 }
312
328 void calibrateGripper(bool reverse = false)
329 {
330 uint32_t position = 0;
331
332 begin(_pin, reverse);
333 calibratedMinCurrent = 65535;
334 calibratedMaxCurrent = 0;
335 uint16_t minCurrentPosition = 0;
336 for (position = 0; position <= 65000; position += 1000)
337 {
338 writePublicData((uint16_t)position);
339 delay(250);
340 uint16_t current =sensor.readAveragedCounts();
341 if (current < calibratedMinCurrent)
342 {
343 calibratedMinCurrent = current;
344 minCurrentPosition = position;
345 }
346 if (current > calibratedMaxCurrent)
347 {
348 calibratedMaxCurrent = current;
349 }
350 }
351
352 writePublicData(minCurrentPosition);
353 delay(500);
354 sensor.calibrateIdleCurrent();
355 calibrateServoRange((calibratedMaxCurrent - calibratedMinCurrent) * 3 / 4, minCurrentPosition);
356
357 }
358
359};
360
383
384 class PCB0031_Grip : public SerialWombatChip
385{
386 public:
387 PCB0031_Grip() : SerialWombatChip(), gs0(*this), gs1(*this), gs2(*this), gs3(*this), powerVoltage(*this)
388 {}
389
390 GripServo gs0,gs1,gs2,gs3;
391 SerialWombatAnalogInput_18AB powerVoltage;
392
393 GripServo* gsArray[4] = { &gs0,&gs1,&gs2,&gs3 };
394
395
405 int16_t begin(uint8_t i2cAddress,bool pin3IsVoltage = false)
406 {
407 SerialWombatChip::begin(Wire, i2cAddress, true);
408 for (int i = 0; i < 3; ++i)
409 {
410 gsArray[i]->begin(i);
411 }
412 if (!pin3IsVoltage)
413 {
414 gs3.begin(3);
415 }
416 else
417 {
418 powerVoltage.begin(3);
419 pin3VoltageEnable = true;
420
421 }
422 }
423
424
436 uint16_t readPowerVoltage_mv()
437 {
438 if (pin3VoltageEnable)
439 {
440 uint32_t v = powerVoltage.readAveraged_mV();
441 v *= (8200 + 2000); // Board has an 8200 / 2000 ohm voltage divider
442 v /= 2000;
443 return ((uint16_t)v);
444 }
445 return (0);
446
447 }
448
449 private:
450 bool pin3VoltageEnable = false;
451};
452
SerialWombat.h
SW_ERROR_PIN_NOT_CAPABLE
@ SW_ERROR_PIN_NOT_CAPABLE
(#15) The commanded pin does not have the hardware support to perform the commanded pin mode
Definition SerialWombatErrors.h:40
GripACS712
A class representing an Allegro ACS712 5A current sensor connected to a SW8B Analog input on a PCB003...
Definition PCB0031_Grip.h:52
GripACS712::calibrateIdleCurrent
uint16_t calibrateIdleCurrent()
called to set the zero calibration current
Definition PCB0031_Grip.h:91
GripACS712::readCurrent_mA
int16_t readCurrent_mA()
returns the current reading in Milliamps. May be negative
Definition PCB0031_Grip.h:103
GripACS712::zeroCurrentCalibration
uint16_t zeroCurrentCalibration
Variable holding the zero calibration value in counts.
Definition PCB0031_Grip.h:84
GripACS712::GripACS712
GripACS712(SerialWombatChip &serialWombatChip)
Constructor for the GripACS712 Class.
Definition PCB0031_Grip.h:59
GripACS712::begin
int16_t begin(uint8_t pin)
Initialize a GripACS712 on the specified pin.
Definition PCB0031_Grip.h:72
GripServo
A class representing a servo output and an Allegro ACS712 5A current sensor connected to a SW8B Analo...
Definition PCB0031_Grip.h:140
GripServo::calibrateGripper
void calibrateGripper(bool reverse=false)
A single calibration call that will fully calibrate a gripper.
Definition PCB0031_Grip.h:328
GripServo::begin
int16_t begin(byte pin, bool reverse=false)
Initialize a GripServo on the specified pin, and its associated sensor on pin + 4.
Definition PCB0031_Grip.h:163
GripServo::calibratedMaxPosition
uint16_t calibratedMaxPosition
The highest position at which relatively low current was pulled during calibration.
Definition PCB0031_Grip.h:176
GripServo::grip
void grip(uint16_t gripStrength=0, uint16_t slowIncrement=100, uint16_t fastSlowThreshold=350, uint16_t fastIncrement=3500)
Close a servo based gripper with a specified force.
Definition PCB0031_Grip.h:241
GripServo::calibratedMinPosition
uint16_t calibratedMinPosition
The position at which the least current was pulled during calibration.
Definition PCB0031_Grip.h:172
GripServo::GripServo
GripServo(SerialWombatChip &serialWombatChip)
Constructor for the GripServo Class.
Definition PCB0031_Grip.h:147
GripServo::release
void release()
Disable the grip control algorithm and move the servo to the smallest non-stall position.
Definition PCB0031_Grip.h:274
GripServo::calibratedMinCurrent
uint16_t calibratedMinCurrent
The lowest observed current during calibration.
Definition PCB0031_Grip.h:180
GripServo::objectPresent
bool objectPresent(uint16_t divisor=50)
Reports if an object is in the gripper based on stop position of servo.
Definition PCB0031_Grip.h:308
GripServo::calibratedMaxCurrent
uint16_t calibratedMaxCurrent
The highest observed current during calibration.
Definition PCB0031_Grip.h:184
GripServo::sensor
GripACS712 sensor
Definition PCB0031_Grip.h:150
GripServo::calibrateServoRange
void calibrateServoRange(uint16_t expectedCurrentRise=400, uint16_t calibrationStartPosition=0x8000)
Sweep a servo across its range measuring current to determine endstops.
Definition PCB0031_Grip.h:195
PCB0031_Grip::gs3
GripServo gs3
Definition PCB0031_Grip.h:390
PCB0031_Grip::powerVoltage
SerialWombatAnalogInput_18AB powerVoltage
Definition PCB0031_Grip.h:391
PCB0031_Grip::gs0
GripServo gs0
Definition PCB0031_Grip.h:390
PCB0031_Grip::gs2
GripServo gs2
Definition PCB0031_Grip.h:390
PCB0031_Grip::begin
int16_t begin(uint8_t i2cAddress, bool pin3IsVoltage=false)
Initialize a PCB0031_Grip pcb on a given I2C address.
Definition PCB0031_Grip.h:405
PCB0031_Grip::PCB0031_Grip
PCB0031_Grip()
Definition PCB0031_Grip.h:387
PCB0031_Grip::readPowerVoltage_mv
uint16_t readPowerVoltage_mv()
Returns the voltage provided to the Servos in mV.
Definition PCB0031_Grip.h:436
PCB0031_Grip::gs1
GripServo gs1
Definition PCB0031_Grip.h:390
PCB0031_Grip::gsArray
GripServo * gsArray[4]
Definition PCB0031_Grip.h:393
SerialWombatAbstractScaledOutput::writeScalingEnabled
int16_t writeScalingEnabled(bool enabled, uint8_t sourcePin)
Enable scaling and set which pin or public data is used as the input source.
Definition SerialWombatAbstractScaledOutput.h:133
SerialWombatAbstractScaledOutput::writeOutputScaling
int16_t writeOutputScaling(uint16_t outputMin, uint16_t outputMax)
Reduces the output range from 0 to 65535 to user specified range.
Definition SerialWombatAbstractScaledOutput.h:186
SerialWombatAbstractScaledOutput::writeRamp
int16_t writeRamp(uint16_t slowIncrement, uint16_t incrementThreshold, uint16_t fastIncrement, Period samplePeriod, RampMode rampMode)
Configure the scaled output block into Ramp control mode.
Definition SerialWombatAbstractScaledOutput.h:405
SerialWombatAbstractScaledOutput::writeScalingTargetValue
int16_t writeScalingTargetValue(uint16_t target)
The target input value for PID control.
Definition SerialWombatAbstractScaledOutput.h:229
SerialWombatAbstractScaledOutput::RAMP_MODE_BOTH
@ RAMP_MODE_BOTH
Definition SerialWombatAbstractScaledOutput.h:87
SerialWombatAbstractScaledOutput::PERIOD_32mS
@ PERIOD_32mS
Definition SerialWombatAbstractScaledOutput.h:78
SerialWombatAnalogInput_18AB
This class extends SerialWombatAnalogInput with SW18AB specific capabilities.
Definition SerialWombatAnalogInput.h:317
SerialWombatAnalogInput_18AB::pin
uint8_t pin()
Used for inheritance.
Definition SerialWombatAnalogInput.h:322
SerialWombatAnalogInput_18AB::SerialWombatAnalogInput_18AB
SerialWombatAnalogInput_18AB(SerialWombatChip &serialWombat)
Definition SerialWombatAnalogInput.h:320
SerialWombatAnalogInput::readAveragedCounts
uint16_t readAveragedCounts()
Retreive an averaged A/D measurement.
Definition SerialWombatAnalogInput.h:214
SerialWombatAnalogInput::begin
int16_t begin(uint8_t pin, uint16_t averageSamples=64, uint16_t filterConstant=0xFF80, AnalogInputPublicDataOutput output=AnalogInputPublicDataOutput::AnalogInputPublicDataOutput_Raw)
Initialize an analog input on a given pin.
Definition SerialWombatAnalogInput.h:105
SerialWombatChip
Class for a Serial Wombat chip. Each Serial Wombat chip on a project should have its own instance.
Definition SerialWombat.h:300
SerialWombatChip::SerialWombatChip
SerialWombatChip()
Definition SerialWombat.cpp:31
SerialWombatChip::begin
int16_t begin(HardwareSerial &serial, bool reset=true)
initialize a Serial Wombat chip to use a Serial Interface.
Definition SerialWombat.h:495
SerialWombatPin::writePublicData
uint16_t writePublicData(uint16_t value)
Write a 16 bit value to this pin.
Definition SerialWombatPin.h:114
SerialWombatPin::readPublicData
uint16_t readPublicData()
Read the 16 Bit public data associated with this pin.
Definition SerialWombatPin.h:65
SerialWombatPin::_pin
uint8_t _pin
Definition SerialWombatPin.h:205
SerialWombatServo_18AB::SerialWombatServo_18AB
SerialWombatServo_18AB(SerialWombatChip &serialWombat)
Definition SerialWombatServo.h:211
SerialWombatServo_18AB::pin
uint8_t pin()
Facilitates inheritance.
Definition SerialWombatServo.h:214
SerialWombatServo::_max
uint16_t _max
Definition SerialWombatServo.h:171
SerialWombatServo::_min
uint16_t _min
Definition SerialWombatServo.h:170
SerialWombatServo::_reverse
bool _reverse
Definition SerialWombatServo.h:172
SerialWombatServo::attach
void attach(uint8_t pin, bool reverse)
Initialize a servo on the specified pin.
Definition SerialWombatServo.h:92