Emergency Programming Help

Welp, with our planned schedule a bust, we seem to be running out of time. Despite this, I still want to get our autonomous program ready, and while I would love to develop this myself, I can’t. Not within next week anyhow.

So if someone wanted to be super generous, and help me figure out how to get an encoder-based function to work, I would be really grateful.

Basically, I want to use 4 shaft encoders on all 4 of the drive wheels to ensure that it always runs the same distance, and always stops right before the fence. I’m sure there is a way to do that…right?

maybe like:

motor[leftdrive] = 80;
until int(value) =x[x being distance to travel]

int(value) = sensorvalue(leftencoder1) + sensorvalue(leftencoder2) //active at top of auton

and then repeat that for the right…except the left side could technically follow that and still do it faster than the right, which would make it veer right…and to counteract that I would need to…

yeah, I really need help.

Oh right, and also some system to where when the arm lifts up if it hits the fence the arm encoder stops, reading this, auton says “ok we have stopped” and then lowers the arm, reverses a bit, and then tries to lift again.

Since this is a subtask activated after the drive-up-to-fence subtask, we only have to worry about this part.

Something along the lines of:

[all the motor steps that do the lifting]
until sensorvalue(encoderarm) stops
[motor control to correct]
loop back to top

except, how do I write that until part?

I’m not extremely qualified to comment on matters of precise programming, but unless you’re using a tank drive I would recommend you just chain together your wheels on either side and only use one quadrature encoder on each side of your drive. Good luck.

Ok so for the first part: Driving straight until you reach a certain distance.
First, I need to know what type of drive you are using. If it is a tank drive, then you only need two shaft encoders and you could do the following:


void driveForward(int inches) {
int ticks = (int)((inches / WHEEL_RADIUS ) * ( 180 / PI )); //this is a formula that you could use to convert from inches to the equivalent ticks in the shaft encoder.  
                float kp = .7; //this will need to be tuned
		float errorR = nMotorEncoder[RightFront] - ticks; //replace the encoders with what you are using
		float errorL = nMotorEncoder[LeftFront] - ticks;
		int speedR = kp * errorR; //make the speed a proportional value of error
		int speedL = kp * errorL;
                int tolerance = 30; //allow some tolerance
                while(abs(errorL) > tolerance|| abs(errorR) > tolerance) {
                        motor[RightFront] = motor[RightBack] = 	-1 * speedR;
			motor[LeftFront] = motor[LeftBack] = 	-1 * speedL;
			errorR = nMotorEncoder[RightFront] - ticks;
			errorL = nMotorEncoder[LeftFront] - ticks;
			speedR = kp * errorR;
			speedL = kp * errorL;
                }

This is basically the simplest way you could do this. It would make sure that you don’t overshoot the target and since each side’s speed is calculated independently, it will make sure it travels straight. There is more you could do with this and complete PID(search other threads for more info), but this is what we have used.
BTW, if this doesn’t compile, feel free to help me because I kind of had to piece this together since I have a bit more in there that makes it overly complicated so I might have missed a brace or something.

Ok sorry, I really misread your first post. What I previously posted would work better than the following, but this is much simpler:


void driveForward(int inches) {
int ticks = (int)((inches / WHEEL_RADIUS ) * ( 180 / PI )); //this is a formula that you could use to convert from inches to the equivalent ticks in the shaft encoder.  
while(SensorValue[rightEncoder] < ticks || SensorValue[leftEncoder] < ticks) {
      motor[rightMotor] = motor[leftMotor] = 100;
}

Hmm, it would seem I deleted my reply…darn

So it’s currently a 4 independent drive, but I can chain the sides together. I believe I got how this works, just to be sure though, what needs to be adjusted to figure in the exact distance the encoders need to go? I assume finding what the value reads after testing driving manually, and then inputting those values in place of…wait. No, ticks = that formula, so… where would I put them??

When you call the function, the inches parameter needs to be adjusted according to how much you need to travel. So say you would need to go forward for 5 inches. You would do this:


driveForward(5);

The function would take care of the rest.

So replace (int inches) with (x distance)
and, if I replace the encoders and motors, it will work?

Sorry, just want to be super clear

—edit—
oh wait no, replace just both the inches with (value x)…

What would x distance be? What unit?
Yes, if you replace the encoders and motor names to yours, it should work. If it doesn’t, tell me and I’ll fix it.

My bad, just like, the inches from the encoder to the desired resting location of the encoder.

So like if I wanted it to go 25 inches, x would be 25.

I could very likely be misunderstanding something

Exactly!

Dude you’re a lifesaver. I was stuck with the

part.

So I just need to balance the powers of the two sides to keep it straight right?
I assume that first one would correct this automatically, but that should do fine.

Recapping a bit; replace just the inside of

with x value and good to go?
or replace both of the words ‘inches’ with x value, and wheel radius with my wheel radius?
or if it gets complicated, replace everything after int ticks= with the raw data values of the distance?

Comprehension isn’t my best skill.

Ok. So don’t change anything from the original function other than wheel radius. replace that with your own. I think you are confusing making a function with calling it. When you make a function, you want it to be usable multiple times. So here is basically what a simple program would look like if you had an autonomous that went forward 5 inches:



void driveForward(int inches) {
int ticks = (int)((inches / WHEEL_RADIUS ) * ( 180 / PI )); //this is a formula that you could use to convert from inches to the equivalent ticks in the shaft encoder.  
while(SensorValue[rightEncoder] < ticks || SensorValue[leftEncoder] < ticks) {
      motor[rightMotor] = motor[leftMotor] = 100;
}

void pre_auton() {
         
}
task autonomous() {
       driveForward(5);
}

This would result in the robot going forward 5 inches, then it would stop. All you would change is the wheel radius and the motor/sensor names. If you have more than 2 motors, add those as well. When you put the 5 in the parantheses, you are giving the function that number. and since the function accepts one integer number called inches, you can pass, or give it one when you call it. Sorry if you’re confused. As you can probably tell, I’m not the best teacher.

Oh wow that’s even better, I was reading it wrong. Thanks a million!