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@1:9b90e7de6e09, 2013-06-09 (annotated)

Committer:
gabdo
Date:
Sun Jun 09 22:33:11 2013 +0000
Revision:
1:9b90e7de6e09
Parent:
0:8681037b9a18
Child:
4:ce6ad16337c5
Updated quadCommand vars

;

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 0:8681037b9a18 7 quadCommand::quadCommand()
gabdo 0:8681037b9a18 8 {
gabdo 0:8681037b9a18 9 myCom = new com( TXPIN, RXPIN ); // Setup com object.
gabdo 0:8681037b9a18 10 world = new sensors(); // Setup the sensors.
gabdo 0:8681037b9a18 11
gabdo 0:8681037b9a18 12 myMotors[0] = new motor( MOTOR1 ); // Connect motor 0 to PTD4 pin.
gabdo 0:8681037b9a18 13 myMotors[1] = new motor( MOTOR2 ); // Connect motor 1 to PTA12 pin.
gabdo 0:8681037b9a18 14 myMotors[2] = new motor( MOTOR3 ); // Connect motor 2 to PTA4 pin.
gabdo 0:8681037b9a18 15 myMotors[3] = new motor( MOTOR4 ); // Connect motor 3 to PTA5 pin.
gabdo 0:8681037b9a18 16
gabdo 0:8681037b9a18 17 rxThrottle = 0;
gabdo 0:8681037b9a18 18 rxPitch = 0;
gabdo 0:8681037b9a18 19 rxRoll = 0;
gabdo 0:8681037b9a18 20 rxYaw = 0;
gabdo 0:8681037b9a18 21 }
gabdo 0:8681037b9a18 22
gabdo 0:8681037b9a18 23 void quadCommand::run()
gabdo 0:8681037b9a18 24 {
gabdo 0:8681037b9a18 25 while(1)
gabdo 0:8681037b9a18 26 {
gabdo 0:8681037b9a18 27 if( myCom->isData() )
gabdo 0:8681037b9a18 28 rxInput();
gabdo 1:9b90e7de6e09 29
gabdo 1:9b90e7de6e09 30 myCom->write( 3, world->getAbsoluteY());
gabdo 1:9b90e7de6e09 31 wait( .05);
gabdo 1:9b90e7de6e09 32 //updatePosition();
gabdo 0:8681037b9a18 33
gabdo 1:9b90e7de6e09 34 // if( globalUpdate )
gabdo 1:9b90e7de6e09 35 // txPosition();
gabdo 0:8681037b9a18 36 }
gabdo 0:8681037b9a18 37 }
gabdo 0:8681037b9a18 38
gabdo 0:8681037b9a18 39 void quadCommand::rxInput()
gabdo 0:8681037b9a18 40 {
gabdo 0:8681037b9a18 41 short * command = myCom->read();
gabdo 0:8681037b9a18 42
gabdo 0:8681037b9a18 43 if( command[0] == 1 ) // Throttle command.
gabdo 0:8681037b9a18 44 rxThrottle = command[1];
gabdo 0:8681037b9a18 45
gabdo 0:8681037b9a18 46 if( command[0] == 2 ) // Pitch command.
gabdo 0:8681037b9a18 47 rxPitch = command[1];
gabdo 0:8681037b9a18 48
gabdo 0:8681037b9a18 49 if( command[0] == 3 ) // Roll command.
gabdo 0:8681037b9a18 50 rxRoll = command[1];
gabdo 0:8681037b9a18 51
gabdo 0:8681037b9a18 52 if( command[0] == 4 ) // Yaw command.
gabdo 0:8681037b9a18 53 rxYaw = command[1];
gabdo 0:8681037b9a18 54
gabdo 0:8681037b9a18 55 myMotors[ 0 ]->setSpeed( rxThrottle + rxPitch - rxRoll ); // Motor 1
gabdo 0:8681037b9a18 56 myMotors[ 1 ]->setSpeed( rxThrottle + rxPitch + rxRoll ); // Motor 2
gabdo 0:8681037b9a18 57 myMotors[ 2 ]->setSpeed( rxThrottle - rxPitch + rxRoll ); // Motor 3
gabdo 0:8681037b9a18 58 myMotors[ 3 ]->setSpeed( rxThrottle - rxPitch - rxRoll ); // Motor 4
gabdo 0:8681037b9a18 59
gabdo 0:8681037b9a18 60 delete[] command;
gabdo 0:8681037b9a18 61 }
gabdo 0:8681037b9a18 62
gabdo 0:8681037b9a18 63 void quadCommand::updatePosition()
gabdo 0:8681037b9a18 64 {
gabdo 0:8681037b9a18 65 float temp;
gabdo 0:8681037b9a18 66
gabdo 0:8681037b9a18 67 temp = world->getAbsoluteX();
gabdo 0:8681037b9a18 68 if( temp != currentRoll )
gabdo 0:8681037b9a18 69 {
gabdo 0:8681037b9a18 70 currentRoll = temp;
gabdo 0:8681037b9a18 71 updatedRoll = true;
gabdo 0:8681037b9a18 72 globalUpdate = true;
gabdo 0:8681037b9a18 73 }
gabdo 0:8681037b9a18 74
gabdo 0:8681037b9a18 75 temp = world->getAbsoluteY();
gabdo 0:8681037b9a18 76 if( temp != currentPitch )
gabdo 0:8681037b9a18 77 {
gabdo 0:8681037b9a18 78 currentPitch = temp;
gabdo 0:8681037b9a18 79 updatedPitch = true;
gabdo 0:8681037b9a18 80 globalUpdate = true;
gabdo 0:8681037b9a18 81 }
gabdo 0:8681037b9a18 82 }
gabdo 0:8681037b9a18 83
gabdo 0:8681037b9a18 84 void quadCommand::txPosition()
gabdo 0:8681037b9a18 85 {
gabdo 0:8681037b9a18 86 if( updatedRoll )
gabdo 0:8681037b9a18 87 {
gabdo 0:8681037b9a18 88 myCom->write( 3, currentRoll);
gabdo 0:8681037b9a18 89 updatedRoll = false;
gabdo 0:8681037b9a18 90 }
gabdo 0:8681037b9a18 91
gabdo 0:8681037b9a18 92 if( updatedPitch )
gabdo 0:8681037b9a18 93 {
gabdo 0:8681037b9a18 94 myCom->write( 2, currentPitch);
gabdo 0:8681037b9a18 95 updatedPitch = false;
gabdo 0:8681037b9a18 96 }
gabdo 0:8681037b9a18 97
gabdo 0:8681037b9a18 98 globalUpdate = false;
gabdo 0:8681037b9a18 99 }