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@59:9dfd9169a5e7, 2013-07-03 (annotated)

Committer:
gabdo
Date:
Wed Jul 03 04:45:03 2013 +0000
Revision:
59:9dfd9169a5e7
Parent:
58:983801808a94
Child:
60:6906a96344a0
Working code, needs to be tuned.

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