/*UPWORK PROJECT
LINEAR ACTUATOR CONTROL
15/08/2023
*/
/* PANEL:
LEFT ------HOME --------RIGHT
1 2 3
*/
// INCLUDES +++++++++++++++++
#include "button.h"
#include <EEPROM.h>
#define INCH_TO_CM(I) ((I) * (2.54))
//DEFINING DRIVERS
#define MOTOR_DRIVER "L298" //Define motor driver. Comment this line and uncomment next line if you must use must use the BTS7960
//#define MOTOR_DRIVER "BTS7960" //Define motor driver. Comment this line and uncomment prev line if you must use must use the L298 driver
bool Debug = false; //assign false to turn off debug statments
#define Home 2
#define Left 1
#define Right 3
#define isInitAcceptableVariable 100 //value used to validate if system is properly initialized.
#define isCallibAcceptableVariable 101 // //value used to validate if system is properly calibrated.
#define systemInitDuration 30000 // timeout for system initialization
#define longPressDuration 10000 // 10s
/* EEPROM ADDRESSES
Four Actuators with four pots ; p1,p2,p3 and p4
P1, P2 are on the same quadrant. p3, p4 are on the complimentary side
For each port, p# -> Addresses (p#AMin, p#AMax, p#ALeft, p#AHome, p#ARight)
*/
//P1 ----------------------------------------------
#define p1AddMin 0
#define p1AddMax 1
#define p1AddLeft 2
#define p1AddHome 3
#define p1AddRight 4
//P2 QUAD 1
#define p2AddMin 5
#define p2AddMax 6
#define p2AddLeft 7
#define p2AddHome 8
#define p2AddRight 9 //###########################
//P3 -------------------------------------------------
#define p3AddMin 10
#define p3AddMax 11
#define p3AddLeft 12
#define p3AddHome 13
#define p3AddRight 14
//P4 QUAD 2
#define p4AddMin 15
#define p4AddMax 16
#define p4AddLeft 17
#define p4AddHome 18
#define p4AddRight 19// #############################
#define isInitAdd 20 // address holding values used to check if system is initialized.
#define isCalibAdd 21 // address holding values used to check if system is calibrated.
//Motors- 1 & 2 belong to one quadrant and should be mounted in similar fashion. 2&3 are mounted on the other quadrant and are complemetary in action to 1 &2
#define motor1 1
#define motor2 2
#define motor3 3
#define motor4 4
//Values for loop press Monitoring
#define lPress 0
#define hPress 1
#define rPress 2
#define lLongPressed 3
#define hLongPressed 4
#define rLongPressed 5
#define LRlongPressed 6
int lastPressMode = -1; //-1 is a placeholder. would be replaced by values for loop monitoring
/* callibration addresses
for every position, pos, we have calibration addresses
\ /
\ /
\ Left Tilt / Right Tilt
\ /
// */
//#define defaultActuatorValue 512
// QUICK TWEAKS ##############################################################
//Pins ++++++++++++++++++++++
const int quad1Pot1 = A0, quad1Pot2 = A1, quad2Pot1 = A6, quad2Pot2 = A7; //Potentiometer Pins for motors 1,2,3 and 4 respectively.
//Driver Params
const int quad1driver1Enable = 25, quad1driver2Enable = 27, quad2driver1Enable = 29, quad2driver2Enable = 31; //used to turn on drivers
const int in1_driv1 = 8, in2_driv1 = 9, in3_driv1 = 10, in4_driv1 = 11; // Pins for controlling motor driver.... For BTS7960 use only first two pins
const int in1_driv2 = 12, in2_driv2 = 13, in3_driv2 = 14, in4_driv2 = 15; // Pins for controlling motor driver.... For BTS7960 use only first two pins
int baudrate = 9600; //BAUDRATE
const int initLED = 34, calibLED = 36, readyLED = 38; //LEDs for indicating if system is init, calibrated and or ready
//BUTTON PARAMS
const int leftB = 18, homeB = 19, rightB = 20; // left, home and right button pins
int debounceDuration = 100; //push button debounce duration. Recommend 50ms for common arduino push buttons
bool buttonLookOutState = false; // logic is pulled low (false) when button is pressed.
//const int leftButtonLEDPin = 44, homeButtonLEDPin = 45, rightButtonLEDPin = 46;
//ACTUATOR PARAMS
int actuatorTolerance = 30; //Toleranc values when moving actuator. decrease for more precision.May lead to oscilliations if too small
float actuatorTimeout = 30000; //wait time for actuators to fully extend or contract
// // ################################################################
//GLOBAL VARIABLES ++++++++++++++++++++++
int currentPosition = Home;
//-------------
//Function Declarations ++++++++++++++
void pressActionLeft();
void pressActionHome();
void pressActionRight();
//bool debounce(button bt);
void blink(int led, int duration = 1000, int itr = 1); //blinks led to get attention. used for calibration and inititialization
void ledON(int led, bool state = true); //turns led on or off
int eRead(int addr, int outRange = 1023, bool test = false);
bool isInit(); //checks if system is previously initialized
bool systemInit(); //initialize system
int curPos(int potPin); //gets actuator pot pin and returns current position of actuator. Range 0 - 1023.0
bool updateMotors(int d1, int d2, int d3, int d4, float timeout = actuatorTimeout); //Takes the desired positon of the Four different motors and tries moving actuators to these positions
float tSince(long int startTime); //returns how much time has elapsed since startTime in Seconds
bool outOfRange(int currentValue, int desiredValue, int tolerance = actuatorTolerance); // returns true if current value is different from desired +- tolerance value
void extendMotor( int mot, int desiredPosition, int currentPosition, int motorPositionStatus );
void retractMotor( int mot, int desiredPosition, int currentPosition, int motorPositionStatus );
void stopMotor( int mot, int desiredPosition, int currentPosition, int motorPositionStatus );
void xCalibrate(int position); //longpressing any buttom calibrates the actuatro params and saves the current posiotion as desired button position //reads actuator current positions and saves in EEPROM as desired positional value for a given button
void eWrite4(int add1, int add2, int add3, int add4, int val1, int val2, int val3, int val4, float range = 1023.0, bool test = false); // gets four memorey addresses and values and write all four to EEPROM. Range is the max value of value being written
void eWrite(int addr, int value, float range = 1023.0, bool test = false);
void eWriteTest(int add);
// //-------------------
//Button Objects
button
leftBO(leftB, 'L', debounceDuration, longPressDuration, buttonLookOutState ),
homeBO(homeB, 'H', debounceDuration, longPressDuration, buttonLookOutState),
rightBO(rightB, 'R', debounceDuration, longPressDuration, buttonLookOutState); //create and init button objects
//---------------------------------------
void setup() {
Serial.begin(baudrate);
pinMode(readyLED, OUTPUT);
pinMode(calibLED, OUTPUT);
pinMode(initLED, OUTPUT);
ledON(readyLED, false);
ledON(calibLED, false);
ledON(initLED, false);
pinMode(in1_driv1, OUTPUT);
pinMode(in2_driv1, OUTPUT);
pinMode(in3_driv1, OUTPUT);
pinMode(in4_driv1, OUTPUT);
pinMode(in1_driv2, OUTPUT);
pinMode(in2_driv2, OUTPUT);
pinMode(in3_driv2, OUTPUT);
pinMode(in4_driv2, OUTPUT);
//Setup and Enable Drivers
pinMode(quad1driver1Enable, OUTPUT);
pinMode(quad1driver2Enable, OUTPUT);
pinMode(quad2driver1Enable, OUTPUT);
pinMode(quad2driver2Enable, OUTPUT);
delay(100);
digitalWrite(quad1driver1Enable, HIGH); // Turn on driver module .
digitalWrite(quad1driver2Enable, HIGH); // Turn on driver module .
digitalWrite(quad2driver1Enable, HIGH); // Turn on driver module .
digitalWrite(quad2driver2Enable, HIGH); // Turn on driver module .
//CHECK IF SYSTEM IS INIT
while (!isInit()) {
blink(initLED, 200);
Serial.println ("System not initiazed.... Hold left and Right buttons for 20s to initialized");
//delay(1000);
if (( leftBO.isLongPressedX()) && (rightBO.isLongPressedX())) {
Serial.println("Starting Init");
systemInit();
Serial.println("\nDone initializing System....");
// leftBO.stateUpdate(false);
// rightBO.stateUpdate(false);
break;
}
}
int d1 = eRead(p1AddMax) / 2; int d2 = eRead(p2AddMax) / 2; int d3 = eRead(p3AddMax) / 2; int d4 = eRead(p4AddMax) / 2;
ledON(calibLED, true);
updateMotors(d1, d2, d3, d4);
currentPosition = Home;
//sensorCurrent = actuatorVoltage / potResistance;
blink(readyLED, 1000, 2);
ledON(readyLED);
Serial.println("Done.");
Serial.println("Position -> Left"); //Default to home position
}
void loop() {
Debug ? Serial.println("In loop") : 1;
if (Debug) delay(1000);
if ( (rightBO.isLongPressedX() && leftBO.isLongPressedX() ) && (lastPressMode != LRlongPressed ) ) { //Init
Serial.println("Starting Init");
systemInit();
Serial.println("\nDone initializing System....");
lastPressMode = LRlongPressed;
}
else if ( (leftBO.isLongPressedX() ) && (lastPressMode != lLongPressed ) ) { //Calib Right Position
Serial.println("Left Button is being long pressed\n Calibrating Left tilt");
blink(calibLED);
xCalibrate(Left);
Serial.println("Done");
lastPressMode = lLongPressed;
}
else if ( (rightBO.isLongPressedX() ) && (lastPressMode != rLongPressed ) ) { //calib Right
Serial.println("Right Button is being long pressed\n Calibrating Right position");
blink(calibLED);
xCalibrate(Right);
Serial.println("Done");
lastPressMode = rLongPressed;
}
else if ( (homeBO.isLongPressedX() ) && (lastPressMode != hLongPressed ) ) { //calib Home
Serial.println("Home Button is being long pressed\n Calibrating Home position");
blink(calibLED);
xCalibrate(Home);
Serial.println("Done");
lastPressMode = hLongPressed;
}
else if ( (leftBO.isPressed() ) && (lastPressMode != lPress ) ) { // left action
pressActionLeft();
lastPressMode = lPress;
}
else if ( (rightBO.isPressed() ) && (lastPressMode != rPress ) ) { // right action
pressActionRight();
lastPressMode = rPress;
}
else if ( (homeBO.isPressed() ) && (lastPressMode != hPress) ) { // home action
pressActionHome();
lastPressMode = hPress;
}
else {
//Do nothing. Invalid button combination
Debug ? Serial.println("Invalid Button pressed at loop function") : 1;
}
//
Debug ? Serial.println("out of loop") : 1;
}
void pressActionLeft() {
Serial.print("Current Position"); Serial.println (currentPosition);
int d1 = eRead(p1AddLeft); int d2 = eRead(p2AddLeft); int d3 = eRead(p3AddLeft); int d4 = eRead(p4AddLeft);
if (currentPosition == Right) {
updateMotors(d1, d2, d3, d4);
Serial.print("move from right to left position "); // Serial.println(eRead(rightCaliAdd));
currentPosition = Left;
}
else if (currentPosition == Left) {
updateMotors(d1, d2, d3, d4);
Serial.println(" maintain left position ");
currentPosition = Left;
}
else if (currentPosition == Home) {
updateMotors(d1, d2, d3, d4);
Serial.println(" move from home to left ");
currentPosition = Left;
}
else {
Serial.println(" default ");
updateMotors(eRead(p1AddHome), eRead(p2AddHome), eRead(p3AddHome), eRead(p4AddHome));
currentPosition = Home;
}
delay(1000);
}//
void pressActionHome() {
int d1 = eRead(p1AddHome); int d2 = eRead(p2AddHome); int d3 = eRead(p3AddHome); int d4 = eRead(p4AddHome);
if ( currentPosition == Home ) {
updateMotors(d1, d2, d3, d4);
Serial.println(" Maintain home position ");
}
else if ( currentPosition == Right) {
updateMotors(d1, d2, d3, d4);
Serial.println(" move from right to home position ");
}
else if (currentPosition == Left) {
updateMotors(d1, d2, d3, d4);
Serial.println(" Move from left to home position ");
}
else {
Serial.println(" default ");
updateMotors(d1, d2, d3, d4);
}
currentPosition = Home;
delay(1000);
}
void pressActionRight() {
int d1 = eRead(p1AddRight); int d2 = eRead(p2AddRight); int d3 = eRead(p3AddRight); int d4 = eRead(p4AddRight);
if (currentPosition == Home) {
updateMotors(d1, d2, d3, d4);
Serial.println("Move from home to right position");
currentPosition = Right;
}
else if (currentPosition == Right) {
updateMotors(d1, d2, d3, d4);
Serial.println(" maintain current right position ");
currentPosition = Right;
}
else if (currentPosition == Left) {
updateMotors(d1, d2, d3, d4);
Serial.println(" move from left position to Right position ");
currentPosition = Right;
}
else {
Serial.println(" default ");
updateMotors(eRead(p1AddHome), eRead(p2AddHome), eRead(p3AddHome), eRead(p4AddHome));
currentPosition = Home;
}
delay(1000);
}
//----------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------
int eRead(int addr, int outRange = 1023, bool test = false) {
int val = EEPROM.read(addr);
delay(10);
val = constrain( (map (val, 0, 255, 0, outRange)), 0, outRange); //
//val = val? val: defaultActuatorValue;
Serial.print("\nValue in ADD" + String (addr) + " : "); Serial.println(val);
return val;
}
void eWrite4(int add1, int add2, int add3, int add4, int val1, int val2, int val3, int val4, float range = 1023.0, bool test = false) {
float mult = 255.0 / 1023.0;
float vf1 = (mult * val1) + 0.5; //add 0.5 to round
float vf2 = (mult * val2) + 0.5; //add 0.5 to round
float vf3 = (mult * val3) + 0.5; //add 0.5 to round
float vf4 = (mult * val4) + 0.5; //add 0.5 to round
//results
int r1 = int (vf1);
int r2 = int (vf2);
int r3 = int (vf3);
int r4 = int (vf4);
int v1 = constrain(r1, 0, 255);
int v2 = constrain(r2, 0, 255);
int v3 = constrain(r3, 0, 255);
int v4 = constrain(r4, 0, 255);
EEPROM.update(add1, v1 );
delay(10);
EEPROM.update(add2, v2 );
delay(10);
EEPROM.update(add3, v3 );
delay(10);
EEPROM.update(add4, v4 );
delay(10);
}
void eWrite(int addr, int value, float range = 1023.0, bool test = false) {
//float val1 = (map (value, 0, 1023.0, 0, 255.0 ) ) + 0.5; //add 0.5 to round
float val1 = (255.0 * value) / 1023.0;
val1 += 0.5; //add 0.5 to round
int val = int(val1);
val = constrain(val, 0, 255);
if (Debug) {
Serial.println("Writng to EEPROM via eWrite Function ---------------------------------- ");
Serial.print("Org Valeu: "); Serial.print(value); Serial.print("\n");
Serial.print("Maped Value Valeu: "); Serial.print(val1); Serial.print("\n");
Serial.print("Constrained Valeu: "); Serial.print(val); Serial.print("\n");
Serial.print("Final Valeu: "); Serial.print(val); Serial.print(" | ");
Serial.println("--------------------------------------------------------------------------");
}
EEPROM.update(addr, val);
delay(20);
// if (Debug) {
// Serial.println("Verifying value Function ---------------------------------- ");
// Serial.print("Stored Valeu: "); Serial.print(EEPROM.read(addr)); Serial.print("\n");
// Serial.print("converted Value Valeu: "); Serial.print(eRead(addr)); Serial.print("\n");
// }
}
void xCalibrate(int position) {
int p1 = analogRead(quad1Pot1); int p2 = analogRead(quad1Pot2); int p3 = analogRead(quad2Pot1); int p4 = analogRead(quad2Pot2);
if (position == Left) {
eWrite4(p1AddLeft, p2AddLeft, p3AddLeft, p4AddLeft, p1, p2, p3, p4 );
Debug ? Serial.println("Left position calibatted successfully") : 1;
}
else if (position == Home) {
eWrite4(p1AddHome, p2AddHome, p3AddHome, p4AddHome, p1, p2, p3, p4 );
Debug ? Serial.println("Home position calibatted successfully") : 1;
}
else if (position == Right) {
eWrite4(p1AddRight, p2AddRight, p3AddRight, p4AddRight, p1, p2, p3, p4 );
Debug ? Serial.println("Right position calibatted successfully") : 1;
}
else {
//Invalid Position
Debug ? Serial.println("Invalid Position assigned to xCalibrate Function... Calibration failed") : 1;
}
}
bool isInit() {
if (EEPROM.read(isInitAdd) == isInitAcceptableVariable) {
ledON(initLED);
return true;
}
else {
ledON(initLED, false);
return false;
}
}
bool isCalib() {
if (EEPROM.read(isCalibAdd) == isCallibAcceptableVariable) {
ledON(calibLED);
return true;
}
else {
ledON(calibLED, false);
return false;
}
}
bool systemInit() {
extendMotor(motor1, -1, -1, false);
extendMotor(motor2, -1, -1, false);
retractMotor(motor3, -1, -1, false);
retractMotor(motor4, -1, -1, false);
delay(systemInitDuration / 2);
stopMotor(motor1, -1, -1, false);
stopMotor(motor2, -1, -1, false);
stopMotor(motor3, -1, -1, false);
stopMotor(motor4, -1, -1, false);
int cp1 = curPos(quad1Pot1), cp2 = curPos(quad1Pot2), cp3 = curPos(quad2Pot1), cp4 = curPos(quad2Pot2);
eWrite4(p1AddMax, p2AddMax, p3AddMin, p4AddMin, cp1, cp2, cp3, cp4);
retractMotor(motor1, -1, -1, false);
retractMotor(motor2, -1, -1, false);
extendMotor(motor3, -1, -1, false);
extendMotor(motor4, -1, -1, false);
delay(systemInitDuration / 2);
stopMotor(motor1, -1, -1, false);
stopMotor(motor2, -1, -1, false);
stopMotor(motor3, -1, -1, false);
stopMotor(motor4, -1, -1, false);
cp1 = curPos(quad1Pot1); cp2 = curPos(quad1Pot2); cp3 = curPos(quad2Pot1); cp4 = curPos(quad2Pot2);
eWrite4(p1AddMin, p2AddMin, p3AddMax, p4AddMax, cp1, cp2, cp3, cp4);
//After Initializing System for first time, write init value
EEPROM.update(isInitAdd, isInitAcceptableVariable );
delay(10);
if (EEPROM.read(isInitAdd) == isInitAcceptableVariable) {
ledON(initLED);
return true;
}
else {
ledON(initLED, false);
return false;
}
}
bool updateMotors(int d1, int d2, int d3, int d4, float timeout = actuatorTimeout ) {
bool s1 = false, s2 = false, s3 = false, s4 = false, isSucceed = false; //motor monitoring states. true if motor is moved to correct positions
//Takes the desired positons, d1 -d4 of the Four different motors and tries moving actuators to these positions
float timeoutSeconds = (actuatorTimeout) / 1000.0;
int emin1 = eRead(p1AddMin), emin2 = eRead(p2AddMin), emin3 = eRead(p3AddMin), emin4 = eRead(p4AddMin),
emax1 = eRead(p1AddMax), emax2 = eRead(p2AddMax), emax3 = eRead(p3AddMax), emax4 = eRead(p4AddMax) ;
//verify if desired values are withing acceptable range
if ( ( (d1 < emin1 ) || (d1 > emax1 ) ) ||
( (d2 < emin2 ) || (d2 > emax2 ) ) ||
( (d3 < emin3 ) || (d3 > emax3) ) ||
( (d4 < emin4 ) || (d4 > emax4) )
) {
if (Debug) {
Serial.println("Failed to move Actuators to positions. Requested position is out of actuator range\n\t\t\t DETAITLS....\n" );
//TABLE
Serial.println(" ACTUATOR | MIN_ACEPTABLE_VAL_FROM_INIT | DESIRED_VALUE | MAX_ACEPTABLE_VAL_FROM_INIT");
Serial.print(" MOTOR 1 | "); Serial.print(emin1); Serial.print(" | ");
Serial.print(d1); Serial.print(" | "); Serial.print(emax1); Serial.print("\n");
Serial.print(" MOTOR 2 | "); Serial.print(emin2); Serial.print(" | ");
Serial.print(d2); Serial.print(" | "); Serial.print(emax2); Serial.print("\n");
Serial.print(" MOTOR 3 | "); Serial.print(emin3); Serial.print(" | ");
Serial.print(d3); Serial.print(" | "); Serial.print(emax3); Serial.print("\n");
Serial.print(" MOTOR 4 | "); Serial.print(emin4); Serial.print(" | ");
Serial.print(d4); Serial.print(" | "); Serial.print(emax4); Serial.print("\n");
}
return false;
}
long int timerStart = millis(); //make time of operation start
while ((tSince(timerStart) <= timeoutSeconds ) //while not timeout
&& (
outOfRange( curPos(quad1Pot1), d1) ||
outOfRange( curPos(quad1Pot2), d2) || //and actuators have not reached desired values
outOfRange( curPos(quad2Pot1), d3) ||
outOfRange( curPos(quad2Pot2), d4)
)
)
{ //Update motors ----------------------------------------------------------------------------------------
//motor1 1111111111111111111111111111111111111111111111111111111111111
int cp1 = curPos(quad1Pot1) ;
if (cp1 > (d1 + actuatorTolerance) ) // if motor is more extended than needed, retractMotor
retractMotor(motor1, d1, cp1, s1 );
else if (cp1 < (d1 - actuatorTolerance))
extendMotor(motor1, d1, cp1, s1 );
else {
s1 = true;
stopMotor(motor1, d1, cp1, s1 );// stop motor if it is within acceptable range
}
//````````````````````````````````````````````````````````````````
//motor2 2222222222222222222222222222222222222222222222222222222222
int cp2 = curPos(quad1Pot2);
if (cp2 > (d2 + actuatorTolerance) ) // if motor is more extended than needed, retractMotor
retractMotor(motor2, d2, cp2, s2);
else if (cp2 < (d2 - actuatorTolerance))
extendMotor(motor2, d2, cp2, s2);
else {
s2 = true;
stopMotor(motor2, d2, cp2, s2); // stop motor if it is within acceptable range
}
//````````````````````````````````````````````````````````````````
//motor3 3333333333333333333333333333333333333333333333333333333
int cp3 = curPos(quad2Pot1) ;
if ( cp3 > (d3 + actuatorTolerance) ) // if motor is more extended than needed, retractMotor
retractMotor(motor3, d3, cp3, s3);
else if (cp3 < (d3 - actuatorTolerance))
extendMotor(motor3, d3, cp3, s3);
else {
s3 = true;
stopMotor(motor3, d3, cp3, s3); // stop motor if it is within acceptable range
}
//`
//motor4 44444444444444444444444444444444444444444444444444444444
int cp4 = curPos(quad2Pot2);
if ( cp4 > (d4 + actuatorTolerance) ) // if motor is more extended than needed, retractMotor
retractMotor(motor4, d4, cp4, s4);
else if (cp4 < (d4 - actuatorTolerance))
extendMotor(motor4, d4, cp4, s4);
else {
s4 = true;
stopMotor(motor4, d4, cp4, s4); // stop motor if it is within acceptable range
}
//`
if (s1 && s2 && s3 && s4) { //break out of loop if all motors have reaced desired positions.
isSucceed = true;
break;
}
//END UPDATE MOTORS ---------------------------------------------------------------------------------------
}
if (!isSucceed) { //Timeout
Serial.println("/n/////////////////////// TIMEOUT FAILED TO MOVE ALL MOTORS TO DESIRED POSITION WITHING SPECIFIED TIME ///////////////////////////");
Serial.println("STOPPING.....................");
stopMotor(motor1, d1, -1 , false); // stop motor if it is within acceptable range
stopMotor(motor2, d2, -1 , false); // stop motor if it is within acceptable range
stopMotor(motor3, d3, -1 , false); // stop motor if it is within acceptable range
stopMotor(motor4, d4, -1 , false); // stop motor if it is within acceptable range
Serial.println("......................STOPPED");
Serial.println("/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////n");
delay(2000);
}
}
int curPos(int potPin) {
//gets actuator pot pin and returns current position of actuator. Range 0 - 1023.0
return analogRead(potPin);
}
float tSince(long int startTime) {
//returns how much time has elapsed since startTime in Seconds
return (float(millis() - startTime) / 1000.00);
}
bool outOfRange(int currentValue, int desiredValue, int tolerance = actuatorTolerance) {
if ( (currentValue < (desiredValue - tolerance )) || (currentValue > (desiredValue + tolerance )) ) {
if (Debug)
{
Serial.print("\nCurrent Value: " + String(currentValue) + " Desired Value : " + String( desiredValue ) + "\n");
Serial.println("Out of Range for Tolerance value: " + String( tolerance));
}
return true;
}
else {
if (Debug) {
Serial.print("\nCurrent Value: " + String(currentValue) + " Desired Value : " + String( desiredValue ) + "\n");
Serial.println("IN Range for Tolerance value: " + String( tolerance));
}
return false;
}
}
void extendMotor( int mot, int desiredPosition, int currentPosition, int motorPositionStatus ) {
if (Debug) {
Serial.print("EXTENDING MOTOR: ");
Serial.print(mot);
Serial.print(" CurPos: ");
Serial.print(currentPosition);
Serial.print(" | DesPos: ");
Serial.print (desiredPosition);
Serial.print(" | Status: ");
Serial.println(motorPositionStatus);
}
if (MOTOR_DRIVER == "L298" ) {
/*Two drivers with two channels each used to control four motors. controns are
in1_driv1 , in2_driv1 , in3_driv1 , in4_driv1 -> Driver 1
in1_driv2 , in2_driv2 , in3_driv2 , in4_driv2 -> Driver 2
*/
if (mot == 1) { // in1_driv1 , in2_driv1
digitalWrite(in1_driv1, HIGH);
digitalWrite(in2_driv1, LOW);
}
else if (mot == 2) { // in3_driv1 , in4_driv1
digitalWrite(in3_driv1, HIGH);
digitalWrite(in4_driv1, LOW);
}
else if (mot == 3) { //in1_driv2 , in2_driv2
digitalWrite(in1_driv2, HIGH);
digitalWrite(in2_driv2, LOW);
}
else if (mot == 4) { // in3_driv2 , in4_driv2
digitalWrite(in3_driv2, HIGH);
digitalWrite(in4_driv2, LOW);
}
else {
//Invalid motor param
Debug ? Serial.println("Invalid motor " + String(mot) + " was passed.... **(())") : 1;
}
}
else if ( MOTOR_DRIVER == "BTS7960" ) {
/* Four BTS7960 drivers can be used in an L298 configuration where every motor has a single driver. in this case, motorType should be set to L298
in1_driv1 , in2_driv1----| Driver 1 (motor 1) , Driver 2 (motor2) |-------- in3_driv1 , in4_driv1 -> Driver 1
in1_driv2 , in2_driv2----| Driver 3 (motor 3) , Driver 4 (motor4) |-------- in3_driv2 , in4_driv2 -> Driver 2
ALTERNATIVELY:
two BTS7960 drivers can be used, with motors on each quadrant paired and connected to one driver and motors on the other paired and connected to the other.
Motors on each quadrant must be in mounted in similar fashion and alligh perfedtly with each other. This is because this implementation assumes both motors are identical
and in sync.
*/
if ((mot == 1) || (mot == 2)) { // in1_driv1 , in2_driv1
digitalWrite(in1_driv1, HIGH);
digitalWrite(in2_driv1, LOW);
}
else if ((mot == 3) || (mot == 4)) { //in1_driv2 , in2_driv2
digitalWrite(in1_driv2, HIGH);
digitalWrite(in2_driv2, LOW);
}
else {
//Invalid motor param
Debug ? Serial.println("Invalid motor " + String(mot) + " was passed.... **(())") : 1;
}
}
else {
Debug ? Serial.println("Can't move motor in extendMotor Function, Unknown Actuator Drivers Selected... Acceptable drivers are BTS7960 or L298" ) : 1;
}
}
void retractMotor( int mot, int desiredPosition, int currentPosition, int motorPositionStatus ) {
if (Debug) {
Serial.print("RETRACT MOTOR: ");
Serial.print(mot);
Serial.print(" CurPos: ");
Serial.print(currentPosition);
Serial.print(" | DesPos: ");
Serial.print (desiredPosition);
Serial.print(" | Status: ");
Serial.println(motorPositionStatus);
}
if (MOTOR_DRIVER == "L298" ) {
/*Two drivers with two channels each used to control four motors. controns are
in1_driv1 , in2_driv1 , in3_driv1 , in4_driv1 -> Driver 1
in1_driv2 , in2_driv2 , in3_driv2 , in4_driv2 -> Driver 2
*/
if (mot == 1) { // in1_driv1 , in2_driv1
digitalWrite(in1_driv1, LOW);
digitalWrite(in2_driv1, HIGH);
}
else if (mot == 2) { // in3_driv1 , in4_driv1
digitalWrite(in3_driv1, LOW);
digitalWrite(in4_driv1, HIGH);
}
else if (mot == 3) { //in1_driv2 , in2_driv2
digitalWrite(in1_driv2, LOW);
digitalWrite(in2_driv2, HIGH);
}
else if (mot == 4) { // in3_driv2 , in4_driv2
digitalWrite(in3_driv2, LOW);
digitalWrite(in4_driv2, HIGH);
}
else {
//Invalid motor param
Debug ? Serial.println("Invalid motor " + String(mot) + " was passed.... **(())") : 1;
}
}
else if ( MOTOR_DRIVER == "BTS7960" ) {
/* Four BTS7960 drivers can be used in an L298 configuration where every motor has a single driver. in this case, motorType should be set to L298
in1_driv1 , in2_driv1----| Driver 1 (motor 1) , Driver 2 (motor2) |-------- in3_driv1 , in4_driv1 -> Driver 1
in1_driv2 , in2_driv2----| Driver 3 (motor 3) , Driver 4 (motor4) |-------- in3_driv2 , in4_driv2 -> Driver 2
ALTERNATIVELY:
two BTS7960 drivers can be used, with motors on each quadrant paired and connected to one driver and motors on the other paired and connected to the other.
Motors on each quadrant must be in mounted in similar fashion and alligh perfedtly with each other. This is because this implementation assumes both motors are identical
and in sync.
*/
if ((mot == 1) || (mot == 2)) { // in1_driv1 , in2_driv1
digitalWrite(in1_driv1, LOW);
digitalWrite(in2_driv1, HIGH);
}
else if ((mot == 3) || (mot == 4)) { //in1_driv2 , in2_driv2
digitalWrite(in1_driv2, LOW);
digitalWrite(in2_driv2, HIGH);
}
else {
//Invalid motor param
Debug ? Serial.println("Invalid motor " + String(mot) + " was passed.... **(())") : 1;
}
}
else {
Debug ? Serial.println("Can't move motor in retractMotor Function, Unknown Actuator Drivers Selected... Acceptable drivers are BTS7960 or L298" ) : 1;
}
}
void stopMotor( int mot, int desiredPosition, int currentPosition, int motorPositionStatus ) {
if (Debug) {
Serial.print("STOP MOTOR: ");
Serial.print(mot);
Serial.print(" CurPos: ");
Serial.print(currentPosition);
Serial.print(" | DesPos: ");
Serial.print (desiredPosition);
Serial.print(" | Status: ");
Serial.println(motorPositionStatus);
}
if (MOTOR_DRIVER == "L298" ) {
/*Two drivers with two channels each used to control four motors. controns are
in1_driv1 , in2_driv1 , in3_driv1 , in4_driv1 -> Driver 1
in1_driv2 , in2_driv2 , in3_driv2 , in4_driv2 -> Driver 2
*/
if (mot == 1) { // in1_driv1 , in2_driv1
digitalWrite(in1_driv1, LOW);
digitalWrite(in2_driv1, LOW);
}
else if (mot == 2) { // in3_driv1 , in4_driv1
digitalWrite(in3_driv1, LOW);
digitalWrite(in4_driv1, LOW);
}
else if (mot == 3) { //in1_driv2 , in2_driv2
digitalWrite(in1_driv2, LOW);
digitalWrite(in2_driv2, LOW);
}
else if (mot == 4) { // in3_driv2 , in4_driv2
digitalWrite(in3_driv2, LOW);
digitalWrite(in4_driv2, LOW);
}
else {
//Invalid motor param
Debug ? Serial.println("Invalid motor " + String(mot) + " was passed.... **(())") : 1;
}
}
else if ( MOTOR_DRIVER == "BTS7960" ) {
/* Four BTS7960 drivers can be used in an L298 configuration where every motor has a single driver. in this case, motorType should be set to L298
in1_driv1 , in2_driv1----| Driver 1 (motor 1) , Driver 2 (motor2) |-------- in3_driv1 , in4_driv1 -> Driver 1
in1_driv2 , in2_driv2----| Driver 3 (motor 3) , Driver 4 (motor4) |-------- in3_driv2 , in4_driv2 -> Driver 2
ALTERNATIVELY:
two BTS7960 drivers can be used, with motors on each quadrant paired and connected to one driver and motors on the other paired and connected to the other.
Motors on each quadrant must be in mounted in similar fashion and alligh perfedtly with each other. This is because this implementation assumes both motors are identical
and in sync.
*/
if ((mot == 1) || (mot == 2)) { // in1_driv1 , in2_driv1
digitalWrite(in1_driv1, LOW);
digitalWrite(in2_driv1, LOW);
}
else if ((mot == 3) || (mot == 4)) { //in1_driv2 , in2_driv2
digitalWrite(in1_driv2, LOW);
digitalWrite(in2_driv2, LOW);
}
else {
//Invalid motor param
Debug ? Serial.println("Invalid motor " + String(mot) + " was passed.... **(())") : 1;
}
}
else {
Debug ? Serial.println("Can't stop motor in stopMotor Function, Unknown Actuator Drivers Selected... Acceptable drivers are BTS7960 or L298" ) : 1;
}
}
void blink(int led, int duration = 1000, int itr = 1) { //blinks led to get attention. used for calibration and inititialization
for (int i = 0; i < itr; i++) {
digitalWrite(led, HIGH);
delay(duration);
digitalWrite(led, LOW);
delay(duration);
}
}
void ledON(int led, bool state = true) {
digitalWrite(led, state );
}
void eWriteTest(int add) {
if (Serial.available()) {
int val = (Serial.parseInt());
Serial.println("---------------------------------NEW TEST-------------------------------");
Serial.print ("eWriteTest>> Read value:"); Serial.println(val);
eWrite(add, val, true);
Serial.println("-----------------------------TESTING COMPLETE-------------------------");
}
}