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@50:197a18e49eb4, 2013-06-10 (annotated)

Committer:
gabdo
Date:
Mon Jun 10 05:04:53 2013 +0000
Revision:
50:197a18e49eb4
Parent:
49:f202fb0d4128
Child:
51:60258b84ebab
A little clean up. Still a transmit data bug

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 17:d73944c3c945 8 quadCommand::quadCommand() :
dereklmc 17:d73944c3c945 9 pidThrottle(1.5,0.1,1.20,DEFAULT_WINDUP_GUARD),
dereklmc 17:d73944c3c945 10 pidPitch(1.5,0.1,1.20,DEFAULT_WINDUP_GUARD),
dereklmc 17:d73944c3c945 11 pidRoll(1.5,0.1,1.20,DEFAULT_WINDUP_GUARD),
dereklmc 17:d73944c3c945 12 pidYaw(1.5,0.1,1.20,DEFAULT_WINDUP_GUARD)
gabdo 0:8681037b9a18 13 {
gabdo 49:f202fb0d4128 14 motorProcess.attach(this, &quadCommand::updateMotors, MOTOR_UPDATE/1000);
gabdo 50:197a18e49eb4 15 sensorProcess.attach( this, &quadCommand::updateCurrent, SENSSOR_DELAY /1000 );
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 50:197a18e49eb4 20 myMotors[0] = new motor( MOTOR1 ); // Connect motor 0 to PTD4 pin.
gabdo 50:197a18e49eb4 21 myMotors[1] = new motor( MOTOR2 ); // Connect motor 1 to PTA12 pin.
gabdo 50:197a18e49eb4 22 myMotors[2] = new motor( MOTOR3 ); // Connect motor 2 to PTA4 pin.
gabdo 50:197a18e49eb4 23 myMotors[3] = new motor( MOTOR4 ); // Connect motor 3 to PTA5 pin.
gabdo 45:088885f4a13d 24
gabdo 45:088885f4a13d 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 0:8681037b9a18 29 }
gabdo 0:8681037b9a18 30
gabdo 9:9e0d0ba5b6b1 31 /******************************* run() ***********************************/
oprospero 26:826448361267 32 /* Called from main */
gabdo 9:9e0d0ba5b6b1 33 /*************************************************************************/
gabdo 9:9e0d0ba5b6b1 34
gabdo 0:8681037b9a18 35 void quadCommand::run()
gabdo 0:8681037b9a18 36 {
gabdo 0:8681037b9a18 37 while(1)
gabdo 43:e2fc699e8e8c 38 {
gabdo 50:197a18e49eb4 39 DEBUG(pc.printf("\r\n------------------- IN RUN ---------------------\r\n");)
gabdo 50:197a18e49eb4 40
oprospero 26:826448361267 41 if( myCom->isData() )
gabdo 0:8681037b9a18 42 rxInput();
gabdo 50:197a18e49eb4 43
gabdo 50:197a18e49eb4 44 sendTelemetry();
gabdo 0:8681037b9a18 45 }
gabdo 0:8681037b9a18 46 }
gabdo 0:8681037b9a18 47
gabdo 9:9e0d0ba5b6b1 48 /***************************** rxInput() *********************************/
oprospero 26:826448361267 49 /*Call from run() */
gabdo 9:9e0d0ba5b6b1 50 /*************************************************************************/
gabdo 9:9e0d0ba5b6b1 51
gabdo 0:8681037b9a18 52 void quadCommand::rxInput()
gabdo 0:8681037b9a18 53 {
gabdo 0:8681037b9a18 54 short * command = myCom->read();
gabdo 0:8681037b9a18 55
gabdo 0:8681037b9a18 56 if( command[0] == 1 ) // Throttle command.
gabdo 9:9e0d0ba5b6b1 57 targetThrottle = command[1];
gabdo 0:8681037b9a18 58
gabdo 0:8681037b9a18 59 if( command[0] == 2 ) // Pitch command.
gabdo 9:9e0d0ba5b6b1 60 targetPitch = command[1];
gabdo 0:8681037b9a18 61
gabdo 0:8681037b9a18 62 if( command[0] == 3 ) // Roll command.
gabdo 9:9e0d0ba5b6b1 63 targetRoll = command[1];
gabdo 0:8681037b9a18 64
gabdo 0:8681037b9a18 65 if( command[0] == 4 ) // Yaw command.
gabdo 9:9e0d0ba5b6b1 66 targetYaw = command[1];
gabdo 12:15d129d681e9 67
gabdo 12:15d129d681e9 68 delete[] command;
dereklmc 8:72791d8c36b7 69 }
dereklmc 8:72791d8c36b7 70
gabdo 9:9e0d0ba5b6b1 71 /*************************** updateMotors() ******************************/
oprospero 26:826448361267 72 /*Called in main by motorprocess */
gabdo 9:9e0d0ba5b6b1 73 /*************************************************************************/
gabdo 9:9e0d0ba5b6b1 74
gabdo 49:f202fb0d4128 75 void quadCommand::updateMotors()
gabdo 49:f202fb0d4128 76 {
dereklmc 15:92ecb025fbc5 77 float throttle, pitch, roll, yaw;
dereklmc 15:92ecb025fbc5 78
dereklmc 15:92ecb025fbc5 79 throttle = pidThrottle.correct(currentThrottle, targetThrottle, MOTOR_UPDATE);
dereklmc 15:92ecb025fbc5 80 pitch = pidPitch.correct(currentPitch, targetPitch, MOTOR_UPDATE);
dereklmc 15:92ecb025fbc5 81 roll = pidRoll.correct(currentRoll, targetRoll, MOTOR_UPDATE);
dereklmc 15:92ecb025fbc5 82 yaw = 0.0; //pidYaw.correct(currentYaw, targetYaw, MOTOR_UPDATE);
dereklmc 32:3d180aa938ba 83
gabdo 49:f202fb0d4128 84 DEBUG(pc.printf("\r\n----------------------------------------\r\n");)
dereklmc 46:f1eb22f41ebc 85 DEBUG(pc.printf("cur : { t : %f, p : %f, r : %f, y : %f }\r\n", currentThrottle, currentPitch, currentRoll, 0.0);)
dereklmc 46:f1eb22f41ebc 86 DEBUG(pc.printf("tar : { t : %f, p : %f, r : %f, y : %f }\r\n", targetThrottle, targetPitch, targetRoll, 0.0);)
dereklmc 41:40e432c5cbe6 87 DEBUG(pc.printf("att : { t : %f, p : %f, r : %f, y : %f }\r\n", throttle, pitch, roll, yaw);)
gabdo 49:f202fb0d4128 88 DEBUG(pc.printf("\r\n----------------------------------------\r\n");)
dereklmc 15:92ecb025fbc5 89
dereklmc 46:f1eb22f41ebc 90 float speed0 = throttle + pitch - roll - yaw;
dereklmc 46:f1eb22f41ebc 91 float speed1 = throttle + pitch + roll + yaw;
dereklmc 46:f1eb22f41ebc 92 float speed2 = throttle - pitch + roll - yaw;
dereklmc 46:f1eb22f41ebc 93 float speed3 = throttle - pitch - roll + yaw;
dereklmc 33:4f62a7a46e71 94
dereklmc 41:40e432c5cbe6 95 DEBUG(pc.printf("motor : [ %f , %f , %f , %f ]\r\n", speed0, speed1, speed2, speed3);)
dereklmc 33:4f62a7a46e71 96
dereklmc 33:4f62a7a46e71 97 myMotors[ 0 ]->setSpeed(speed0); // Motor 1
dereklmc 33:4f62a7a46e71 98 myMotors[ 1 ]->setSpeed(speed1); // Motor 2
dereklmc 33:4f62a7a46e71 99 myMotors[ 2 ]->setSpeed(speed2); // Motor 3
dereklmc 33:4f62a7a46e71 100 myMotors[ 3 ]->setSpeed(speed3); // Motor 4
gabdo 40:8c01bf294768 101 }
gabdo 40:8c01bf294768 102
gabdo 40:8c01bf294768 103 /**************************** sendData() *********************************/
gabdo 40:8c01bf294768 104 /* */
gabdo 40:8c01bf294768 105 /*************************************************************************/
gabdo 40:8c01bf294768 106
gabdo 40:8c01bf294768 107 void quadCommand::sendTelemetry()
gabdo 40:8c01bf294768 108 {
gabdo 49:f202fb0d4128 109 myCom->write( 2, (short)(currentPitch * 900 / 10 ) );
gabdo 49:f202fb0d4128 110 myCom->write( 3, (short)(currentRoll * 900 / 10 ) );
gabdo 43:e2fc699e8e8c 111 }
gabdo 43:e2fc699e8e8c 112
gabdo 43:e2fc699e8e8c 113 /************************** updateCurrent() ******************************/
gabdo 43:e2fc699e8e8c 114 /* */
gabdo 43:e2fc699e8e8c 115 /*************************************************************************/
gabdo 43:e2fc699e8e8c 116
gabdo 43:e2fc699e8e8c 117 void quadCommand::updateCurrent()
gabdo 43:e2fc699e8e8c 118 {
gabdo 43:e2fc699e8e8c 119 currentPitch = world->getAbsoluteX();
gabdo 43:e2fc699e8e8c 120 currentRoll = world->getAbsoluteY();
gabdo 0:8681037b9a18 121 }