I am making an x-drive and some wheels were running faster then they were supposed to so I wrote out some code to fix it. My Genius friend helped me write it and he said it is the same problem he had (the faster motor problem) and he told me exactly how to write it. Here is the code
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1, , sensorQuadEncoderOnI2CPort, , AutoAssign)
#pragma config(Sensor, I2C_2, , sensorQuadEncoderOnI2CPort, , AutoAssign)
#pragma config(Sensor, I2C_3, , sensorQuadEncoderOnI2CPort, , AutoAssign)
#pragma config(Sensor, I2C_4, , sensorQuadEncoderOnI2CPort, , AutoAssign)
#pragma config(Motor, port1, FR, tmotorVex393_HBridge, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor, port2, BL, tmotorVex393_MC29, openLoop, encoderPort, I2C_2)
#pragma config(Motor, port3, BR, tmotorVex393_MC29, openLoop, encoderPort, I2C_3)
#pragma config(Motor, port10, FL, tmotorVex393_HBridge, openLoop, encoderPort, I2C_4)
//!!Code automatically generated by ‘ROBOTC’ configuration wizard !!//
void fixThis (){
float rlRatio = 1.0* motor[FR] / motor[BL]; //finding out the ratio of the motor’s power
float lrRatio = 1.0* motor[FL] / motor[BR];
float rlEncoderRatio = 1.0*SensorValue[I2C_1] / SensorValue[I2C_2]; // findig out the same motor encoder value ratio
float lrEncoderRatio = 1.0*SensorValue[I2C_4] / SensorValue[I2C_3];
float rlDifference = rlEncoderRatio - rlRatio; // Finding the difference of both to see how off th motors power is than it is supposed to be
float lrDifference = lrEncoderRatio - lrRatio;
float lrConstant = 1.4; //The Constant will be used to correct the motor's power later
float rlConstant = 1.4;
if (lrDifference < 0) {
motor[BR] -= lrDifference * lrConstant;
}
if (lrDifference > 0) {
motor[FL] -= lrDifference * lrConstant;
}
if (rlDifference > 0){
motor[BL] -= rlDifference * rlConstant;
}
if (rlDifference < 0) {
motor[FR] -= rlDifference * rlConstant;
}
}
task main()
{
SensorValue[I2C_1] = 1;
SensorValue[I2C_2] = 1;
SensorValue[I2C_3] = 1;
SensorValue[I2C_4] = 1;
while (true) {
motor[FR] = vexRT[Ch3] - vexRT[Ch4] - vexRT[Ch1];
motor[BR] = vexRT[Ch3] - -vexRT[Ch4] + -vexRT[Ch1];
motor[FL] = vexRT[Ch3] + vexRT[Ch4] + vexRT[Ch1];
motor[BL]= vexRT[Ch3] + -vexRT[Ch4] - -vexRT[Ch1];
fixThis ();
}
}
and when I run it on my robot i get this error
Exception Error in User Program:
Exception Type: ‘Divide by zero(91)’
Program Slot: 0, Task ID: main[0]
Error at PC: fixThis+0x0074
TaskState: ‘Exception’
The program allows me to press start but When I do that the robot has no response (the robot had response when the wheels were moving faster than the other wheels) And I checked the local variables in debugging and it said this
rlRatio = 1
lrRatio = 1
rlEncoderRatio = -1.#QO (That is not a mistake in typing)
lrEncoderRatio = 0
rlDifference = 0
lrDifference = 0
lrConstant = 0
rlConstant = 0
I have tried so many things but yet nothing seems to work
please give me some instructions on what I need to do to fix this problem