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@63:f4cb482ac5d4, 2013-07-28 (annotated)

Committer:
oprospero
Date:
Sun Jul 28 21:44:00 2013 +0000
Revision:
63:f4cb482ac5d4
Parent:
62:70fc58d4be9b 
working state

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