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@56:a550127695b2, 2013-07-02 (annotated)

Committer:
gabdo
Date:
Tue Jul 02 21:03:46 2013 +0000
Revision:
56:a550127695b2
Parent:
55:bca9c9e92da6
Child:
58:983801808a94
Problems with motor #2

;

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 17:d73944c3c945 7 quadCommand::quadCommand() :
gabdo 55:bca9c9e92da6 8 pidPitch(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD),
gabdo 55:bca9c9e92da6 9 pidRoll(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD),
gabdo 55:bca9c9e92da6 10 pidYaw(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD)
gabdo 0:8681037b9a18 11 {
gabdo 49:f202fb0d4128 12 motorProcess.attach(this, &quadCommand::updateMotors, MOTOR_UPDATE/1000);
gabdo 50:197a18e49eb4 13
gabdo 50:197a18e49eb4 14 myCom = new com( TXPIN, RXPIN ); // Setup com object.
gabdo 50:197a18e49eb4 15 world = new sensors(); // Setup the sensors.
gabdo 43:e2fc699e8e8c 16
gabdo 56:a550127695b2 17 myMotors[0] = new motor( MOTOR1 ); // Connect motor 0 to PTD4 pin.
gabdo 56:a550127695b2 18 myMotors[1] = new motor( MOTOR2 ); // Connect motor 1 to PTA12 pin.
gabdo 56:a550127695b2 19 myMotors[2] = new motor( MOTOR3 ); // Connect motor 2 to PTA4 pin.
gabdo 56:a550127695b2 20 myMotors[3] = new motor( MOTOR4 ); // Connect motor 3 to PTA5 pin.
gabdo 56:a550127695b2 21
gabdo 56:a550127695b2 22 // Initialize variables to zero
gabdo 56:a550127695b2 23 targetThrottle = currentThrottle = 0;
gabdo 45:088885f4a13d 24 targetPitch = currentPitch = 0;
gabdo 45:088885f4a13d 25 targetRoll = currentRoll = 0;
gabdo 45:088885f4a13d 26 targetYaw = currentYaw = 0;
gabdo 55:bca9c9e92da6 27 pitchTrim = 0;
gabdo 55:bca9c9e92da6 28 rollTrim = 0;
gabdo 0:8681037b9a18 29 }
gabdo 0:8681037b9a18 30
gabdo 9:9e0d0ba5b6b1 31 /******************************* run() ***********************************/
gabdo 55:bca9c9e92da6 32 /* */
gabdo 9:9e0d0ba5b6b1 33 /*************************************************************************/
gabdo 9:9e0d0ba5b6b1 34
gabdo 0:8681037b9a18 35 void quadCommand::run()
gabdo 0:8681037b9a18 36 {
gabdo 0:8681037b9a18 37 while(1)
gabdo 43:e2fc699e8e8c 38 {
oprospero 26:826448361267 39 if( myCom->isData() )
gabdo 51:60258b84ebab 40 rxInput();
gabdo 0:8681037b9a18 41 }
gabdo 0:8681037b9a18 42 }
gabdo 0:8681037b9a18 43
gabdo 9:9e0d0ba5b6b1 44 /***************************** rxInput() *********************************/
gabdo 55:bca9c9e92da6 45 /* Commands: 0 -> Ack, does not get placed in rxQueue. */
gabdo 55:bca9c9e92da6 46 /* 1 -> Throttle */
gabdo 55:bca9c9e92da6 47 /* 2 -> Pitch */
gabdo 55:bca9c9e92da6 48 /* 3 -> Roll */
gabdo 55:bca9c9e92da6 49 /* 4 -> Yaw */
gabdo 56:a550127695b2 50 /* 5 -> Roll Trim Left */
gabdo 56:a550127695b2 51 /* 6 -> Roll Trim Right */
gabdo 56:a550127695b2 52 /* 7 -> Pitch Trim Forward */
gabdo 56:a550127695b2 53 /* 8 -> Pitch Trim Back */
gabdo 9:9e0d0ba5b6b1 54 /*************************************************************************/
gabdo 9:9e0d0ba5b6b1 55
gabdo 0:8681037b9a18 56 void quadCommand::rxInput()
gabdo 0:8681037b9a18 57 {
gabdo 0:8681037b9a18 58 short * command = myCom->read();
gabdo 0:8681037b9a18 59
gabdo 0:8681037b9a18 60 if( command[0] == 1 ) // Throttle command.
gabdo 9:9e0d0ba5b6b1 61 targetThrottle = command[1];
gabdo 0:8681037b9a18 62
gabdo 0:8681037b9a18 63 if( command[0] == 2 ) // Pitch command.
gabdo 9:9e0d0ba5b6b1 64 targetPitch = command[1];
gabdo 0:8681037b9a18 65
gabdo 0:8681037b9a18 66 if( command[0] == 3 ) // Roll command.
gabdo 9:9e0d0ba5b6b1 67 targetRoll = command[1];
gabdo 0:8681037b9a18 68
gabdo 56:a550127695b2 69 if( command[0] == 4 ) // Yaw command.
gabdo 9:9e0d0ba5b6b1 70 targetYaw = command[1];
gabdo 56:a550127695b2 71
gabdo 56:a550127695b2 72 if( command[0] == 5 ) // Roll Trim Left.
gabdo 55:bca9c9e92da6 73 rollTrim += TRIM_VALUE;
gabdo 55:bca9c9e92da6 74
gabdo 56:a550127695b2 75 if( command[0] == 6 ) // Roll Trim Right.
gabdo 55:bca9c9e92da6 76 rollTrim -= TRIM_VALUE;
gabdo 55:bca9c9e92da6 77
gabdo 56:a550127695b2 78 if( command[0] == 7 ) // Pitch Trim Forward
gabdo 55:bca9c9e92da6 79 pitchTrim += TRIM_VALUE;
gabdo 55:bca9c9e92da6 80
gabdo 56:a550127695b2 81 if( command[0] == 8 ) // Pitch Trim Back
gabdo 55:bca9c9e92da6 82 pitchTrim -= TRIM_VALUE;
gabdo 55:bca9c9e92da6 83
gabdo 56:a550127695b2 84 delete[] command;
dereklmc 8:72791d8c36b7 85 }
dereklmc 8:72791d8c36b7 86
gabdo 55:bca9c9e92da6 87
gabdo 9:9e0d0ba5b6b1 88 /*************************** updateMotors() ******************************/
oprospero 26:826448361267 89 /*Called in main by motorprocess */
gabdo 9:9e0d0ba5b6b1 90 /*************************************************************************/
gabdo 9:9e0d0ba5b6b1 91
gabdo 49:f202fb0d4128 92 void quadCommand::updateMotors()
gabdo 49:f202fb0d4128 93 {
gabdo 53:cce34958f952 94 updateCurrent();
dereklmc 15:92ecb025fbc5 95 float throttle, pitch, roll, yaw;
dereklmc 15:92ecb025fbc5 96
gabdo 56:a550127695b2 97 throttle = targetThrottle;
dereklmc 15:92ecb025fbc5 98 pitch = pidPitch.correct(currentPitch, targetPitch, MOTOR_UPDATE);
dereklmc 15:92ecb025fbc5 99 roll = pidRoll.correct(currentRoll, targetRoll, MOTOR_UPDATE);
gabdo 56:a550127695b2 100 yaw = pidYaw.correct(currentYaw, targetYaw, MOTOR_UPDATE);
dereklmc 15:92ecb025fbc5 101
gabdo 55:bca9c9e92da6 102 int go = 0;
gabdo 55:bca9c9e92da6 103 if( targetThrottle > 0 )
gabdo 55:bca9c9e92da6 104 go = 1;
gabdo 56:a550127695b2 105
gabdo 56:a550127695b2 106 myMotors[ 0 ]->setSpeed( go * ( throttle - roll - pitch + yaw)); // Motor 1
gabdo 56:a550127695b2 107 myMotors[ 1 ]->setSpeed( go * ( throttle + roll - pitch - yaw)); // Motor 2
gabdo 56:a550127695b2 108 myMotors[ 2 ]->setSpeed( go * ( throttle + roll + pitch + yaw)); // Motor 3
gabdo 56:a550127695b2 109 myMotors[ 3 ]->setSpeed( go * ( throttle - roll + pitch - yaw)); // Motor 4
gabdo 43:e2fc699e8e8c 110 }
gabdo 43:e2fc699e8e8c 111
gabdo 43:e2fc699e8e8c 112 /************************** updateCurrent() ******************************/
gabdo 43:e2fc699e8e8c 113 /* */
gabdo 43:e2fc699e8e8c 114 /*************************************************************************/
gabdo 43:e2fc699e8e8c 115
gabdo 43:e2fc699e8e8c 116 void quadCommand::updateCurrent()
gabdo 43:e2fc699e8e8c 117 {
gabdo 55:bca9c9e92da6 118 currentPitch = world->getAbsoluteX() - pitchTrim;
gabdo 55:bca9c9e92da6 119 currentRoll = world->getAbsoluteY() - rollTrim;
gabdo 0:8681037b9a18 120 }