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@40:8c01bf294768, 2013-06-10 (annotated)

Committer:
gabdo
Date:
Mon Jun 10 01:32:09 2013 +0000
Revision:
40:8c01bf294768
Parent:
34:d6696dc977a2
Child:
42:9b5d58070e92
Minor updates

;

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"
dereklmc 32:3d180aa938ba 6 #include "debug.h"
gabdo 0:8681037b9a18 7
dereklmc 15:92ecb025fbc5 8 const float quadCommand::MOTOR_UPDATE(20.0f);
dereklmc 8:72791d8c36b7 9
dereklmc 17:d73944c3c945 10 quadCommand::quadCommand() :
dereklmc 17:d73944c3c945 11 pidThrottle(1.5,0.1,1.20,DEFAULT_WINDUP_GUARD),
dereklmc 17:d73944c3c945 12 pidPitch(1.5,0.1,1.20,DEFAULT_WINDUP_GUARD),
dereklmc 17:d73944c3c945 13 pidRoll(1.5,0.1,1.20,DEFAULT_WINDUP_GUARD),
dereklmc 17:d73944c3c945 14 pidYaw(1.5,0.1,1.20,DEFAULT_WINDUP_GUARD)
gabdo 0:8681037b9a18 15 {
gabdo 40:8c01bf294768 16 Ticker sensorProcess;
gabdo 40:8c01bf294768 17 sensorProcess.attach( this, &quadCommand::sendTelemetry, 2 );
gabdo 0:8681037b9a18 18 myCom = new com( TXPIN, RXPIN ); // Setup com object.
gabdo 0:8681037b9a18 19 world = new sensors(); // Setup the sensors.
gabdo 0:8681037b9a18 20
gabdo 0:8681037b9a18 21 myMotors[0] = new motor( MOTOR1 ); // Connect motor 0 to PTD4 pin.
gabdo 0:8681037b9a18 22 myMotors[1] = new motor( MOTOR2 ); // Connect motor 1 to PTA12 pin.
gabdo 0:8681037b9a18 23 myMotors[2] = new motor( MOTOR3 ); // Connect motor 2 to PTA4 pin.
gabdo 0:8681037b9a18 24 myMotors[3] = new motor( MOTOR4 ); // Connect motor 3 to PTA5 pin.
gabdo 0:8681037b9a18 25 }
gabdo 0:8681037b9a18 26
gabdo 9:9e0d0ba5b6b1 27 /******************************* run() ***********************************/
oprospero 26:826448361267 28 /* Called from main */
gabdo 9:9e0d0ba5b6b1 29 /*************************************************************************/
gabdo 9:9e0d0ba5b6b1 30
gabdo 0:8681037b9a18 31 void quadCommand::run()
gabdo 0:8681037b9a18 32 {
gabdo 0:8681037b9a18 33 while(1)
oprospero 26:826448361267 34 { // Checks if there's data from serial
oprospero 26:826448361267 35 if( myCom->isData() )
gabdo 0:8681037b9a18 36 rxInput();
gabdo 0:8681037b9a18 37 }
gabdo 0:8681037b9a18 38 }
gabdo 0:8681037b9a18 39
gabdo 9:9e0d0ba5b6b1 40 /***************************** rxInput() *********************************/
oprospero 26:826448361267 41 /*Call from run() */
gabdo 9:9e0d0ba5b6b1 42 /*************************************************************************/
gabdo 9:9e0d0ba5b6b1 43
gabdo 0:8681037b9a18 44 void quadCommand::rxInput()
gabdo 0:8681037b9a18 45 {
gabdo 0:8681037b9a18 46 short * command = myCom->read();
gabdo 0:8681037b9a18 47
gabdo 0:8681037b9a18 48 if( command[0] == 1 ) // Throttle command.
gabdo 9:9e0d0ba5b6b1 49 targetThrottle = command[1];
gabdo 0:8681037b9a18 50
gabdo 0:8681037b9a18 51 if( command[0] == 2 ) // Pitch command.
gabdo 9:9e0d0ba5b6b1 52 targetPitch = command[1];
gabdo 0:8681037b9a18 53
gabdo 0:8681037b9a18 54 if( command[0] == 3 ) // Roll command.
gabdo 9:9e0d0ba5b6b1 55 targetRoll = command[1];
gabdo 0:8681037b9a18 56
gabdo 0:8681037b9a18 57 if( command[0] == 4 ) // Yaw command.
gabdo 9:9e0d0ba5b6b1 58 targetYaw = command[1];
gabdo 12:15d129d681e9 59
gabdo 12:15d129d681e9 60 delete[] command;
dereklmc 8:72791d8c36b7 61 }
dereklmc 8:72791d8c36b7 62
gabdo 9:9e0d0ba5b6b1 63 /*************************** updateMotors() ******************************/
oprospero 26:826448361267 64 /*Called in main by motorprocess */
gabdo 9:9e0d0ba5b6b1 65 /*************************************************************************/
gabdo 9:9e0d0ba5b6b1 66
dereklmc 8:72791d8c36b7 67 void quadCommand::updateMotors() {
dereklmc 15:92ecb025fbc5 68 float throttle, pitch, roll, yaw;
dereklmc 15:92ecb025fbc5 69
dereklmc 15:92ecb025fbc5 70 throttle = pidThrottle.correct(currentThrottle, targetThrottle, MOTOR_UPDATE);
dereklmc 15:92ecb025fbc5 71 pitch = pidPitch.correct(currentPitch, targetPitch, MOTOR_UPDATE);
dereklmc 15:92ecb025fbc5 72 roll = pidRoll.correct(currentRoll, targetRoll, MOTOR_UPDATE);
dereklmc 15:92ecb025fbc5 73 yaw = 0.0; //pidYaw.correct(currentYaw, targetYaw, MOTOR_UPDATE);
dereklmc 32:3d180aa938ba 74
dereklmc 33:4f62a7a46e71 75 DEBUG(pc.printf("att : { t : %f, p : %f, r : %f, y : %f }", throttle, pitch, roll, yaw);)
dereklmc 15:92ecb025fbc5 76
dereklmc 33:4f62a7a46e71 77 int speed0 = throttle + pitch - roll - yaw;
dereklmc 33:4f62a7a46e71 78 int speed1 = throttle + pitch + roll + yaw;
dereklmc 33:4f62a7a46e71 79 int speed2 = throttle - pitch + roll - yaw;
dereklmc 33:4f62a7a46e71 80 int speed3 = throttle - pitch - roll + yaw;
dereklmc 33:4f62a7a46e71 81
dereklmc 33:4f62a7a46e71 82 DEBUG(pc.printf("motor : [ %f , %f , %f , %f ]", speed0, speed1, speed2, speed3);)
dereklmc 33:4f62a7a46e71 83
dereklmc 33:4f62a7a46e71 84 myMotors[ 0 ]->setSpeed(speed0); // Motor 1
dereklmc 33:4f62a7a46e71 85 myMotors[ 1 ]->setSpeed(speed1); // Motor 2
dereklmc 33:4f62a7a46e71 86 myMotors[ 2 ]->setSpeed(speed2); // Motor 3
dereklmc 33:4f62a7a46e71 87 myMotors[ 3 ]->setSpeed(speed3); // Motor 4
gabdo 40:8c01bf294768 88 }
gabdo 40:8c01bf294768 89
gabdo 40:8c01bf294768 90 /**************************** sendData() *********************************/
gabdo 40:8c01bf294768 91 /* */
gabdo 40:8c01bf294768 92 /*************************************************************************/
gabdo 40:8c01bf294768 93
gabdo 40:8c01bf294768 94 void quadCommand::sendTelemetry()
gabdo 40:8c01bf294768 95 {
gabdo 40:8c01bf294768 96
gabdo 0:8681037b9a18 97 }