/**
Tests that the motors are wired up correctly.
When running, verify that the motor and direction printed over serial is what
is observed on the robot.
Also prints the change in encoder values after each movement - expect the
readings to positive, and correspond with the motor that moved
*/
void tests::motor_wiring() {
Drive md;
md.resetEncoders();
if(md.faulted()) {
Serial.println("Motor driver has a fault");
md.clearFault();
if(md.faulted()) {
Serial.println("Motor driver still has a fault");
return;
}
}
else
Serial.println("Motors are ok");
{
Serial.print("Motor R, forward: ");
uint32_t encr = md.getMREncoder();
uint32_t encl = md.getMLEncoder();
Serial.flush();
md.setMRSpeed(100);
delay(2e3);
md.setMRSpeed(0);
int32_t diffr = md.getMREncoder() - encr;
int32_t diffl = md.getMLEncoder() - encl;
Serial.print("dr =");
Serial.print(diffr);
Serial.print(", dl =");
Serial.println(diffl);
}
if(md.faulted())
Serial.println("Motor driver has a fault");
{
Serial.print("Motor L, forward: ");
uint32_t encr = md.getMREncoder();
uint32_t encl = md.getMLEncoder();
Serial.flush();
md.setMLSpeed(100);
delay(2e3);
md.setMLSpeed(0);
int32_t diffr = md.getMREncoder() - encr;
int32_t diffl = md.getMLEncoder() - encl;
Serial.print("dr =");
Serial.print(diffr);
Serial.print(", dl =");
Serial.println(diffl);
}
if(md.faulted())
Serial.println("Motor driver has a fault");
}
/**
Tests that the PID controllers work
*/
void tests::motor_feedback() {
Drive md;
MotorController cont(md);
EncoderMeasurement measure(md);
uint32_t prevTime;
int i =0;
while(true) {
uint32_t currTime = micros();
if (currTime - prevTime >= PERIOD_MICROS) {
prevTime = currTime;
measure.update();
cont.controlMR(0.1, measure.mVR); // right motor PI control
cont.controlML(-0.1, measure.mVL); //left motor PI control
i++;
if(i % 100 == 0) {
Serial.print(measure.encoderRCount);
Serial.print(", ");
Serial.println(measure.encoderLCount);
Serial.print(": ");
Serial.print(measure.mVR, 4);
Serial.print(", ");
Serial.println(measure.mVL, 4);
}
if(md.faulted()) {
Serial.println("Motor fault");
md.setMLSpeed(0);
md.setMRSpeed(0);
delay(100);
md.clearFault();
}
}
}
}