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();
}