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@58:983801808a94, 2013-07-03 (annotated)

Committer:
gabdo
Date:
Wed Jul 03 03:51:45 2013 +0000
Revision:
58:983801808a94
Parent:
56:a550127695b2
Child:
59:9dfd9169a5e7
I can't find the motor issue

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 49:f202fb0d4128 9 motorProcess.attach(this, &quadCommand::updateMotors, MOTOR_UPDATE/1000);
gabdo 50:197a18e49eb4 10
gabdo 50:197a18e49eb4 11 myCom = new com( TXPIN, RXPIN ); // Setup com object.
gabdo 50:197a18e49eb4 12 world = new sensors(); // Setup the sensors.
gabdo 43:e2fc699e8e8c 13
gabdo 58:983801808a94 14 // Create PID instances.
gabdo 58:983801808a94 15 pidPitch = new PID(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD);
gabdo 58:983801808a94 16 pidRoll = new PID(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD);
gabdo 58:983801808a94 17 pidYaw = new PID(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD);
gabdo 58:983801808a94 18
gabdo 58:983801808a94 19 myMotors[0] = new motor( MOTOR1 ); // Connect motor 0 to PTD4 pin.
gabdo 58:983801808a94 20 myMotors[1] = new motor( MOTOR2 ); // Connect motor 1 to PTA12 pin.
gabdo 58:983801808a94 21 myMotors[2] = new motor( MOTOR3 ); // Connect motor 2 to PTA4 pin.
gabdo 58:983801808a94 22 myMotors[3] = new motor( MOTOR4 ); // Connect motor 3 to PTA5 pin.
gabdo 58:983801808a94 23
gabdo 56:a550127695b2 24 // Initialize variables to zero
gabdo 56:a550127695b2 25 targetThrottle = currentThrottle = 0;
gabdo 45:088885f4a13d 26 targetPitch = currentPitch = 0;
gabdo 45:088885f4a13d 27 targetRoll = currentRoll = 0;
gabdo 45:088885f4a13d 28 targetYaw = currentYaw = 0;
gabdo 55:bca9c9e92da6 29 pitchTrim = 0;
gabdo 55:bca9c9e92da6 30 rollTrim = 0;
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 56:a550127695b2 86 delete[] command;
dereklmc 8:72791d8c36b7 87 }
dereklmc 8:72791d8c36b7 88
gabdo 55:bca9c9e92da6 89
gabdo 9:9e0d0ba5b6b1 90 /*************************** updateMotors() ******************************/
oprospero 26:826448361267 91 /*Called in main by motorprocess */
gabdo 9:9e0d0ba5b6b1 92 /*************************************************************************/
gabdo 9:9e0d0ba5b6b1 93
gabdo 49:f202fb0d4128 94 void quadCommand::updateMotors()
gabdo 49:f202fb0d4128 95 {
gabdo 53:cce34958f952 96 updateCurrent();
gabdo 58:983801808a94 97 float throttle, pitch, roll, yaw;
gabdo 58:983801808a94 98
gabdo 56:a550127695b2 99 throttle = targetThrottle;
gabdo 58:983801808a94 100 pitch = pidPitch->correct(currentPitch, targetPitch, MOTOR_UPDATE);
gabdo 58:983801808a94 101 roll = pidRoll->correct(currentRoll, targetRoll, MOTOR_UPDATE);
gabdo 58:983801808a94 102 yaw = pidYaw->correct(currentYaw, targetYaw, MOTOR_UPDATE);
dereklmc 15:92ecb025fbc5 103
gabdo 55:bca9c9e92da6 104 int go = 0;
gabdo 55:bca9c9e92da6 105 if( targetThrottle > 0 )
gabdo 55:bca9c9e92da6 106 go = 1;
gabdo 56:a550127695b2 107
gabdo 58:983801808a94 108 // Set motor speeds
gabdo 58:983801808a94 109 myMotors[ 0 ]->setSpeed( go * ( throttle - roll - pitch + yaw));
gabdo 58:983801808a94 110 myMotors[ 1 ]->setSpeed( go * ( throttle + roll - pitch - yaw));
gabdo 58:983801808a94 111 myMotors[ 2 ]->setSpeed( go * ( throttle + roll + pitch + yaw));
gabdo 58:983801808a94 112 myMotors[ 3 ]->setSpeed( go * ( throttle - roll + pitch - yaw));
gabdo 43:e2fc699e8e8c 113 }
gabdo 43:e2fc699e8e8c 114
gabdo 43:e2fc699e8e8c 115 /************************** updateCurrent() ******************************/
gabdo 43:e2fc699e8e8c 116 /* */
gabdo 43:e2fc699e8e8c 117 /*************************************************************************/
gabdo 43:e2fc699e8e8c 118
gabdo 43:e2fc699e8e8c 119 void quadCommand::updateCurrent()
gabdo 43:e2fc699e8e8c 120 {
gabdo 55:bca9c9e92da6 121 currentPitch = world->getAbsoluteX() - pitchTrim;
gabdo 55:bca9c9e92da6 122 currentRoll = world->getAbsoluteY() - rollTrim;
gabdo 0:8681037b9a18 123 }