/*
* Based on the already existing fuzzy sets and taking the
* encoders data as input, the fuzzy controller computes the
* new output that needs to be written on the motors.
*/
void MotorController::fuzzyCompute()
{
#ifdef MOTOR_CONTROLLER_DEBUG
_leftMotorSpeed = leftEncoder.getSpeed();
_rightMotorSpeed = rightEncoder.getSpeed();
Serial.print("Left motor speed: ");
Serial.print(_leftMotorSpeed);
Serial.print(" Right motor speed: ");
Serial.println(_rightMotorSpeed);
_leftMotorPWM = DEBUG_PWM;
_rightMotorPWM = DEBUG_PWM;
#else
_leftMotorSpeed = leftEncoder.getSpeed();
fuzzyController.setInput(1, _leftMotorSpeed);
fuzzyController.fuzzify();
_leftMotorPWM = fuzzyController.defuzzify(1);
_rightMotorSpeed = rightEncoder.getSpeed();
fuzzyController.setInput(1, _rightMotorSpeed);
fuzzyController.fuzzify();
_rightMotorPWM = fuzzyController.defuzzify(1);
#endif
}
int main(int argc, char** argv)
{
Motor* motor = new Motor(0);
Encoder* encoder = new Encoder(motor);
PIDController* pidController = new PIDController(motor, encoder, POSITION_REV, 1, 0, 0);
pidController->setSetpoint(10);
pidController->enable();
Motor* motor2 = new Motor(1);
Encoder* encoder2 = new Encoder(motor2);
PIDController* pidController2 = new PIDController(motor2, encoder2, SPEED, 0, 0, 0, .4/20);
pidController2->setSetpoint(20);
pidController2->enable();
int ticks = 0;
while(!pidController->onTarget())
{
pidController->update();
pidController2->update();
std::cout << "tick:\t" << ++ticks << "\tcurrent position:\t" << encoder->getPosition() << "\tcurrent speed:\t" << encoder->getSpeed() << "\n";
std::cout << "tick:\t" << ticks << "\tcurrent speed:\t" << encoder2->getSpeed() << "\n";
}
std::cout << "done with position " << encoder->getPosition() << "\n";
return 0;
}