While attempting to code the autonomous for our X-Holonomic, we ran into an issue with resetting the values of the quadrature encoders attached to the four drive motors. I have attached the problematic code below. Any help is much appreciated.
task autonomous()
{
//==========================================FORWARD======================================
while((SensorValue[RightEncoder]+SensorValue[RightEncoderF]) < (2*200)) //Forward for defined rotations
{
if(((SensorValue[LeftEncoder]* -1)+(SensorValue[LeftEncoderF]* -1)) < (SensorValue[RightEncoder]+SensorValue[RightEncoderF]))
{ //If the left side has reversed more than the right...
motor[port3] = 115; //slow down the left...
motor[port2] = 127; //and speed up the right
motor[port4] = 115; //slow down the left...
motor[port5] = 127; //and speed up the right
}
if(((SensorValue[LeftEncoder]* -1)+(SensorValue[LeftEncoderF]* -1)) > (SensorValue[RightEncoder]+SensorValue[RightEncoderF]))
{ //If the right side has reversed more than the left...
motor[port3] = 127; //speed up the left...
motor[port2] = 115; //and slow down the right...
motor[port4] = 127; //speed up the left...
motor[port5] = 115; //and slow down the right...
}
if(((SensorValue[LeftEncoder]* -1)+(SensorValue[LeftEncoderF]* -1)) == (SensorValue[RightEncoder]+SensorValue[RightEncoderF]))
{ //If the left and right have reversed the same amount...
motor[port3] = 127; //run the motors at the same speed
motor[port2] = 127;
motor[port4] = 127; //run the motors at the same speed
motor[port5] = 127;
}
}
motor[port2] = 0;
motor[port3] = 0;
motor[port4] = 0;
motor[port5] = 0;
Sensorvalue[RightEncoder] = 0;
Sensorvalue[RightEncoderF] = 0;
Sensorvalue[LeftEncoder] = 0;
Sensorvalue[LeftEncoderF] = 0;
//===========================================END==============================================
wait1Msec(2000);
//==========================================FORWARD======================================
while((SensorValue[RightEncoder]+SensorValue[RightEncoderF]) < (2*200)) //Forward for defined rotations
{
if(((SensorValue[LeftEncoder]* -1)+(SensorValue[LeftEncoderF]* -1)) < (SensorValue[RightEncoder]+SensorValue[RightEncoderF]))
{ //If the left side has reversed more than the right...
motor[port3] = 115; //slow down the left...
motor[port2] = 127; //and speed up the right
motor[port4] = 115; //slow down the left...
motor[port5] = 127; //and speed up the right
}
if(((SensorValue[LeftEncoder]* -1)+(SensorValue[LeftEncoderF]* -1)) > (SensorValue[RightEncoder]+SensorValue[RightEncoderF]))
{ //If the right side has reversed more than the left...
motor[port3] = 127; //speed up the left...
motor[port2] = 115; //and slow down the right...
motor[port4] = 127; //speed up the left...
motor[port5] = 115; //and slow down the right...
}
if(((SensorValue[LeftEncoder]* -1)+(SensorValue[LeftEncoderF]* -1)) == (SensorValue[RightEncoder]+SensorValue[RightEncoderF]))
{ //If the left and right have reversed the same amount...
motor[port3] = 127; //run the motors at the same speed
motor[port2] = 127;
motor[port4] = 127; //run the motors at the same speed
motor[port5] = 127;
}
}
motor[port2] = 0;
motor[port3] = 0;
motor[port4] = 0;
motor[port5] = 0;
Thanks.