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 return 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
329 void calibrateGripper(bool reverse = false, uint16_t rangeConstant = 49152)
330 {
331 uint32_t position = 0;
332
333 begin(_pin, reverse);
334 calibratedMinCurrent = 65535;
335 calibratedMaxCurrent = 0;
336 uint16_t minCurrentPosition = 0;
337 for (position = 0; position <= 65000; position += 1000)
338 {
339 writePublicData((uint16_t)position);
340 delay(250);
341 uint16_t current =sensor.readAveragedCounts();
342 if (current < calibratedMinCurrent)
343 {
344 calibratedMinCurrent = current;
345 minCurrentPosition = position;
346 }
347 if (current > calibratedMaxCurrent)
348 {
349 calibratedMaxCurrent = current;
350 }
351 }
352
353 writePublicData(minCurrentPosition);
354 delay(1000);
355 sensor.calibrateIdleCurrent();
356 uint32_t range = (calibratedMaxCurrent - calibratedMinCurrent);
357 range *= rangeConstant;
358 range >>= 16;
359 calibrateServoRange((uint16_t) range, minCurrentPosition);
360
361 }
362
363};
364
387
388 class PCB0031_Grip : public SerialWombatChip
389{
390 public:
391 PCB0031_Grip() : SerialWombatChip(), gs0(*this), gs1(*this), gs2(*this), gs3(*this), powerVoltage(*this)
392 {}
393
394 GripServo gs0,gs1,gs2,gs3;
395 SerialWombatAnalogInput_18AB powerVoltage;
396
397 GripServo* gsArray[4] = { &gs0,&gs1,&gs2,&gs3 };
398
399
409 int16_t begin(uint8_t i2cAddress,bool pin3IsVoltage = false)
410 {
411 SerialWombatChip::begin(Wire, i2cAddress, true);
412 for (int i = 0; i < 3; ++i)
413 {
414 gsArray[i]->begin(i);
415 }
416 if (!pin3IsVoltage)
417 {
418 gs3.begin(3);
419 }
420 else
421 {
422 powerVoltage.begin(3);
423 pin3VoltageEnable = true;
424
425 }
426 return (0);
427 }
428
429
441 uint16_t readPowerVoltage_mv()
442 {
443 if (pin3VoltageEnable)
444 {
445 uint32_t v = powerVoltage.readAveraged_mV();
446 v *= (8200 + 2000); // Board has an 8200 / 2000 ohm voltage divider
447 v /= 2000;
448 return ((uint16_t)v);
449 }
450 return (0);
451
452 }
453
454 private:
455 bool pin3VoltageEnable = false;
456};
457
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::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::calibrateGripper
void calibrateGripper(bool reverse=false, uint16_t rangeConstant=49152)
A single calibration call that will fully calibrate a gripper.
Definition PCB0031_Grip.h:329
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:394
PCB0031_Grip::powerVoltage
SerialWombatAnalogInput_18AB powerVoltage
Definition PCB0031_Grip.h:395
PCB0031_Grip::gs0
GripServo gs0
Definition PCB0031_Grip.h:394
PCB0031_Grip::gs2
GripServo gs2
Definition PCB0031_Grip.h:394
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:409
PCB0031_Grip::PCB0031_Grip
PCB0031_Grip()
Definition PCB0031_Grip.h:391
PCB0031_Grip::readPowerVoltage_mv
uint16_t readPowerVoltage_mv()
Returns the voltage provided to the Servos in mV.
Definition PCB0031_Grip.h:441
PCB0031_Grip::gs1
GripServo gs1
Definition PCB0031_Grip.h:394
PCB0031_Grip::gsArray
GripServo * gsArray[4]
Definition PCB0031_Grip.h:397
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:407
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 publicDataOutput=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:315
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:550
SerialWombatPin::writePublicData
uint16_t writePublicData(uint16_t value)
Write a 16 bit value to this pin.
Definition SerialWombatPin.h:113
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:213
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