added gy80 dcm
Dependencies: mbed DCM_AHRS_GY80 PID MMA8451Q
Fork of quadCommand by
quadCommand/quadCommand.cpp
- Committer:
- gabdo
- Date:
- 2013-06-10
- Revision:
- 51:60258b84ebab
- Parent:
- 50:197a18e49eb4
- Child:
- 52:24590a45b807
File content as of revision 51:60258b84ebab:
/************************ quadCommand.cpp ********************************/ /* */ /*************************************************************************/ #include "quadCommand.h" #include "debug.h" quadCommand::quadCommand() : pidThrottle(1.5,0.1,1.20,DEFAULT_WINDUP_GUARD), pidPitch(1.5,0.1,1.20,DEFAULT_WINDUP_GUARD), pidRoll(1.5,0.1,1.20,DEFAULT_WINDUP_GUARD), pidYaw(1.5,0.1,1.20,DEFAULT_WINDUP_GUARD) { motorProcess.attach(this, &quadCommand::updateMotors, MOTOR_UPDATE/1000); sensorProcess.attach( this, &quadCommand::updateCurrent, SENSSOR_DELAY/1000 ); // sendProcess.attach( this, &quadCommand::sendTelemetry, SEND_DELAY/1000 ); myCom = new com( TXPIN, RXPIN ); // Setup com object. world = new sensors(); // Setup the sensors. myMotors[0] = new motor( MOTOR1 ); // Connect motor 0 to PTD4 pin. myMotors[1] = new motor( MOTOR2 ); // Connect motor 1 to PTA12 pin. myMotors[2] = new motor( MOTOR3 ); // Connect motor 2 to PTA4 pin. myMotors[3] = new motor( MOTOR4 ); // Connect motor 3 to PTA5 pin. targetThrottle = currentThrottle = 0; targetPitch = currentPitch = 0; targetRoll = currentRoll = 0; targetYaw = currentYaw = 0; } /******************************* run() ***********************************/ /* Called from main */ /*************************************************************************/ void quadCommand::run() { while(1) { if( myCom->isData() ) rxInput(); } } /***************************** rxInput() *********************************/ /*Call from run() */ /*************************************************************************/ void quadCommand::rxInput() { short * command = myCom->read(); if( command[0] == 1 ) // Throttle command. targetThrottle = command[1]; if( command[0] == 2 ) // Pitch command. targetPitch = command[1]; if( command[0] == 3 ) // Roll command. targetRoll = command[1]; if( command[0] == 4 ) // Yaw command. targetYaw = command[1]; delete[] command; } /*************************** updateMotors() ******************************/ /*Called in main by motorprocess */ /*************************************************************************/ void quadCommand::updateMotors() { float throttle, pitch, roll, yaw; throttle = pidThrottle.correct(currentThrottle, targetThrottle, MOTOR_UPDATE); pitch = pidPitch.correct(currentPitch, targetPitch, MOTOR_UPDATE); roll = pidRoll.correct(currentRoll, targetRoll, MOTOR_UPDATE); yaw = 0.0; //pidYaw.correct(currentYaw, targetYaw, MOTOR_UPDATE); DEBUG(pc.printf("\r\n----------------------------------------\r\n");) DEBUG(pc.printf("cur : { t : %f, p : %f, r : %f, y : %f }\r\n", currentThrottle, currentPitch, currentRoll, 0.0);) DEBUG(pc.printf("tar : { t : %f, p : %f, r : %f, y : %f }\r\n", targetThrottle, targetPitch, targetRoll, 0.0);) DEBUG(pc.printf("att : { t : %f, p : %f, r : %f, y : %f }\r\n", throttle, pitch, roll, yaw);) DEBUG(pc.printf("\r\n----------------------------------------\r\n");) float speed0 = throttle + pitch - roll - yaw; float speed1 = throttle + pitch + roll + yaw; float speed2 = throttle - pitch + roll - yaw; float speed3 = throttle - pitch - roll + yaw; DEBUG(pc.printf("motor : [ %f , %f , %f , %f ]\r\n", speed0, speed1, speed2, speed3);) myMotors[ 0 ]->setSpeed(speed0); // Motor 1 myMotors[ 1 ]->setSpeed(speed1); // Motor 2 myMotors[ 2 ]->setSpeed(speed2); // Motor 3 myMotors[ 3 ]->setSpeed(speed3); // Motor 4 } /**************************** sendData() *********************************/ /* */ /*************************************************************************/ void quadCommand::sendTelemetry() { myCom->write( 2, (short)(currentPitch * 90) ); myCom->write( 3, (short)(currentRoll * 90) ); } /************************** updateCurrent() ******************************/ /* */ /*************************************************************************/ void quadCommand::updateCurrent() { currentPitch = world->getAbsoluteX(); currentRoll = world->getAbsoluteY(); }