MotorMixer.cpp

00001 #include "MotorMixer.h"
00002 
00003 MotorMixer::MotorMixer(PinName motor1, PinName motor2, PinName motor3, PinName motor4)
00004 {   
00005     _motorPower = MotorPower();
00006     
00007     _motor1 = new PwmOut(motor1);
00008     _motor2 = new PwmOut(motor2);
00009     _motor3 = new PwmOut(motor3);
00010     _motor4 = new PwmOut(motor4);
00011     
00012     //Set period
00013     double period = 1.0 / FLIGHT_CONTROLLER_FREQUENCY;
00014     _motor1->period(period);
00015     _motor2->period(period);
00016     _motor3->period(period);
00017     _motor4->period(period);
00018     
00019     //Disable
00020     setPower(MOTORS_OFF);
00021     
00022     DEBUG("Motor power initialised\r\n");  
00023 }
00024 
00025 MotorMixer::~MotorMixer(){}
00026 
00027 void MotorMixer::computePower(PidWrapper::PidOutput pidOutput, double throttle)
00028 {    
00029     //Calculate base power to apply from throttle - returns 1060 at min, 1860 at max
00030     double basePower = MOTORS_MIN + (throttle * 800);
00031     
00032     MotorPower motorPower = MotorPower();
00033     
00034     //Map motor power - each PID returns -100 <-> 100
00035     motorPower.motor1 = basePower + pidOutput.pitch + pidOutput.roll + pidOutput.yaw;
00036     motorPower.motor2 = basePower + pidOutput.pitch - pidOutput.roll - pidOutput.yaw;
00037     motorPower.motor3 = basePower - pidOutput.pitch - pidOutput.roll + pidOutput.yaw;
00038     motorPower.motor4 = basePower - pidOutput.pitch + pidOutput.roll - pidOutput.yaw;
00039     
00040     //Specify intial motor power limits
00041     double motorFix = 0;
00042     double motorMin = motorPower.motor1;
00043     double motorMax = motorPower.motor1;
00044     
00045     //Check motor power is within limits - if not add/remove constant to all motors to keep motor ratio the same
00046     if(motorPower.motor1 < motorMin) motorMin = motorPower.motor1;
00047     if(motorPower.motor1 > motorMax) motorMax = motorPower.motor1;
00048     if(motorPower.motor2 < motorMin) motorMin = motorPower.motor2;
00049     if(motorPower.motor2 > motorMax) motorMax = motorPower.motor2;
00050     if(motorPower.motor3 < motorMin) motorMin = motorPower.motor3;
00051     if(motorPower.motor3 > motorMax) motorMax = motorPower.motor3;
00052     if(motorPower.motor4 < motorMin) motorMin = motorPower.motor4;
00053     if(motorPower.motor4 > motorMax) motorMax = motorPower.motor4;
00054         
00055     //Check if min or max is outside of the limits
00056     if(motorMin < MOTORS_MIN) motorFix = MOTORS_MIN - motorMin;
00057     else if(motorMax > MOTORS_MAX) motorFix = MOTORS_MAX - motorMax;
00058     
00059     //Add/remove constant
00060     motorPower.motor1 += motorFix;
00061     motorPower.motor2 += motorFix;
00062     motorPower.motor3 += motorFix;
00063     motorPower.motor4 += motorFix;
00064     
00065     //Set motor power
00066     setPower(motorPower);
00067 }
00068 
00069 void MotorMixer::computePower(double throttle)
00070 {    
00071     //Calculate base power to apply from throttle - returns 1060 at min, 1860 at max
00072     double basePower = MOTORS_MIN + (throttle * 800);
00073     
00074     MotorPower motorPower = MotorPower();
00075     motorPower.motor1 = basePower;
00076     motorPower.motor2 = basePower;
00077     motorPower.motor3 = basePower;
00078     motorPower.motor4 = basePower;
00079     
00080     //Set motor power
00081     setPower(motorPower);
00082 }
00083 
00084 void MotorMixer::setPower(double motor1Power, double motor2Power, double motor3Power, double motor4Power)
00085 {
00086     _motorPower.motor1 = motor1Power;
00087     _motorPower.motor2 = motor2Power;
00088     _motorPower.motor3 = motor3Power;
00089     _motorPower.motor4 = motor4Power;
00090     
00091     #ifdef MOTORS_ENABLED
00092         _motor1->pulsewidth_us(motor1Power);
00093         _motor2->pulsewidth_us(motor2Power);
00094         _motor3->pulsewidth_us(motor3Power);
00095         _motor4->pulsewidth_us(motor4Power);
00096     #endif
00097 }
00098 
00099 void MotorMixer::setPower(double motorPower)
00100 {
00101     setPower(motorPower, motorPower, motorPower, motorPower);
00102 }
00103 
00104 void MotorMixer::setPower(MotorMixer::MotorPower motorPower)
00105 {
00106     setPower(motorPower.motor1, motorPower.motor2, motorPower.motor3, motorPower.motor4);
00107 }
00108 
00109 MotorMixer::MotorPower MotorMixer::getMotorPower()
00110 {
00111     return _motorPower;
00112 }