I'm using an Uno to control a servo using input from an RC remote, read using pulseIn(). The program starts out assuming wherever the servo is, is the default position (number of turns = 0) and will always return to that position when given no throttle, or when braking. The goal is to count the number of turns the servo completes, and not allow it to turn above a certain amount of maximum turns (say, max # of turns = 2). The number of turns is calculated by using a power function that calculates the time to turn based on a given pulse length (experimentally mapped out), and then dividing the time elapsed on that pulse length by the time to rotate (experimental data).
My issue is there's a disconnect between how my counting works and how many turns the servo really does. It will complete around 2-3 turns in reality at full throttle (2000 us pulse length), but program counts a fraction of a turn (~.1 turn). I'm using millis() to count from when it sets the speed to when it adjusts the count of the turns. I don't understand why this is not working the way I want it to. My best guess is that it doesn't like where I've placed the millis() command to count, giving an inaccurate result of timing. I also read that pulseIn() basically stops the CPU from being used, so maybe that's messing up how millis() counts? I tried adding the pulselength (scaled from us to ms) to the time elapsed, but it didn't completely fix the issues.
I've attached my code below, please let me know if anything about it is unclear. I apologize for the excessive commented out prints, there was a lot of debugging involved.
#include <Servo.h>
//"Stop" pulse length is 1500 us, +/- some deadzone
// Max (full throttle) pulse length remote puts out is 2000 us
// Min (full brake) pulse length remote puts out is ~975 us
int pin = 7; //Input pin for reading PWM
signed long pulseLength = 0; //pulseLength of pulse length
signed long prevTime = 0; //Previous time when pulse length gets set
signed long newTime = 0; //New time at start of loop
int deadzone = 75; //Deadzone range
Servo servo1; //Servo
int serPin = 3; // Servo output pin
float maxTurn = 2; //Max # of turns
float numTurns = 0; //Current # of turns
double tEl = 0; //Time elapsed [ms]
int fullBrake = 900; //Brake pulse length [us]
int slowBrake = 1100; //Coasting pulse length [us]
int doNothing = 1500; //"stop" pulse length [us]
void setup() {
servo1.attach(serPin); // setup for the servo, lets us know what pin it's on
Serial.begin(115200); //Serial BAUD rate.
pinMode(pin, INPUT); //Sets up input pin for receiver
}
void loop() {
newTime = millis(); //Gets current time
tEl = (newTime-prevTime) + (pulseLength/1000); //Finds time elapsed from when last loop occured [ms]
numTurns = countTurn(pulseLength, tEl, numTurns); //Adjusts numturns
pulseLength = pulseIn(pin, HIGH);
if(pulseLength > (1500+deadzone)){
if(numTurns < maxTurn) //If not at the max # of turns
servo1.writeMicroseconds(pulseLength);
else{ //If it is, don't turn anymore
servo1.writeMicroseconds(doNothing);
pulseLength = doNothing;
}
}
else if(pulseLength < (1500-deadzone)){ //Brake case
if(numTurns > 0){ //If not below zero turns, brake real fast
servo1.writeMicroseconds(fullBrake);
pulseLength = fullBrake;
}
else{ //Otherwise just don't do anything
servo1.writeMicroseconds(doNothing);
pulseLength = doNothing;
}
}
else{
if(numTurns > 0){ //'Coasting case' where if there's no input (between dead zone) and it's not at 0 turns
//Designed to be a slower braking effect than pulling the remote's throttle all the way down
servo1.writeMicroseconds(slowBrake);
pulseLength = slowBrake;
}
else{ //Otherwise do nothing
servo1.writeMicroseconds(1500);
pulseLength = doNothing;
}
}
prevTime = millis();
//Serial.println("Pulse length:");
//Serial.println(pulseLength);
Serial.println("Num turns:");
Serial.println(numTurns);
delay(100);
}
double countTurn(signed long pL, double dur, float nT){
double ttr = 0; //Time to rotate [s]
int brake = 1; //Determines whether to add or subtract turns
float newTurn = 0; //Amt of turns to be added
pL -= 1500; //Sets pulselength within bounds of power law
//Serial.println("pulseLength input:");
//Serial.println(dur, 9);
if(pL < 0){ //Checks to see if braking
pL = abs(pL); //Gets abs value (should be mirror for reverse)
brake = -1; //Changes to brake mode so nT gets subtracted
}
dur /= 1000; //Scales [ms] to [s]
ttr = 724.96 * pow(pL, -.99); // Gets time required to make a rotation at set speed, accounting for brake or not [s]
newTurn = dur/ttr * brake; // Adds or subtracts amt of turning based on if braking
//Serial.println("pL inside cT:");
//Serial.println(pL);
//Serial.println("Brake value:");
//Serial.println(brake);
//Serial.println("pulseLength after [s]:");
//Serial.println(dur, 9);
//Serial.println("ttr");
//Serial.println(ttr, 9);
//Serial.println("nT added");
//Serial.println(newTurn, 9);
if (abs(pL) <= deadzone){ //If within the deadzone, the # of turns added is set to 0.
newTurn = 0; //This is really just a case to deal with NaN causing issues (i.e 1500 pulse length exactly)
}
return (nT + newTurn);
}
