VIII-Appendix

References

1. https://www.frontiersin.org/articles/10.3389/fmech.2020.609340/full
2. https://journals.sagepub.com/doi/full/10.5772/50917
3. https://link.springer.com/chapter/10.1007/978-3-030-20131-9_2

Code

//import library
#include <SparkFun_TB6612.h>

//define pins
#define AIN1 7
#define AIN2 6
#define PWMA 5
#define STBY 8

int pot = A0;

//offset val
const int offsetA = 1;
float inputVal = 0.0;
int speedVal = 0;
float desiredPotVal = 0.0;
float motorStep = 290.0/1023.0;
float desiredAng = 90.0;
float tolerance = 0.5;
float potValTolerance = 2.0;
float trueAngle = 0.0;
int dt = 5;

//initialize motors
Motor motor1 = Motor(AIN1, AIN2, PWMA, offsetA, STBY);

void setup() {
// put your setup code here, to run once:
pinMode(pot, INPUT);
Serial.begin(9600);
}

void loop() {

float desiredPotVal = desiredAng/motorStep;//(int) floor(desiredAng/motorStep); //value to look for from analog input for the given angle
float inputVal = analogRead(pot);
float potValTolerance = tolerance/motorStep;

//motor needs to turn CCW searching for desiredPot Val
if ((desiredPotVal-potValTolerance) > inputVal){
motor1.drive(150);
float inputVal = analogRead(pot);
Serial.println(desiredPotVal);
Serial.println(inputVal);
delay(dt);
motor1.brake();
}
//motor needs to move CW
else if ((desiredPotVal+potValTolerance) < inputVal){
motor1.drive(-150);
float inputVal = analogRead(pot);
Serial.println(desiredPotVal);
Serial.println(inputVal);
delay(dt);
motor1.brake();
}
else{
motor1.brake();
Serial.println("Exited");
}

}

BOM