My motors will not stop in auton please help.

VEX V5 General Discussion Technical Discussion
#1

I’m currently working on an autonomous and for some reason, my mogo lift motors are continuously running even though I have statements from encoder motors. Someone, please help me. My code below:

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  leftBaseEncoder, sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  rightBaseEncoder, sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_3,  leftMogoLIftEncoder, sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port2,           leftBase,      tmotorVex393_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor,  port3,           rightBase,     tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_2)
#pragma config(Motor,  port4,           mogoLiftMotors,    tmotorVex393_MC29, openLoop, encoderPort, I2C_3)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

const float baseWheelLength = 11.5; //In inches, from front wheel to bottom wheel
const float baseWheelWidth = 13.25; //In inches, from left wheel to right wheel
const float wheelDiameter = 4; //In inches, diameter of omniwheel
const float mogoLiftGearRatio = 5; //driving gear teeth over driven gear teeth, gear ratio is 12 dividing by 60 (12 / 60)

void driveStraight(float distance, float speed, int direction) //distance in inches, speed from 1 to 127, direction only -1 or 1
{

	while(fabs(SensorValue[leftBaseEncoder]) < ((distance / (wheelDiameter * PI)) * 3600) &&
		fabs(SensorValue[rightBaseEncoder]) < ((distance / (wheelDiameter * PI)) * 3600))
	{

		motor[leftBase] = speed * direction;
		motor[rightBase] = speed * direction;

	}

}

void pivotRobot(float desiredAngle, int leftOrRight, float speed) //desiredAnlge in degrees, leftOrRight only -1 for left and only 1 for
{																																	//right, speeed from 1 to 127

	while(fabs(SensorValue[leftBaseEncoder]) < (((desiredAngle * sqrt(pow(baseWheelLength, 2) + pow(baseWheelWidth, 2))) / wheelDiameter) * 10) &&
		fabs(SensorValue[rightBaseEncoder]) < (((desiredAngle * sqrt(pow(baseWheelLength, 2) + pow(baseWheelWidth, 2))) / wheelDiameter)) * 10)
	{

		motor[leftBase] = speed * leftOrRight;
		motor[rightBase] = -1 * speed * leftOrRight;

	}

}

/*void moveMogoLift(float mogoLiftGearEncoderValue, float speed, int inOrOut) //mogoLiftGearEncoderValue is wanted angle rotation on the gear
{																																						//that the lift is attached to, speed from 1 to 127, inOrOut
	//only -1 for in and only 1 for out
	while(fabs(SensorValue[leftMogoLIftEncoder]) < ((mogoLiftGearEncoderValue * mogoLiftGearRatio) * 10))
	{

		motor[mogoLiftMotors] = speed * inOrOut;

	}

}*/

void moveMogoLift(bool goInOrOut, float speed) //in is true and out is false, speed is from 1 to 127
{

	if(goInOrOut)
	{

		while(SensorValue[leftMogoLIftEncoder] < -10 * 10 * mogoLiftGearRatio)
		{

			motor[mogoLiftMotors] = speed * 1;

		}

	}

	else 
	{

		while(SensorValue[leftMogoLIftEncoder] > -100 * 10 * mogoLiftGearRatio)
		{
			
			motor[mogoLiftMotors] = speed * -1;
		
		}

	}
	
	motor[mogoLiftMotors] = 0;
	wait1Msec(100);

}

void resetSensors()
{

	SensorValue[leftBaseEncoder] = 0;
	SensorValue[rightBaseEncoder] = 0;
	SensorValue[leftMogoLIftEncoder] = 0;

}

//Above is the first file, below is second

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  leftBaseEncoder, sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_2,  rightBaseEncoder, sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Sensor, I2C_3,  leftMogoLIftEncoder, sensorQuadEncoderOnI2CPort,    , AutoAssign )
#pragma config(Motor,  port2,           leftBase,      tmotorVex393_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor,  port3,           rightBase,     tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_2)
#pragma config(Motor,  port4,           mogoLiftMotors,    tmotorVex393_MC29, openLoop, encoderPort, I2C_3)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "robotFunctionsList.c"

task main() //RightMogoTo20PtZoneAuton
{

	//moveMogoLift(50, 120, -1); //Put down and out mogo lift
	moveMogoLift(false, 120);
	resetSensors();
	driveStraight(52, 120, 1); //Drive to right mogo
	resetSensors();
	//moveMogoLift(50, 120, 1); //Pick up right mogo
	moveMogoLift(true, 120);
	resetSensors();
	pivotRobot(165.068582822, -1, 100); //Pivot the robot to face perpendicular bisector of big bar
	resetSensors();
	driveStraight(93.1450481776, 120, 1); //Drive to perpendicular bisector of big bar
	resetSensors();
	pivotRobot(59.9314171781, -1, 100); //Rotate to face to big bar
	resetSensors();
	driveStraight(25.4558441227, 120, 1); //Drive to big bar
	resetSensors();
	//moveMogoLift(50, 120, -1); //Place down mogo into 20 point zone
	moveMogoLift(false, 120);
	resetSensors();

}
#2

Motors don’t stop because you don’t tell them to stop. You are missing a motor] = 0 command at the end of your functions. At a minimum add this after the loop in drive straight:

motor[leftBase] = 0;
motor[rightBase] = 0;

However, that is going to stop the motors but due to momentum your robot will roll quite a bit. Ideally, you’d have a PID loop (lots of forum threads about that, do a search) but a simpler solution (which I can’t really recommend for most cases) is a “break” sort of thing like so:

void driveStraight(float distance, float speed, int direction) //distance in inches, speed from 1 to 127, direction only -1 or 1
{

	while(fabs(SensorValue[leftBaseEncoder]) < ((distance / (wheelDiameter * PI)) * 3600) &&
		fabs(SensorValue[rightBaseEncoder]) < ((distance / (wheelDiameter * PI)) * 3600))
	{

		motor[leftBase] = speed * direction;
		motor[rightBase] = speed * direction;

	}
       motor[leftBase] = 20 * direction * -1;
       motor[rightBase] = 20 * direction * -1; //stop momentum by spinning opposite direction for a bit
       wait(0.1);
       motor[leftBase] =0;
       motor[rightBase] = 0;

}

Bear in mind, this will throw of your robot and can be battery dependant and thus isn’t the best option for accurate results, especially for prog skills or more complex autons. Look into PID is my advice.

#3

That’s great and all and I don’t want to seem like a jerk but I wasn’t talking about those motors. I was talking about the mogo lift motors. But thank you for the feedback! Will take what you said into much consideration.

#4

My bad, I did misread that.

Although, I don’t see where you are stopping those drive motors so u need to implement that because that will be your next immediate problem. The mogo lift motors most likely aren’t stopping because you are never breaking out of the while conditional. In the false case, it seems like you have a typo (but I can’t be sure)

while(SensorValue[leftMogoLIftEncoder] < -10 * 10 * mogoLiftGearRatio)   //Seems like what you want. 

while(SensorValue[leftMogoLIftEncoder] > -100 * 10 * mogoLiftGearRatio) //Typo here, 100 instead of 10
#5

I’m going to look more into those while statements. Can’t I just take the absolute value of the sensor? Without having to deal with negatives?

#6