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@55:bca9c9e92da6, 2013-06-13 (annotated)

Committer:
gabdo
Date:
Thu Jun 13 01:11:03 2013 +0000
Revision:
55:bca9c9e92da6
Parent:
53:cce34958f952
Child:
56:a550127695b2
Good job!

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 17:d73944c3c945 7 quadCommand::quadCommand() :
gabdo 55:bca9c9e92da6 8 // Proportional, Intagral, Derivitive, Windup
gabdo 55:bca9c9e92da6 9 pidThrottle(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD),
gabdo 55:bca9c9e92da6 10 pidPitch(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD),
gabdo 55:bca9c9e92da6 11 pidRoll(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD),
gabdo 55:bca9c9e92da6 12 pidYaw(PID_P,PID_I,PID_D,DEFAULT_WINDUP_GUARD)
gabdo 0:8681037b9a18 13 {
gabdo 49:f202fb0d4128 14 motorProcess.attach(this, &quadCommand::updateMotors, MOTOR_UPDATE/1000);
gabdo 53:cce34958f952 15 //sensorProcess.attach( this, &quadCommand::updateCurrent, SENSSOR_DELAY/1000 );
gabdo 53:cce34958f952 16 //sendProcess.attach( this, &quadCommand::sendTelemetry, SEND_DELAY/1000 );
gabdo 50:197a18e49eb4 17
gabdo 50:197a18e49eb4 18 myCom = new com( TXPIN, RXPIN ); // Setup com object.
gabdo 50:197a18e49eb4 19 world = new sensors(); // Setup the sensors.
gabdo 55:bca9c9e92da6 20 //pc = new Serial(USBTX, USBRX);
gabdo 43:e2fc699e8e8c 21
gabdo 55:bca9c9e92da6 22 myMotors[0] = new motor( MOTOR1 ); // Connect motor 0 to PTD4 pin. // motor 2
gabdo 55:bca9c9e92da6 23 myMotors[1] = new motor( MOTOR2 ); // Connect motor 1 to PTA12 pin. // motor 1
gabdo 55:bca9c9e92da6 24 myMotors[2] = new motor( MOTOR3 ); // Connect motor 2 to PTA4 pin. // motor 3
gabdo 55:bca9c9e92da6 25 myMotors[3] = new motor( MOTOR4 ); // Connect motor 3 to PTA5 pin. // motor 4
gabdo 55:bca9c9e92da6 26
gabdo 55:bca9c9e92da6 27 myMotors[1]->setPulseMin(0.0011850);
gabdo 55:bca9c9e92da6 28 myMotors[2]->setPulseMin(0.00116875);
gabdo 55:bca9c9e92da6 29 myMotors[3]->setPulseMin(0.0011625);
gabdo 45:088885f4a13d 30
gabdo 45:088885f4a13d 31 targetThrottle = currentThrottle = 0;
gabdo 45:088885f4a13d 32 targetPitch = currentPitch = 0;
gabdo 45:088885f4a13d 33 targetRoll = currentRoll = 0;
gabdo 45:088885f4a13d 34 targetYaw = currentYaw = 0;
gabdo 55:bca9c9e92da6 35 pitchTrim = 0;
gabdo 55:bca9c9e92da6 36 rollTrim = 0;
gabdo 0:8681037b9a18 37 }
gabdo 0:8681037b9a18 38
gabdo 9:9e0d0ba5b6b1 39 /******************************* run() ***********************************/
gabdo 55:bca9c9e92da6 40 /* */
gabdo 9:9e0d0ba5b6b1 41 /*************************************************************************/
gabdo 9:9e0d0ba5b6b1 42
gabdo 0:8681037b9a18 43 void quadCommand::run()
gabdo 0:8681037b9a18 44 {
gabdo 0:8681037b9a18 45 while(1)
gabdo 43:e2fc699e8e8c 46 {
oprospero 26:826448361267 47 if( myCom->isData() )
gabdo 51:60258b84ebab 48 rxInput();
gabdo 0:8681037b9a18 49 }
gabdo 0:8681037b9a18 50 }
gabdo 0:8681037b9a18 51
gabdo 9:9e0d0ba5b6b1 52 /***************************** rxInput() *********************************/
gabdo 55:bca9c9e92da6 53 /* Commands: 0 -> Ack, does not get placed in rxQueue. */
gabdo 55:bca9c9e92da6 54 /* 1 -> Throttle */
gabdo 55:bca9c9e92da6 55 /* 2 -> Pitch */
gabdo 55:bca9c9e92da6 56 /* 3 -> Roll */
gabdo 55:bca9c9e92da6 57 /* 4 -> Yaw */
gabdo 55:bca9c9e92da6 58 /* 5 -> Motor 1 trimUp */
gabdo 55:bca9c9e92da6 59 /* 6 -> Motor 1 trimDown */
gabdo 55:bca9c9e92da6 60 /* 7 -> Motor 1 setTrim */
gabdo 55:bca9c9e92da6 61 /* 8 -> Motor 2 trimUp */
gabdo 55:bca9c9e92da6 62 /* 9 -> Motor 2 trimDown */
gabdo 55:bca9c9e92da6 63 /* 10-> Motor 2 setTrim */
gabdo 55:bca9c9e92da6 64 /* 11-> Motor 3 trimUp */
gabdo 55:bca9c9e92da6 65 /* 12-> Motor 3 trimDown */
gabdo 55:bca9c9e92da6 66 /* 13-> Motor 3 setTrim */
gabdo 55:bca9c9e92da6 67 /* 14-> Motor 4 trimUp */
gabdo 55:bca9c9e92da6 68 /* 15-> Motor 4 trimDown */
gabdo 55:bca9c9e92da6 69 /* 16-> Motor 4 setTrim */
gabdo 55:bca9c9e92da6 70 /* 17-> valueUp */
gabdo 55:bca9c9e92da6 71 /* 18-> valueDown */
gabdo 9:9e0d0ba5b6b1 72 /*************************************************************************/
gabdo 9:9e0d0ba5b6b1 73
gabdo 0:8681037b9a18 74 void quadCommand::rxInput()
gabdo 0:8681037b9a18 75 {
gabdo 0:8681037b9a18 76 short * command = myCom->read();
gabdo 0:8681037b9a18 77
gabdo 0:8681037b9a18 78 if( command[0] == 1 ) // Throttle command.
gabdo 9:9e0d0ba5b6b1 79 targetThrottle = command[1];
gabdo 0:8681037b9a18 80
gabdo 0:8681037b9a18 81 if( command[0] == 2 ) // Pitch command.
gabdo 9:9e0d0ba5b6b1 82 targetPitch = command[1];
gabdo 0:8681037b9a18 83
gabdo 0:8681037b9a18 84 if( command[0] == 3 ) // Roll command.
gabdo 9:9e0d0ba5b6b1 85 targetRoll = command[1];
gabdo 0:8681037b9a18 86
gabdo 0:8681037b9a18 87 if( command[0] == 4 ) // Yaw command.
gabdo 9:9e0d0ba5b6b1 88 targetYaw = command[1];
gabdo 12:15d129d681e9 89
gabdo 55:bca9c9e92da6 90 if( command[0] == 5 ) // Roll Trim Left.
gabdo 55:bca9c9e92da6 91 rollTrim += TRIM_VALUE;
gabdo 55:bca9c9e92da6 92
gabdo 55:bca9c9e92da6 93 if( command[0] == 6 ) // Roll Trim Right.
gabdo 55:bca9c9e92da6 94 rollTrim -= TRIM_VALUE;
gabdo 55:bca9c9e92da6 95
gabdo 55:bca9c9e92da6 96 if( command[0] == 7 ) // Pitch Trim Forward
gabdo 55:bca9c9e92da6 97 pitchTrim += TRIM_VALUE;
gabdo 55:bca9c9e92da6 98
gabdo 55:bca9c9e92da6 99 if( command[0] == 8 ) // Pitch Trim Back
gabdo 55:bca9c9e92da6 100 pitchTrim -= TRIM_VALUE;
gabdo 55:bca9c9e92da6 101
gabdo 12:15d129d681e9 102 delete[] command;
dereklmc 8:72791d8c36b7 103 }
dereklmc 8:72791d8c36b7 104
gabdo 55:bca9c9e92da6 105
gabdo 9:9e0d0ba5b6b1 106 /*************************** updateMotors() ******************************/
oprospero 26:826448361267 107 /*Called in main by motorprocess */
gabdo 9:9e0d0ba5b6b1 108 /*************************************************************************/
gabdo 9:9e0d0ba5b6b1 109
gabdo 49:f202fb0d4128 110 void quadCommand::updateMotors()
gabdo 49:f202fb0d4128 111 {
gabdo 53:cce34958f952 112 updateCurrent();
dereklmc 15:92ecb025fbc5 113 float throttle, pitch, roll, yaw;
dereklmc 15:92ecb025fbc5 114
gabdo 55:bca9c9e92da6 115 throttle = targetThrottle; //pidThrottle.correct(currentThrottle, targetThrottle, MOTOR_UPDATE);
dereklmc 15:92ecb025fbc5 116 pitch = pidPitch.correct(currentPitch, targetPitch, MOTOR_UPDATE);
dereklmc 15:92ecb025fbc5 117 roll = pidRoll.correct(currentRoll, targetRoll, MOTOR_UPDATE);
gabdo 55:bca9c9e92da6 118 yaw = 0.0f; //pidYaw.correct(currentYaw, targetYaw, MOTOR_UPDATE);
dereklmc 32:3d180aa938ba 119
gabdo 55:bca9c9e92da6 120 //pc->printf("\r\n----------------------------------------\r\n");
gabdo 55:bca9c9e92da6 121 //pc->printf("cur : { t : %f, p : %f, r : %f, y : %f }\r\n", currentThrottle, currentPitch, currentRoll, 0.0);
gabdo 55:bca9c9e92da6 122 //pc->printf("tar : { t : %f, p : %f, r : %f, y : %f }\r\n", targetThrottle, targetPitch, targetRoll, 0.0);
gabdo 55:bca9c9e92da6 123 //pc->printf("att : { t : %f, p : %f, r : %f, y : %f }\r\n", throttle, pitch, roll, yaw);
gabdo 55:bca9c9e92da6 124 //pc->printf("\r\n----------------------------------------\r\n");
dereklmc 15:92ecb025fbc5 125
gabdo 55:bca9c9e92da6 126 int go = 0;
gabdo 55:bca9c9e92da6 127 if( targetThrottle > 0 )
gabdo 55:bca9c9e92da6 128 go = 1;
gabdo 55:bca9c9e92da6 129
gabdo 55:bca9c9e92da6 130 float speed0 = go * (throttle - roll - pitch + yaw);
gabdo 55:bca9c9e92da6 131 float speed1 = go * (throttle + roll - pitch - yaw);
gabdo 55:bca9c9e92da6 132 float speed2 = go * (throttle + roll + pitch - yaw);
gabdo 55:bca9c9e92da6 133 float speed3 = go * (throttle - roll + pitch + yaw);
dereklmc 33:4f62a7a46e71 134
gabdo 52:24590a45b807 135 //DEBUG(pc.printf("motor : [ %f , %f , %f , %f ]\r\n", speed0, speed1, speed2, speed3);)
dereklmc 33:4f62a7a46e71 136
dereklmc 33:4f62a7a46e71 137 myMotors[ 0 ]->setSpeed(speed0); // Motor 1
dereklmc 33:4f62a7a46e71 138 myMotors[ 1 ]->setSpeed(speed1); // Motor 2
dereklmc 33:4f62a7a46e71 139 myMotors[ 2 ]->setSpeed(speed2); // Motor 3
dereklmc 33:4f62a7a46e71 140 myMotors[ 3 ]->setSpeed(speed3); // Motor 4
gabdo 40:8c01bf294768 141 }
gabdo 40:8c01bf294768 142
gabdo 40:8c01bf294768 143 /**************************** sendData() *********************************/
gabdo 40:8c01bf294768 144 /* */
gabdo 40:8c01bf294768 145 /*************************************************************************/
gabdo 40:8c01bf294768 146
gabdo 40:8c01bf294768 147 void quadCommand::sendTelemetry()
gabdo 40:8c01bf294768 148 {
gabdo 51:60258b84ebab 149 myCom->write( 2, (short)(currentPitch * 90) );
gabdo 51:60258b84ebab 150 myCom->write( 3, (short)(currentRoll * 90) );
gabdo 43:e2fc699e8e8c 151 }
gabdo 43:e2fc699e8e8c 152
gabdo 43:e2fc699e8e8c 153 /************************** updateCurrent() ******************************/
gabdo 43:e2fc699e8e8c 154 /* */
gabdo 43:e2fc699e8e8c 155 /*************************************************************************/
gabdo 43:e2fc699e8e8c 156
gabdo 43:e2fc699e8e8c 157 void quadCommand::updateCurrent()
gabdo 43:e2fc699e8e8c 158 {
gabdo 55:bca9c9e92da6 159 currentPitch = world->getAbsoluteX() - pitchTrim;
gabdo 55:bca9c9e92da6 160 currentRoll = world->getAbsoluteY() - rollTrim;
gabdo 0:8681037b9a18 161 }