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@8:72791d8c36b7, 2013-06-09 (annotated)

Committer:
dereklmc
Date:
Sun Jun 09 23:36:31 2013 +0000
Revision:
8:72791d8c36b7
Parent:
4:ce6ad16337c5
Child:
9:9e0d0ba5b6b1
Child:
15:92ecb025fbc5
Pulled out update motors, set to ticker.

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
dereklmc 8:72791d8c36b7 7 const float quadCommand::MOTOR_UPDATE(20.0);
dereklmc 8:72791d8c36b7 8
gabdo 0:8681037b9a18 9 quadCommand::quadCommand()
gabdo 0:8681037b9a18 10 {
gabdo 0:8681037b9a18 11 myCom = new com( TXPIN, RXPIN ); // Setup com object.
gabdo 0:8681037b9a18 12 world = new sensors(); // Setup the sensors.
gabdo 0:8681037b9a18 13
gabdo 0:8681037b9a18 14 myMotors[0] = new motor( MOTOR1 ); // Connect motor 0 to PTD4 pin.
gabdo 0:8681037b9a18 15 myMotors[1] = new motor( MOTOR2 ); // Connect motor 1 to PTA12 pin.
gabdo 0:8681037b9a18 16 myMotors[2] = new motor( MOTOR3 ); // Connect motor 2 to PTA4 pin.
gabdo 0:8681037b9a18 17 myMotors[3] = new motor( MOTOR4 ); // Connect motor 3 to PTA5 pin.
gabdo 0:8681037b9a18 18
gabdo 0:8681037b9a18 19 rxThrottle = 0;
gabdo 0:8681037b9a18 20 rxPitch = 0;
gabdo 0:8681037b9a18 21 rxRoll = 0;
gabdo 0:8681037b9a18 22 rxYaw = 0;
gabdo 0:8681037b9a18 23 }
gabdo 0:8681037b9a18 24
gabdo 0:8681037b9a18 25 void quadCommand::run()
gabdo 0:8681037b9a18 26 {
gabdo 0:8681037b9a18 27 while(1)
gabdo 0:8681037b9a18 28 {
gabdo 0:8681037b9a18 29 if( myCom->isData() )
gabdo 0:8681037b9a18 30 rxInput();
gabdo 0:8681037b9a18 31 }
gabdo 0:8681037b9a18 32 }
gabdo 0:8681037b9a18 33
gabdo 0:8681037b9a18 34 void quadCommand::rxInput()
gabdo 0:8681037b9a18 35 {
gabdo 0:8681037b9a18 36 short * command = myCom->read();
gabdo 0:8681037b9a18 37
gabdo 0:8681037b9a18 38 if( command[0] == 1 ) // Throttle command.
gabdo 0:8681037b9a18 39 rxThrottle = command[1];
gabdo 0:8681037b9a18 40
gabdo 0:8681037b9a18 41 if( command[0] == 2 ) // Pitch command.
gabdo 0:8681037b9a18 42 rxPitch = command[1];
gabdo 0:8681037b9a18 43
gabdo 0:8681037b9a18 44 if( command[0] == 3 ) // Roll command.
gabdo 0:8681037b9a18 45 rxRoll = command[1];
gabdo 0:8681037b9a18 46
gabdo 0:8681037b9a18 47 if( command[0] == 4 ) // Yaw command.
dereklmc 8:72791d8c36b7 48 rxYaw = command[1];
dereklmc 8:72791d8c36b7 49 }
dereklmc 8:72791d8c36b7 50
dereklmc 8:72791d8c36b7 51 void quadCommand::updateMotors() {
dereklmc 8:72791d8c36b7 52 myMotors[ 0 ]->setSpeed( rxThrottle + rxPitch - rxRoll - rxYaw); // Motor 1
dereklmc 8:72791d8c36b7 53 myMotors[ 1 ]->setSpeed( rxThrottle + rxPitch + rxRoll + rxYaw); // Motor 2
dereklmc 8:72791d8c36b7 54 myMotors[ 2 ]->setSpeed( rxThrottle - rxPitch + rxRoll - rxYaw); // Motor 3
dereklmc 8:72791d8c36b7 55 myMotors[ 3 ]->setSpeed( rxThrottle - rxPitch - rxRoll + rxYaw); // Motor 4
gabdo 0:8681037b9a18 56
gabdo 0:8681037b9a18 57 delete[] command;
gabdo 0:8681037b9a18 58 }