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@62:70fc58d4be9b, 2013-07-26 (annotated)

Committer:
oprospero
Date:
Fri Jul 26 02:42:31 2013 +0000
Revision:
62:70fc58d4be9b
Parent:
60:6906a96344a0
Child:
63:f4cb482ac5d4
minor fixes

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