Prosper Van / Mbed 2 deprecated quadCommand2

added gy80 dcm

Dependencies:   mbed DCM_AHRS_GY80 PID MMA8451Q

Fork of quadCommand by Greg Abdo

quadCommand/quadCommand.cpp@18:c7df2edd73f0, 2013-06-10 (annotated)

Committer:
dereklmc
Date:
Mon Jun 10 00:12:48 2013 +0000
Revision:
18:c7df2edd73f0
Parent:
12:15d129d681e9
Parent:
17:d73944c3c945
Child:
26:826448361267
Child:
32:3d180aa938ba
Merge R14, R13

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gabdo 0:8681037b9a18 1 /************************ quadCommand.cpp ********************************/
gabdo 0:8681037b9a18 2 /* */
gabdo 0:8681037b9a18 3 /*************************************************************************/
gabdo 0:8681037b9a18 4
gabdo 0:8681037b9a18 5 #include "quadCommand.h"
gabdo 0:8681037b9a18 6
dereklmc 15:92ecb025fbc5 7 const float quadCommand::MOTOR_UPDATE(20.0f);
dereklmc 8:72791d8c36b7 8
dereklmc 17:d73944c3c945 9 quadCommand::quadCommand() :
dereklmc 17:d73944c3c945 10 pidThrottle(1.5,0.1,1.20,DEFAULT_WINDUP_GUARD),
dereklmc 17:d73944c3c945 11 pidPitch(1.5,0.1,1.20,DEFAULT_WINDUP_GUARD),
dereklmc 17:d73944c3c945 12 pidRoll(1.5,0.1,1.20,DEFAULT_WINDUP_GUARD),
dereklmc 17:d73944c3c945 13 pidYaw(1.5,0.1,1.20,DEFAULT_WINDUP_GUARD)
gabdo 0:8681037b9a18 14 {
gabdo 0:8681037b9a18 15 myCom = new com( TXPIN, RXPIN ); // Setup com object.
gabdo 0:8681037b9a18 16 world = new sensors(); // Setup the sensors.
gabdo 0:8681037b9a18 17
gabdo 0:8681037b9a18 18 myMotors[0] = new motor( MOTOR1 ); // Connect motor 0 to PTD4 pin.
gabdo 0:8681037b9a18 19 myMotors[1] = new motor( MOTOR2 ); // Connect motor 1 to PTA12 pin.
gabdo 0:8681037b9a18 20 myMotors[2] = new motor( MOTOR3 ); // Connect motor 2 to PTA4 pin.
gabdo 0:8681037b9a18 21 myMotors[3] = new motor( MOTOR4 ); // Connect motor 3 to PTA5 pin.
gabdo 0:8681037b9a18 22 }
gabdo 0:8681037b9a18 23
gabdo 9:9e0d0ba5b6b1 24 /******************************* run() ***********************************/
gabdo 9:9e0d0ba5b6b1 25 /* */
gabdo 9:9e0d0ba5b6b1 26 /*************************************************************************/
gabdo 9:9e0d0ba5b6b1 27
gabdo 0:8681037b9a18 28 void quadCommand::run()
gabdo 0:8681037b9a18 29 {
gabdo 0:8681037b9a18 30 while(1)
gabdo 0:8681037b9a18 31 {
gabdo 0:8681037b9a18 32 if( myCom->isData() )
gabdo 0:8681037b9a18 33 rxInput();
gabdo 0:8681037b9a18 34 }
gabdo 0:8681037b9a18 35 }
gabdo 0:8681037b9a18 36
gabdo 9:9e0d0ba5b6b1 37 /***************************** rxInput() *********************************/
gabdo 9:9e0d0ba5b6b1 38 /* */
gabdo 9:9e0d0ba5b6b1 39 /*************************************************************************/
gabdo 9:9e0d0ba5b6b1 40
gabdo 0:8681037b9a18 41 void quadCommand::rxInput()
gabdo 0:8681037b9a18 42 {
gabdo 0:8681037b9a18 43 short * command = myCom->read();
gabdo 0:8681037b9a18 44
gabdo 0:8681037b9a18 45 if( command[0] == 1 ) // Throttle command.
gabdo 9:9e0d0ba5b6b1 46 targetThrottle = command[1];
gabdo 0:8681037b9a18 47
gabdo 0:8681037b9a18 48 if( command[0] == 2 ) // Pitch command.
gabdo 9:9e0d0ba5b6b1 49 targetPitch = command[1];
gabdo 0:8681037b9a18 50
gabdo 0:8681037b9a18 51 if( command[0] == 3 ) // Roll command.
gabdo 9:9e0d0ba5b6b1 52 targetRoll = command[1];
gabdo 0:8681037b9a18 53
gabdo 0:8681037b9a18 54 if( command[0] == 4 ) // Yaw command.
gabdo 9:9e0d0ba5b6b1 55 targetYaw = command[1];
gabdo 12:15d129d681e9 56
gabdo 12:15d129d681e9 57 delete[] command;
dereklmc 8:72791d8c36b7 58 }
dereklmc 8:72791d8c36b7 59
gabdo 9:9e0d0ba5b6b1 60 /*************************** updateMotors() ******************************/
gabdo 9:9e0d0ba5b6b1 61 /* */
gabdo 9:9e0d0ba5b6b1 62 /*************************************************************************/
gabdo 9:9e0d0ba5b6b1 63
dereklmc 8:72791d8c36b7 64 void quadCommand::updateMotors() {
dereklmc 15:92ecb025fbc5 65 float throttle, pitch, roll, yaw;
dereklmc 15:92ecb025fbc5 66
dereklmc 15:92ecb025fbc5 67 throttle = pidThrottle.correct(currentThrottle, targetThrottle, MOTOR_UPDATE);
dereklmc 15:92ecb025fbc5 68 pitch = pidPitch.correct(currentPitch, targetPitch, MOTOR_UPDATE);
dereklmc 15:92ecb025fbc5 69 roll = pidRoll.correct(currentRoll, targetRoll, MOTOR_UPDATE);
dereklmc 15:92ecb025fbc5 70 yaw = 0.0; //pidYaw.correct(currentYaw, targetYaw, MOTOR_UPDATE);
dereklmc 15:92ecb025fbc5 71
dereklmc 15:92ecb025fbc5 72 myMotors[ 0 ]->setSpeed( throttle + pitch - roll - yaw); // Motor 1
dereklmc 15:92ecb025fbc5 73 myMotors[ 1 ]->setSpeed( throttle + pitch + roll + yaw); // Motor 2
dereklmc 15:92ecb025fbc5 74 myMotors[ 2 ]->setSpeed( throttle - pitch + roll - yaw); // Motor 3
dereklmc 15:92ecb025fbc5 75 myMotors[ 3 ]->setSpeed( throttle - pitch - roll + yaw); // Motor 4
gabdo 0:8681037b9a18 76 }