void QuaduinoFlight::init() {
motorsActive = false;
minPulseWidth = 1100;
maxPulseWidth = 1600;
gravityHover = 200;
frontMotorOffset = 11;
rearMotorOffset = 5;
rightMotorOffset = 7;
leftMotorOffset = 9;
consKp = 0.3;
consKi = 0.003;
consKd = 0.192;
aggKp = 0.93;
aggKi = 0.1;
aggKd = 0.2;
angleToSwap = 15;
writeMotor(FRONT, 0);
writeMotor(RIGHT, 0);
writeMotor(REAR, 0);
writeMotor(LEFT, 0);
yawPID = &PID(&yawInput, &yawOutput, &yawSetpoint, 4.0, 0.0, 0.0, DIRECT);
pitchPID = &PID(&pitchInput, &pitchOutput, &pitchSetpoint, 4.0, 0.0, 0.0, DIRECT);
rollPID = &PID(&rollInput, &rollOutput, &rollSetpoint, 4.0, 0.0, 0.0, DIRECT);
setupPidControl();
}
void QuaduinoFlight::runMotorsMax() {
// set all motors to maximum pulse width (highest speed)
writeMotor(FRONT, maxPulseWidth);
writeMotor(RIGHT, maxPulseWidth);
writeMotor(REAR, maxPulseWidth);
writeMotor(LEFT, maxPulseWidth);
}
void QuaduinoFlight::runMotorsMin() {
// set all motors to minimum pulse width (lowest speed)
writeMotor(FRONT, minPulseWidth);
writeMotor(RIGHT, minPulseWidth);
writeMotor(REAR, minPulseWidth);
writeMotor(LEFT, minPulseWidth);
}
bool Motor::setStopCommand(enum StopCommand cmd)
{
size_t len;
bool retval = false;
const char *cmdstr;
if (!isStopCommandSupported(cmd))
return retval;
cmdstr = stopCommandNames[cmd];
len = strlen(cmdstr);
retval = writeMotor("stop_command", cmdstr, len);
if (!retval)
return retval;
stop_command = cmd;
return retval;
}
bool Motor::setSpeedRegulation(bool value)
{
char truebuf[] = SPEED_REGULATION_TRUE;
char falsebuf[] = SPEED_REGULATION_FALSE;
char *buf;
unsigned int len;
bool retval;
if (value) {
buf = truebuf;
len = sizeof(truebuf);
} else {
buf = falsebuf;
len = sizeof(falsebuf);
}
len--; /* We don't need to write the null character */
retval = writeMotor("speed_regulation", buf, len);
if (retval)
speed_regulation = value;
return retval;
}
