void MotorMixer::computePower(double throttle)
{
//Calculate base power to apply from throttle - returns 1060 at min, 1860 at max
double basePower = MOTORS_MIN + (throttle * 800);
MotorPower motorPower = MotorPower();
motorPower.motor1 = basePower;
motorPower.motor2 = basePower;
motorPower.motor3 = basePower;
motorPower.motor4 = basePower;
//Set motor power
setPower(motorPower);
}
extern "C" void PIOINT1_IRQHandler(void)
{
U64 systickcnt = SysTickCnt;
if (GPIO1_MIS(0))
{
g_motorPos += g_motorInc;
g_motorTime = systickcnt;
if (g_motorPos == g_motorStop)
{
MotorDir(0, 0); // brake motor
MotorPower(0, 100); // stop the motor
}
GPIO1_IC(0); // clear the interrupt flag
__NOP();__NOP(); // required after clearing interrupt flags
}
}
void MotorMixer::computePower(PidWrapper::PidOutput pidOutput, double throttle)
{
//Calculate base power to apply from throttle - returns 1060 at min, 1860 at max
double basePower = MOTORS_MIN + (throttle * 800);
MotorPower motorPower = MotorPower();
//Map motor power - each PID returns -100 <-> 100
motorPower.motor1 = basePower + pidOutput.pitch + pidOutput.roll + pidOutput.yaw;
motorPower.motor2 = basePower + pidOutput.pitch - pidOutput.roll - pidOutput.yaw;
motorPower.motor3 = basePower - pidOutput.pitch - pidOutput.roll + pidOutput.yaw;
motorPower.motor4 = basePower - pidOutput.pitch + pidOutput.roll - pidOutput.yaw;
//Specify intial motor power limits
double motorFix = 0;
double motorMin = motorPower.motor1;
double motorMax = motorPower.motor1;
//Check motor power is within limits - if not add/remove constant to all motors to keep motor ratio the same
if(motorPower.motor1 < motorMin) motorMin = motorPower.motor1;
if(motorPower.motor1 > motorMax) motorMax = motorPower.motor1;
if(motorPower.motor2 < motorMin) motorMin = motorPower.motor2;
if(motorPower.motor2 > motorMax) motorMax = motorPower.motor2;
if(motorPower.motor3 < motorMin) motorMin = motorPower.motor3;
if(motorPower.motor3 > motorMax) motorMax = motorPower.motor3;
if(motorPower.motor4 < motorMin) motorMin = motorPower.motor4;
if(motorPower.motor4 > motorMax) motorMax = motorPower.motor4;
//Check if min or max is outside of the limits
if(motorMin < MOTORS_MIN) motorFix = MOTORS_MIN - motorMin;
else if(motorMax > MOTORS_MAX) motorFix = MOTORS_MAX - motorMax;
//Add/remove constant
motorPower.motor1 += motorFix;
motorPower.motor2 += motorFix;
motorPower.motor3 += motorFix;
motorPower.motor4 += motorFix;
//Set motor power
setPower(motorPower);
}
MotorMixer::MotorMixer(PinName motor1, PinName motor2, PinName motor3, PinName motor4)
{
_motorPower = MotorPower();
_motor1 = new PwmOut(motor1);
_motor2 = new PwmOut(motor2);
_motor3 = new PwmOut(motor3);
_motor4 = new PwmOut(motor4);
//Set period
double period = 1.0 / FLIGHT_CONTROLLER_FREQUENCY;
_motor1->period(period);
_motor2->period(period);
_motor3->period(period);
_motor4->period(period);
//Disable
setPower(MOTORS_OFF);
DEBUG("Motor power initialised\r\n");
}
int main(void)
{
// Set system frequency to 48MHz
SystemFrequency = ConfigurePLL(12000000UL, 48000000UL);
// Enable clock to IO Configuration block. Needed for UART, SPI, I2C, etc...
SYSCON_SYSAHBCLKCTRL_IOCON(SYSAHBCLKCTRL_ENABLE);
SysTick_Config(SystemFrequency/10000 - 1); // Generate interrupt each .1 ms
// set direction on port 0_7 (pin 28) to output
GPIO0_DIR(7, GPIO_OUTPUT);
GPIO0_DATA(7, 1); // turn on diagnostic led
GPIO1_DIR(4, GPIO_OUTPUT); // configure port 1_5 (pin 14) for output
GPIO1_DIR(5, GPIO_OUTPUT); // configure port 1_5 (pin 14) for output
// PWM output (pin 1) is tied to 1,2EN on H-bridge (SN754410 Quad Half H-bridge) //
SetupPWM();
MotorDir(0, 0); // turn on braking at 100%
MotorPower(0, 100);
// motor details:
// 120:1 gearbox
// 8 teeth on motor gear
// 32 teeth on gear with optical wheel
// 4 edges per rot of optical wheel
// 8/32*120*4 = 106.666 edges per rev
// At 5.5V max speed with just 2 gears,
// pulse width is avg 9.5 ms.
//
// a full stop should be no edge for about 150ms
// and motor on brake.
//
g_motorPos = 0;
g_motorStop = 0;
g_motorTime = 0;
g_motorInc = 0;
// setup interrupts on PIO1_0
IOCON_PIO1_0_MODE(PIO1_0_MODE_PULLDOWN_RESISTOR);
GPIO1_DIR(0, GPIO_INPUT);
GPIO1_IS(0, GPIO_EDGE_SENSITIVE);
GPIO1_IBE(0, GPIO_BOTH_EDGES);
GPIO1_IE(0, GPIO_INTERRUPT_ENABLE);
NVIC_EnableIRQ(EINT1_IRQn);
U64 time1, time2, oldTime;
int pos, oldPos;
oldTime = 0xFFFFFFFFFFFFFFFF;
U64 systickcnt;
int desiredMotorInc; // g_motorInc can only be changed when in a stopped state
while (1)
{
FlashLED();
desiredMotorInc = g_motorInc = 1;
g_motorStop = 100;
MotorDir(0, 1);
MotorPower(0, 100);
while (1)
{
do {
time1 = g_motorTime;
pos = g_motorPos;
time2 = g_motorTime;
} while (time1 != time2);
if (pos > g_motorStop)
{
desiredMotorInc = -1;
}
else if (pos < g_motorStop)
{
desiredMotorInc = 1;
}
systickcnt = SysTickCnt;
if ((systickcnt - time2) > 200 /*ms*/)
{
// stopped
if (pos == g_motorStop)
{
break; //achieved goal pos and we are stopped
}
if (desiredMotorInc != g_motorInc)
{
g_motorInc = desiredMotorInc; // g_motorInc must always be 1 or -1, otherwise we lose track of position
// change direction and turn power back on
MotorDir(0, desiredMotorInc);
MotorPower(0, 20);
}
}
}
FlashLED();
desiredMotorInc = g_motorInc = -1;
g_motorStop = 0;
MotorDir(0, -1);
MotorPower(0, 100);
while (1)
{
do {
time1 = g_motorTime;
pos = g_motorPos;
time2 = g_motorTime;
} while (time1 != time2);
if (pos > g_motorStop)
{
desiredMotorInc = -1;
}
else if (pos < g_motorStop)
{
desiredMotorInc = 1;
}
systickcnt = SysTickCnt;
if ((systickcnt - time2) > 200 /*ms*/)
{
// stopped
if (pos == g_motorStop)
{
break; //achieved goal pos and we are stopped
}
if (desiredMotorInc != g_motorInc)
{
g_motorInc = desiredMotorInc; // g_motorInc must always be 1 or -1, otherwise we lose track of position
// change direction and turn power back on
MotorDir(0, desiredMotorInc);
MotorPower(0, 20);
}
}
}
}
}