Joseph Roberts / Mbed 2 deprecated Quadcopter_mk2

New version of quadcopter software written to OO principles

Dependencies:   mbed MODSERIAL filter mbed-rtos ConfigFile PID PPM FreeIMU_external_magnetometer TinyGPS

FlightController/MotorMixer/MotorMixer.cpp@4:9ffbf9101992, 2015-05-08 (annotated)

Committer:
joe4465
Date:
Fri May 08 09:07:38 2015 +0000
Revision:
4:9ffbf9101992
Parent:
2:969dfa4f2436 
End of FYP

Who changed what in which revision?

UserRevisionLine numberNew contents of line
joe4465 0:c6a85bb2a827 1 #include "MotorMixer.h"
joe4465 0:c6a85bb2a827 2
joe4465 2:969dfa4f2436 3 MotorMixer::MotorMixer(PinName motor1, PinName motor2, PinName motor3, PinName motor4)
joe4465 2:969dfa4f2436 4 {
joe4465 2:969dfa4f2436 5 _motorPower = MotorPower();
joe4465 2:969dfa4f2436 6
joe4465 0:c6a85bb2a827 7 _motor1 = new PwmOut(motor1);
joe4465 0:c6a85bb2a827 8 _motor2 = new PwmOut(motor2);
joe4465 0:c6a85bb2a827 9 _motor3 = new PwmOut(motor3);
joe4465 0:c6a85bb2a827 10 _motor4 = new PwmOut(motor4);
joe4465 0:c6a85bb2a827 11
joe4465 2:969dfa4f2436 12 //Set period
joe4465 2:969dfa4f2436 13 double period = 1.0 / FLIGHT_CONTROLLER_FREQUENCY;
joe4465 0:c6a85bb2a827 14 _motor1->period(period);
joe4465 0:c6a85bb2a827 15 _motor2->period(period);
joe4465 0:c6a85bb2a827 16 _motor3->period(period);
joe4465 0:c6a85bb2a827 17 _motor4->period(period);
joe4465 0:c6a85bb2a827 18
joe4465 0:c6a85bb2a827 19 //Disable
joe4465 2:969dfa4f2436 20 setPower(MOTORS_OFF);
joe4465 0:c6a85bb2a827 21
joe4465 2:969dfa4f2436 22 DEBUG("Motor power initialised\r\n");
joe4465 0:c6a85bb2a827 23 }
joe4465 0:c6a85bb2a827 24
joe4465 2:969dfa4f2436 25 MotorMixer::~MotorMixer(){}
joe4465 2:969dfa4f2436 26
joe4465 2:969dfa4f2436 27 void MotorMixer::computePower(PidWrapper::PidOutput pidOutput, double throttle)
joe4465 2:969dfa4f2436 28 {
joe4465 0:c6a85bb2a827 29 //Calculate base power to apply from throttle - returns 1060 at min, 1860 at max
joe4465 2:969dfa4f2436 30 double basePower = MOTORS_MIN + (throttle * 800);
joe4465 2:969dfa4f2436 31
joe4465 2:969dfa4f2436 32 MotorPower motorPower = MotorPower();
joe4465 0:c6a85bb2a827 33
joe4465 0:c6a85bb2a827 34 //Map motor power - each PID returns -100 <-> 100
joe4465 2:969dfa4f2436 35 motorPower.motor1 = basePower + pidOutput.pitch + pidOutput.roll + pidOutput.yaw;
joe4465 2:969dfa4f2436 36 motorPower.motor2 = basePower + pidOutput.pitch - pidOutput.roll - pidOutput.yaw;
joe4465 2:969dfa4f2436 37 motorPower.motor3 = basePower - pidOutput.pitch - pidOutput.roll + pidOutput.yaw;
joe4465 2:969dfa4f2436 38 motorPower.motor4 = basePower - pidOutput.pitch + pidOutput.roll - pidOutput.yaw;
joe4465 0:c6a85bb2a827 39
joe4465 0:c6a85bb2a827 40 //Specify intial motor power limits
joe4465 2:969dfa4f2436 41 double motorFix = 0;
joe4465 2:969dfa4f2436 42 double motorMin = motorPower.motor1;
joe4465 2:969dfa4f2436 43 double motorMax = motorPower.motor1;
joe4465 0:c6a85bb2a827 44
joe4465 0:c6a85bb2a827 45 //Check motor power is within limits - if not add/remove constant to all motors to keep motor ratio the same
joe4465 2:969dfa4f2436 46 if(motorPower.motor1 < motorMin) motorMin = motorPower.motor1;
joe4465 2:969dfa4f2436 47 if(motorPower.motor1 > motorMax) motorMax = motorPower.motor1;
joe4465 2:969dfa4f2436 48 if(motorPower.motor2 < motorMin) motorMin = motorPower.motor2;
joe4465 2:969dfa4f2436 49 if(motorPower.motor2 > motorMax) motorMax = motorPower.motor2;
joe4465 2:969dfa4f2436 50 if(motorPower.motor3 < motorMin) motorMin = motorPower.motor3;
joe4465 2:969dfa4f2436 51 if(motorPower.motor3 > motorMax) motorMax = motorPower.motor3;
joe4465 2:969dfa4f2436 52 if(motorPower.motor4 < motorMin) motorMin = motorPower.motor4;
joe4465 2:969dfa4f2436 53 if(motorPower.motor4 > motorMax) motorMax = motorPower.motor4;
joe4465 0:c6a85bb2a827 54
joe4465 0:c6a85bb2a827 55 //Check if min or max is outside of the limits
joe4465 0:c6a85bb2a827 56 if(motorMin < MOTORS_MIN) motorFix = MOTORS_MIN - motorMin;
joe4465 0:c6a85bb2a827 57 else if(motorMax > MOTORS_MAX) motorFix = MOTORS_MAX - motorMax;
joe4465 0:c6a85bb2a827 58
joe4465 0:c6a85bb2a827 59 //Add/remove constant
joe4465 2:969dfa4f2436 60 motorPower.motor1 += motorFix;
joe4465 2:969dfa4f2436 61 motorPower.motor2 += motorFix;
joe4465 2:969dfa4f2436 62 motorPower.motor3 += motorFix;
joe4465 2:969dfa4f2436 63 motorPower.motor4 += motorFix;
joe4465 2:969dfa4f2436 64
joe4465 2:969dfa4f2436 65 //Set motor power
joe4465 2:969dfa4f2436 66 setPower(motorPower);
joe4465 2:969dfa4f2436 67 }
joe4465 2:969dfa4f2436 68
joe4465 2:969dfa4f2436 69 void MotorMixer::computePower(double throttle)
joe4465 2:969dfa4f2436 70 {
joe4465 2:969dfa4f2436 71 //Calculate base power to apply from throttle - returns 1060 at min, 1860 at max
joe4465 2:969dfa4f2436 72 double basePower = MOTORS_MIN + (throttle * 800);
joe4465 2:969dfa4f2436 73
joe4465 2:969dfa4f2436 74 MotorPower motorPower = MotorPower();
joe4465 2:969dfa4f2436 75 motorPower.motor1 = basePower;
joe4465 2:969dfa4f2436 76 motorPower.motor2 = basePower;
joe4465 2:969dfa4f2436 77 motorPower.motor3 = basePower;
joe4465 2:969dfa4f2436 78 motorPower.motor4 = basePower;
joe4465 2:969dfa4f2436 79
joe4465 2:969dfa4f2436 80 //Set motor power
joe4465 2:969dfa4f2436 81 setPower(motorPower);
joe4465 0:c6a85bb2a827 82 }
joe4465 0:c6a85bb2a827 83
joe4465 2:969dfa4f2436 84 void MotorMixer::setPower(double motor1Power, double motor2Power, double motor3Power, double motor4Power)
joe4465 0:c6a85bb2a827 85 {
joe4465 2:969dfa4f2436 86 _motorPower.motor1 = motor1Power;
joe4465 2:969dfa4f2436 87 _motorPower.motor2 = motor2Power;
joe4465 2:969dfa4f2436 88 _motorPower.motor3 = motor3Power;
joe4465 2:969dfa4f2436 89 _motorPower.motor4 = motor4Power;
joe4465 2:969dfa4f2436 90
joe4465 2:969dfa4f2436 91 #ifdef MOTORS_ENABLED
joe4465 2:969dfa4f2436 92 _motor1->pulsewidth_us(motor1Power);
joe4465 2:969dfa4f2436 93 _motor2->pulsewidth_us(motor2Power);
joe4465 2:969dfa4f2436 94 _motor3->pulsewidth_us(motor3Power);
joe4465 2:969dfa4f2436 95 _motor4->pulsewidth_us(motor4Power);
joe4465 2:969dfa4f2436 96 #endif
joe4465 0:c6a85bb2a827 97 }
joe4465 0:c6a85bb2a827 98
joe4465 2:969dfa4f2436 99 void MotorMixer::setPower(double motorPower)
joe4465 0:c6a85bb2a827 100 {
joe4465 0:c6a85bb2a827 101 setPower(motorPower, motorPower, motorPower, motorPower);
joe4465 0:c6a85bb2a827 102 }
joe4465 0:c6a85bb2a827 103
joe4465 2:969dfa4f2436 104 void MotorMixer::setPower(MotorMixer::MotorPower motorPower)
joe4465 2:969dfa4f2436 105 {
joe4465 2:969dfa4f2436 106 setPower(motorPower.motor1, motorPower.motor2, motorPower.motor3, motorPower.motor4);
joe4465 2:969dfa4f2436 107 }
joe4465 2:969dfa4f2436 108
joe4465 0:c6a85bb2a827 109 MotorMixer::MotorPower MotorMixer::getMotorPower()
joe4465 0:c6a85bb2a827 110 {
joe4465 0:c6a85bb2a827 111 return _motorPower;
joe4465 0:c6a85bb2a827 112 }